Refactored Tango and ARCore texture management. Tango: added support with First-person AR mode. ARCoreJava: Added localization filtering speed option (avoid ARCore jumps), also manage when ARCore is lost (new session is created, relocalization needed to continue mapping).

app/android/jni/CameraARCore.cpp

@@ -43,9 +43,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
 	context_(context),
 	activity_(activity),
 	arInstallRequested_(false),
-	textureId_(9999),
-	uvs_initialized_(false),
-	updateOcclusionImage_(false),
 	depthFromMotion_(depthFromMotion)
 {
 }
@@ -53,12 +50,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
-
-	if(textureId_ != 9999)
-	{
-		glDeleteTextures(1, &textureId_);
-		textureId_ = 9999;
-	}
 }
 
 
@@ -176,7 +167,7 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
 	}
 
-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -279,7 +270,6 @@ void CameraARCore::close()
 	arPose_ = nullptr;
 
 	CameraMobile::close();
-	occlusionImage_ = cv::Mat();
 }
 
 LaserScan CameraARCore::scanFromPointCloudData(
@@ -355,7 +345,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}
 
-	if(textureId_ == 9999)
+	if(textureId_ == 0)
 	{
 		glGenTextures(1, &textureId_);
 		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
@@ -364,6 +354,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 	}
+	if(textureId_!=0)
 		ArSession_setCameraTextureName(arSession_, textureId_);
 
 	// Update session to get current frame and render camera background.
@@ -454,7 +445,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
-				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				if(is_depth_supported)
 				{
 					LOGD("Acquire depth image!");
 					ArImage * depthImage = nullptr;
@@ -481,11 +472,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
 
-						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+						cv::Mat occlusionImage = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
 
 						float scaleX = (float)depth_width / (float)width;
 						float scaleY = (float)depth_height / (float)height;
-						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+						CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+						this->setOcclusionImage(occlusionImage, occlusionModel);
 					}
 					ArImage_release(depthImage);
 				}
@@ -557,7 +549,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -592,16 +584,15 @@ void CameraARCore::capturePoseOnly()
 		return;
 	}
 
-	if(textureId_ == 9999)
+	if(textureId_ != 0)
 	{
-		glGenTextures(1, &textureId_);
 		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
 		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-	}
 		ArSession_setCameraTextureName(arSession_, textureId_);
+	}
 
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -662,7 +653,7 @@ void CameraARCore::capturePoseOnly()
 		int32_t is_depth_supported = 0;
 		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
 
-		if(is_depth_supported && updateOcclusionImage_)
+		if(is_depth_supported)
 		{
 			LOGD("Acquire depth image!");
 			ArImage * depthImage = nullptr;
@@ -689,7 +680,7 @@ void CameraARCore::capturePoseOnly()
 
 				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
 
-				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+				cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
 
 				float fx,fy, cx, cy;
 				int32_t rgb_width, rgb_height;
@@ -700,7 +691,8 @@ void CameraARCore::capturePoseOnly()
 
 				float scaleX = (float)width / (float)rgb_width;
 				float scaleY = (float)height / (float)rgb_height;
-				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+				CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+				this->setOcclusionImage(occlusionImage, occlusionModel);
 			}
 			ArImage_release(depthImage);
 		}

app/android/jni/CameraARCore.h

@@ -67,9 +67,6 @@ public:
 	const float* uvsTransformed() const {return transformed_uvs_;}
 	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
 
-	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
-	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
-
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -97,17 +94,8 @@ private:
 	ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
 	ArPose * arPose_ = nullptr;
 	bool arInstallRequested_;
-	GLuint textureId_;
 	UMutex arSessionMutex_;
 
-	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
-	bool uvs_initialized_ = false;
-	glm::mat4 viewMatrix_;
-	glm::mat4 projectionMatrix_;
-
-	bool updateOcclusionImage_;
-	cv::Mat occlusionImage_;
-	CameraModel occlusionModel_;
 	bool depthFromMotion_;
 };
 

app/android/jni/CameraMobile.cpp

@@ -56,7 +56,7 @@ CameraMobile::CameraMobile(bool smoothing) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		spinOncePreviousStamp_(0.0),
-		textureId_(9999),
+		textureId_(0),
 		uvs_initialized_(false),
 		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
@@ -64,18 +64,11 @@ CameraMobile::CameraMobile(bool smoothing) :
 		colorCameraToDisplayRotation_(ROTATION_0),
 		originUpdate_(false)
 {
-    glGenTextures(1, &textureId_);
 }
 
 CameraMobile::~CameraMobile() {
 	// Disconnect camera service
 	close();
-    
-    if(textureId_ != 9999)
-    {
-        glDeleteTextures(1, &textureId_);
-        textureId_ = 9999;
-    }
 }
 
 bool CameraMobile::init(const std::string &, const std::string &)
@@ -94,10 +87,22 @@ void CameraMobile::close()
 	originUpdate_ = false;
 	pose_ = Transform();
 	data_ = SensorData();
+
+    if(textureId_ != 0)
+    {
+        glDeleteTextures(1, &textureId_);
+        textureId_ = 0;
+    }
 }
 
 void CameraMobile::resetOrigin()
 {
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	pose_ = Transform();
+	data_ = SensorData();
 	originUpdate_ = true;
 }
 
@@ -150,7 +155,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
         viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
     }
     
-    if(textureId_ == 9999)
+    if(textureId_ == 0)
     {
     	glGenTextures(1, &textureId_);
     }
@@ -381,7 +386,7 @@ void CameraMobile::mainLoop()
 		previousPose_ = pose;
 		previousStamp_ = data.stamp();
 	}
-	else if(!this->isKilled())
+	else if(!this->isKilled() && info.odomPose.isNull())
 	{
 		LOGW("Odometry lost");
 		this->post(new OdometryEvent());

app/android/jni/CameraMobile.h

@@ -116,6 +116,7 @@ public:
     bool uvsInitialized() const {return uvs_initialized_;}
     const float* uvsTransformed() const {return transformed_uvs_;}
     void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+    ScreenRotation getScreenRotation() const {return colorCameraToDisplayRotation_;}
     
     void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
     const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }

app/android/jni/CameraTango.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include <tango_client_api.h>
 #include <tango_support_api.h>
+#include "tango-gl/camera.h"
 
 namespace rtabmap {
 
@@ -42,11 +43,22 @@ const int holeSize = 5;
 const float maxDepthError = 0.10;
 const int scanDownsampling = 1;
 
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
+const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
+const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
+const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};
+
 // Callbacks
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(app->isRunning() && point_cloud->num_points>0)
+	if(point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,8 +67,7 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(app->isRunning())
+
-	{
 	cv::Mat tangoImage;
 	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 	{
@@ -84,7 +95,6 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
 	}
 }
-}
 
 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 {
@@ -110,7 +120,6 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0)
 {
@@ -430,7 +439,7 @@ void CameraTango::close()
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
-	if(this->isRunning() && !cloud.empty())
+	if(!cloud.empty())
 	{
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
 
@@ -447,144 +456,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//  So, synchronize with the last RGB frame before the Depth is acquired
 		if(!tangoColor_.empty())
 		{
+			UTimer timer;
 			double dt = fabs(timestamp - tangoColorStamp_);
 
 			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
 
 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = cloud_.empty();
+				bool notify = !data_.isValid();
-				cloud_ = cloud.clone();
-				cloudStamp_ = timestamp;
-				if(notify)
-				{
-					//LOGD("Cloud: Release semaphore");
-					dataReady_.release();
-				}
-			}
-		}
-	}
-}
 
-void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
+				cv::Mat tangoImage = tangoColor_;
-{
-	if(this->isRunning() && !tangoImage.empty())
-	{
-		//LOGD("RGB received! %fs", timestamp);
-
-		boost::mutex::scoped_lock  lock(dataMutex_);
-
-		tangoColor_ = tangoImage.clone();
-		tangoColorStamp_ = timestamp;
-		tangoColorType_ = type;
-	}
-}
-
-void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
-{
-	this->post(new CameraInfoEvent(type, key, value));
-}
-
-std::string CameraTango::getSerial() const
-{
-	return "Tango";
-}
-
-rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
-{
-	UASSERT(tangoPose);
-	rtabmap::Transform pose;
-
-	pose = rtabmap::Transform(
-			tangoPose->translation[0],
-			tangoPose->translation[1],
-			tangoPose->translation[2],
-			tangoPose->orientation[0],
-			tangoPose->orientation[1],
-			tangoPose->orientation[2],
-			tangoPose->orientation[3]);
-
-	return pose;
-}
-
-rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
-{
-	rtabmap::Transform pose;
-
-	TangoPoseData pose_start_service_T_device;
-	TangoCoordinateFramePair frame_pair;
-	frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
-	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
-	TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
-	if (status != TANGO_SUCCESS)
-	{
-		LOGE(
-			"PoseData: Failed to get transform between the Start of service and "
-			"device frames at timestamp %lf",
-			timestamp);
-	}
-	if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
-	{
-		LOGW(
-			"PoseData: Failed to get transform between the Start of service and "
-			"device frames at timestamp %lf",
-			timestamp);
-	}
-	else
-	{
-
-		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
-	}
-
-	return pose;
-}
-
-SensorData CameraTango::captureImage(CameraInfo * info)
-{
-	//LOGI("Capturing image...");
-
-	SensorData data;
-	if(!dataReady_.acquire(1, 2000))
-	{
-		if(this->isRunning())
-		{
-			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
-			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
-
-			boost::mutex::scoped_lock  lock(dataMutex_);
-			if(!cloud_.empty() && !tangoColor_.empty())
-			{
-				UERROR("cloud and image were set!?");
-			}
-		}
-		cloud_ = cv::Mat();
-		cloudStamp_ = 0.0;
-		tangoColor_ = cv::Mat();
-		tangoColorStamp_ = 0.0;
-		tangoColorType_ = 0;
-	}
-	else
-	{
-		cv::Mat cloud;
-		cv::Mat tangoImage;
 				cv::Mat rgb;
-		double cloudStamp = 0.0;
+				double cloudStamp = timestamp;
-		double rgbStamp = 0.0;
+				double rgbStamp = tangoColorStamp_;
-		int tangoColorType = 0;
+				int tangoColorType = tangoColorType_;
 
-		{
-			boost::mutex::scoped_lock  lock(dataMutex_);
-			cloud = cloud_;
-			cloudStamp = cloudStamp_;
-			cloud_ = cv::Mat();
-			cloudStamp_ = 0.0;
-			tangoImage = tangoColor_;
-			rgbStamp = tangoColorStamp_;
-			tangoColorType = tangoColorType_;
 				tangoColor_ = cv::Mat();
 				tangoColorStamp_ = 0.0;
 				tangoColorType_ = 0;
-		}
 
 				LOGD("tangoColorType=%d", tangoColorType);
 				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
@@ -606,7 +495,8 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				else
 				{
 					LOGE("Not supported color format : %d.", tangoColorType);
-			return data;
+					data_ = SensorData();
+					return;
 				}
 
 				//for(int i=0; i<rgb.cols; ++i)
@@ -685,7 +575,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 					unsigned short minDepthValue=10000;
 					for(unsigned int i=0; i<cloud.total(); ++i)
 					{
-				float * p = cloud.ptr<float>(0,i);
+						const float * p = cloud.ptr<float>(0,i);
 						cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
 
 						if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
@@ -773,6 +663,14 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 						odom = getOriginOffset() * odom;
 					}
 
+					// occlusion depth
+					if(!depth.empty())
+					{
+						rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
+						depthModel.setLocalTransform(odom*model.localTransform());
+						this->setOcclusionImage(depth, depthModel);
+					}
+
 					//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
 					//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
 
@@ -780,17 +678,213 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 					if(rawScanPublished_)
 					{
-				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
 					else
 					{
-				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
-			info->odomPose = odom;
+					data_.setGroundTruth(odom);
 				}
 				else
 				{
 					LOGE("Could not get depth and rgb images!?!");
+					data_ = SensorData();
+					return;
+				}
+
+				if(notify)
+				{
+					dataReady_.release();
+				}
+				LOGD("process cloud received %fs", timer.ticks());
+			}
+		}
+	}
+}
+
+void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
+{
+	if(!tangoImage.empty())
+	{
+		//LOGD("RGB received! %fs", timestamp);
+
+		boost::mutex::scoped_lock  lock(dataMutex_);
+
+		tangoColor_ = tangoImage.clone();
+		tangoColorStamp_ = timestamp;
+		tangoColorType_ = type;
+	}
+}
+
+void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
+{
+	this->post(new CameraInfoEvent(type, key, value));
+}
+
+std::string CameraTango::getSerial() const
+{
+	return "Tango";
+}
+
+rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
+{
+	UASSERT(tangoPose);
+	rtabmap::Transform pose;
+
+	pose = rtabmap::Transform(
+			tangoPose->translation[0],
+			tangoPose->translation[1],
+			tangoPose->translation[2],
+			tangoPose->orientation[0],
+			tangoPose->orientation[1],
+			tangoPose->orientation[2],
+			tangoPose->orientation[3]);
+
+	return pose;
+}
+
+rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
+{
+	rtabmap::Transform pose;
+
+	TangoPoseData pose_start_service_T_device;
+	TangoCoordinateFramePair frame_pair;
+	frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
+	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
+	if (status != TANGO_SUCCESS)
+	{
+		LOGE(
+			"PoseData: Failed to get transform between the Start of service and "
+			"device frames at timestamp %lf",
+			timestamp);
+	}
+	if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
+	{
+		LOGW(
+			"PoseData: Failed to get transform between the Start of service and "
+			"device frames at timestamp %lf",
+			timestamp);
+	}
+	else
+	{
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
+	}
+
+	return pose;
+}
+
+SensorData CameraTango::captureImage(CameraInfo * info)
+{
+	//LOGI("Capturing image...");
+
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
+	{
+		double video_overlay_timestamp;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
+
+		if (status == TANGO_SUCCESS)
+		{
+			if(info)
+			{
+				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
+			}
+
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
+			  if (rotation > 3) {
+				  rotation -= 4;
+			  }
+
+			TangoDoubleMatrixTransformData matrix_transform;
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
+					video_overlay_timestamp,
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					static_cast<TangoSupportRotation>(rotation),
+					&matrix_transform);
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
+			{
+				// Get projection matrix
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);
+
+				if (ret == TANGO_SUCCESS) {
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
+					float image_height = static_cast<float>(color_camera_intrinsics.height);
+					float fx = static_cast<float>(color_camera_intrinsics.fx);
+					float fy = static_cast<float>(color_camera_intrinsics.fy);
+					float cx = static_cast<float>(color_camera_intrinsics.cx);
+					float cy = static_cast<float>(color_camera_intrinsics.cy);
+
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
+					if(!getOriginOffset().isNull())
+					{
+						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+					}
+
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
+
+					switch(rotation)
+					{
+					case ROTATION_90:
+						memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
+						break;
+					case ROTATION_180:
+						memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
+						break;
+					case ROTATION_270:
+						memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
+						break;
+					case ROTATION_0:
+					default:
+						memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
+					}
+					uvs_initialized_ = true;
+				}
+				else
+				{
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
+				}
+			}
+			else
+			{
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
+			}
+		}
+		else
+		{
+			UERROR("TangoService_updateTextureExternalOes failed!");
+		}
+	}
+
+	SensorData data;
+	if(dataReady_.acquireTry(1))
+	{
+		boost::mutex::scoped_lock  lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
+		if(info)
+		{
+			info->odomPose = data.groundTruth();
+			data.setGroundTruth(Transform());
 		}
 	}
 	return data;

app/android/jni/CameraTango.h

@@ -71,8 +71,7 @@ private:
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	cv::Mat cloud_;
+	SensorData data_;
-	double cloudStamp_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;

app/android/jni/RTABMapApp.cpp

@@ -73,7 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #define LOW_RES_PIX 2
-#define DEBUG_RENDERING_PERFORMANCE
+//#define DEBUG_RENDERING_PERFORMANCE
 
 const int g_optMeshId = -100;
 
@@ -894,10 +894,6 @@ bool RTABMapApp::startCamera()
 		LOGI("Cloud density level %d", cloudDensityLevel_);
 
 		LOGI("Start camera thread");
-		if(cameraDriver_ == 0)
-		{
-			camera_->start();
-		}
 		cameraJustInitialized_ = true;
 		return true;
 	}
@@ -1222,75 +1218,27 @@ int RTABMapApp::Render()
 		glm::mat4 arProjectionMatrix(0);
 		glm::mat4 arViewMatrix(0);
 		rtabmap::Mesh occlusionMesh;
-		if((cameraDriver_ == 1 || cameraDriver_ == 2 || cameraDriver_ == 3) && camera_!=0)
+
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
-#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ <= 2)
-				if(cameraDriver_ == 1)
-				{
-					((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
-				}
-#endif
-                if(cameraDriver_ == 1 || cameraDriver_ == 2)
 				{
 					camera_->spinOnce();
 				}
-
+#ifdef DEBUG_RENDERING_PERFORMANCE
-#ifdef RTABMAP_ARCORE
+				LOGW("Camera spinOnce %fs", time.ticks());
-				if(cameraDriver_ == 1)
-				{
-					if(main_scene_.background_renderer_ == 0 &&
-							((rtabmap::CameraARCore*)camera_)->getTextureId() != 9999 &&
-							((rtabmap::CameraARCore*)camera_)->getTextureId() != 0)
-					{
-						main_scene_.background_renderer_ = new BackgroundRenderer();
-						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId(), true);
-					}
-					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
-					{
-						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
-						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
-						if(graphOptimization_ && !mapToOdom_.isIdentity())
-						{
-							rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
-							arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
-						}
-					}
-					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
-					{
-						rtabmap::CameraModel occlusionModel;
-						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
-
-						if(occlusionModel.isValidForProjection())
-						{
-							pcl::IndicesPtr indices(new std::vector<int>);
-                            int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
-							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
-							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
-							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
-							occlusionMesh.indices = indices;
-							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
-						}
-						else
-						{
-							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
-						}
-					}
-				}
 #endif
-                if(cameraDriver_ == 3)
+
+				if(cameraDriver_ != 2)
 				{
-                    if(main_scene_.background_renderer_ == 0 &&
+					if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
-                    		((rtabmap::CameraMobile*)camera_)->getTextureId() != 9999 &&
-                    		((rtabmap::CameraMobile*)camera_)->getTextureId() != 0)
 					{
 						main_scene_.background_renderer_ = new BackgroundRenderer();
-                        main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), false);
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
 					}
-                    if(((rtabmap::CameraMobile*)camera_)->uvsInitialized())
+					if(camera_->uvsInitialized())
 					{
 						uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
 						((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
@@ -1322,6 +1270,9 @@ int RTABMapApp::Render()
 						}
 					}
 				}
+#ifdef DEBUG_RENDERING_PERFORMANCE
+				LOGW("Update background and occlusion mesh %fs", time.ticks());
+#endif
 			}
 		}
 
@@ -3691,6 +3642,13 @@ void RTABMapApp::postCameraPoseEvent(
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(cameraDriver_ == 3 && camera_)
 	{
+		if(qx==0 && qy==0 && qz==0 && qw==0)
+		{
+			// Lost! clear buffer
+			poseBuffer_.clear();
+			camera_->resetOrigin(); // we are lost, create new session on next valid frame
+			return;
+		}
 		rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 		camera_->poseReceived(pose);

app/android/res/layout/activity_settings.xml

@@ -102,6 +102,13 @@
                 android:title="@string/pref_title_depth_from_motion"
                 android:summary="@string/pref_summary_depth_from_motion"
                 android:defaultValue="@string/pref_default_depth_from_motion"/>
+             <ListPreference
+		        android:key="@string/pref_key_arcore_localization_filtering_speed"
+		        android:title="@string/pref_title_arcore_localization_filtering_speed"
+		        android:summary="@string/pref_summary_arcore_localization_filtering_speed"
+		        android:entries="@array/pref_arcore_localization_filtering_speed_keys"
+		   	    android:entryValues="@array/pref_arcore_localization_filtering_speed_values"
+		   		android:defaultValue="@string/pref_default_arcore_localization_filtering_speed"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"

app/android/res/menu/optionmenu.xml

@@ -12,23 +12,6 @@
        <item android:id="@+id/open" android:title="Library" android:showAsAction="ifRoom"/>
 	   <item android:id="@+id/save" android:title="Save"/>
        
-       <item android:id="@+id/export" android:title="Export">
-          <menu>
-              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
-                  <menu>
-                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
-                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
-                  </menu>
-              </item>
-	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
-			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
-		          </menu>
-	          </item>
-          </menu>
-       </item>
-       
         <item android:id="@+id/post_processing" android:title="Optimize">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
@@ -46,6 +29,23 @@
           </menu>
        </item>
        
+       <item android:id="@+id/export" android:title="Assemble">
+          <menu>
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
+                  </menu>
+              </item>
+	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
+       
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">

app/android/res/values/strings.xml

@@ -75,7 +75,7 @@
     <string name="pref_key_resolution">pref_key_resolution</string>
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
-    <string name="pref_default_smoothing">true</string>
+    <string name="pref_default_smoothing">false</string>
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
@@ -83,12 +83,14 @@
     <string name="pref_default_camera_driver">0</string>
     <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
     <string name="pref_default_depth_from_motion">false</string>
+    <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
+    <string name="pref_default_arcore_localization_filtering_speed">1</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>
     <string name="pref_default_max_speed">0</string>
     <string name="pref_key_time_thr">pref_key_time_thr</string>
-    <string name="pref_default_time_thr">1000</string>
+    <string name="pref_default_time_thr">0</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
     <string name="pref_default_mem_thr">0</string>
     <string name="pref_key_loop_thr">pref_key_loop_thr</string>
@@ -98,7 +100,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">3</string>
+    <string name="pref_default_opt_error">1</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -162,7 +164,7 @@
     <string name="pref_title_triangle">Mesh Triangle Size</string>
     <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
     <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
-    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Assembling results.</string>
     <string name="pref_title_min_depth">Min Depth</string>
     <string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</string>
     <string name="pref_title_depth">Max Depth</string>
@@ -327,6 +329,8 @@
     <string name="pref_summary_camera_driver">AR sdk used for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
     <string name="pref_title_depth_from_motion">Depth From Motion</string>
     <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
+    <string name="pref_title_arcore_localization_filtering_speed">ARCore Localization Filtering Speed</string>
+    <string name="pref_summary_arcore_localization_filtering_speed">Filter ARCore\'s localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set a speed threshold to detect those events.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>
@@ -406,6 +410,24 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_keys">
+        <item>"Disabled"</item>
+        <item>"5 m/s"</item>
+        <item>"4 m/s"</item>
+        <item>"3 m/s"</item>
+        <item>"2 m/s"</item>
+        <item>"1 m/s"</item>
+        <item>"0.5 m/s"</item>
+    </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+    </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>
         <item>"High"</item>
@@ -708,9 +730,9 @@
         <item>"1"</item>
     </string-array>
 
-    <string name="pref_title_export_sub">Exporting&#8230;</string>
+    <string name="pref_title_export_sub">Assembling&#8230;</string>
-    <string name="pref_title_export">Exporting</string>
+    <string name="pref_title_export">Assembling</string>
-    <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
+    <string name="pref_summary_export">Advanced parameters used when assembling the map.</string>
     <string name="pref_title_cloud_voxel">Voxel Size</string>
     <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
     <string name="pref_title_texture_size">Texture Size</string>
@@ -723,8 +745,8 @@
     <string name="pref_summary_max_texture_distance">Maximum distance from a camera for polygons to be textured by this camera.</string>
     <string name="pref_title_min_texture_cluster_size">Min Texture Cluster Size</string>
     <string name="pref_summary_min_texture_cluster_size">Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</string>
-    <string name="pref_title_block_render">Block Rendering Thread While Exporting</string>
+    <string name="pref_title_block_render">Block Rendering Thread While Assembling</string>
-    <string name="pref_summary_block_render">This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors.</string>
+    <string name="pref_summary_block_render">This decreases assembling time, but freezes rendering while assembling. This also clears temporary the rendered clouds/meshes from memory during assembling, this can be useful to avoid out of memory errors.</string>
 
     <string-array name="pref_cloud_voxel_keys">
         <item>"0.2 m"</item>
@@ -830,7 +852,7 @@
     <string name="pref_title_opt_depth">Reconstruction Depth</string>
     <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
     <string name="pref_title_opt_color_radius">Color Radius</string>
-    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
+    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When assembling with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
     <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
 	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>

app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java

@@ -8,7 +8,10 @@ import java.util.EnumSet;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+import com.google.ar.core.Anchor;
 import com.google.ar.core.Camera;
+import com.google.ar.core.CameraConfig;
+import com.google.ar.core.CameraConfigFilter;
 import com.google.ar.core.CameraIntrinsics;
 import com.google.ar.core.Config;
 import com.google.ar.core.Coordinates2d;
@@ -41,6 +44,7 @@ import android.os.HandlerThread;
 import android.support.annotation.NonNull;
 import android.util.Log;
 import android.view.Surface;
+import android.widget.Toast;
 
 public class ARCoreSharedCamera {
 
@@ -52,8 +56,9 @@ public class ARCoreSharedCamera {
       };
 	
 	private static RTABMapActivity mActivity;
-	public ARCoreSharedCamera(RTABMapActivity c) {
+	public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
 		mActivity = c;
+		mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
 	}
 
 	// Depth TOF Image.
@@ -67,6 +72,12 @@ public class ARCoreSharedCamera {
 	// ARCore session that supports camera sharing.
 	private Session sharedSession;
 	
+	private Pose previousAnchorPose = null;
+	private long previousAnchorTimeStamp;
+	private Pose arCoreCorrection = Pose.IDENTITY;
+	private Pose odomPose = Pose.IDENTITY;
+	private float mARCoreLocalizationFilteringSpeed = 1.0f;
+
 	// Camera capture session. Used by both non-AR and AR modes.
 	private CameraCaptureSession captureSession;
 
@@ -84,6 +95,8 @@ public class ARCoreSharedCamera {
 	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
 	private SharedCamera sharedCamera;
 	
+	private Toast mToast = null;
+
 	// Camera ID for the camera used by ARCore.
 	private String cameraId;
 	private String depthCameraId;
@@ -105,6 +118,10 @@ public class ARCoreSharedCamera {
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
 	
 	// Camera device state callback.
 	private final CameraDevice.StateCallback cameraDeviceCallback =
@@ -356,6 +373,32 @@ public class ARCoreSharedCamera {
 				return false;
 			}
 			
+			// First obtain the session handle before getting the list of various camera configs.
+			// Create filter here with desired fps filters.
+			CameraConfigFilter cameraConfigFilter = new CameraConfigFilter(sharedSession);
+			CameraConfig[] cameraConfigs = sharedSession.getSupportedCameraConfigs(cameraConfigFilter).toArray(new CameraConfig[0]);
+			Log.i(TAG, "Size of supported CameraConfigs list is " + cameraConfigs.length);
+
+			// Determine the highest and lowest CPU resolutions.
+			int highestResolutionIndex=-1;
+			int highestResolution = 0;
+			for(int i=0; i<cameraConfigs.length; ++i)
+			{
+				Log.i(TAG, "Camera ID: " + cameraConfigs[i].getCameraId());
+				Log.i(TAG, "Resolution: " + cameraConfigs[i].getImageSize().getWidth() + "x" + cameraConfigs[i].getImageSize().getHeight());
+				if(highestResolution == 0 || highestResolution < cameraConfigs[i].getImageSize().getWidth())
+				{
+					highestResolutionIndex = i;
+					highestResolution = cameraConfigs[i].getImageSize().getWidth();
+				}
+			}
+			if(highestResolutionIndex>=0)
+			{
+				//Log.i(TAG, "Setting camera resolution to " + cameraConfigs[highestResolutionIndex].getImageSize().getWidth() + "x" + cameraConfigs[highestResolutionIndex].getImageSize().getHeight());
+				//FIXME: Can we make it work to use HD rgb images? To avoid this error "CaptureRequest contains unconfigured Input/Output Surface!"
+				//sharedSession.setCameraConfig(cameraConfigs[highestResolutionIndex]);
+			}
+
 			// Enable auto focus mode while ARCore is running.
 			Config config = sharedSession.getConfig();
 			config.setFocusMode(Config.FocusMode.FIXED);
@@ -573,7 +616,8 @@ public class ARCoreSharedCamera {
 		Log.w(TAG, "close()");
 		
 		if (sharedSession != null)  {
-			sharedSession.pause();
+			sharedSession.close();
+			sharedSession = null;
 		}
 
 		if (captureSession != null) {
@@ -582,6 +626,7 @@ public class ARCoreSharedCamera {
 		}
 		if (cameraDevice != null) {
 			cameraDevice.close();
+			cameraDevice = null;
 		}
 		
 		mTOFImageReader.close();
@@ -596,6 +641,7 @@ public class ARCoreSharedCamera {
 
 	public void setDisplayGeometry(int rotation, int width, int height)
 	{
+		if(sharedSession!=null)
 			sharedSession.setDisplayGeometry(rotation, width, height);
 	}
 	
@@ -603,7 +649,7 @@ public class ARCoreSharedCamera {
 	// Draw frame when in AR mode. Called on the GL thread.
 	public void updateGL() throws CameraNotAvailableException {
 
-		if(!mReady.get())
+		if(!mReady.get() || sharedSession == null)
 		{
 			return;
 		}
@@ -624,18 +670,93 @@ public class ARCoreSharedCamera {
 			camera = frame.getCamera();
 		}else
 		{
+			Log.e(TAG, String.format("frame.getCamera() null!"));
 			return;
 		}
 
-		if (camera == null) return;
+		if (camera == null) { 
-		// If not tracking, don't draw 3D objects.
+			Log.e(TAG, String.format("camera is null!"));
-		if (camera.getTrackingState() == TrackingState.PAUSED) return;
+			return;
+		}
+		if (camera.getTrackingState() != TrackingState.TRACKING) {
+			final String trackingState = camera.getTrackingState().toString();
+			Log.e(TAG, String.format("Tracking lost! state=%s", trackingState));
+			// This will force a new session on the next frame received
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0);
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(mToast!=null && previousAnchorPose != null)
+					{
+						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
+						if(!mToast.getView().isShown())
+						{
+							mToast.makeText(mActivity.getApplicationContext(), 
+									msg, Toast.LENGTH_LONG).show();
+						}
+						else
+						{
+							mToast.setText(msg);
+						}
+						previousAnchorPose = null;
+					}
+				}
+			});
+			return;
+		}
 
 		if (frame.getTimestamp() != 0) {
 			Pose pose = camera.getPose();
+			
+			// Remove ARCore SLAM corrections by integrating pose from previous frame anchor
+			if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0)
+			{
+				odomPose = pose;
+			}
+			else
+			{
+				float[] t = previousAnchorPose.inverse().compose(pose).getTranslation();
+				final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
+				if(speed>=mARCoreLocalizationFilteringSpeed)
+				{
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
+					t = arCoreCorrection.getTranslation();
+					Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
+					
+					t = odomPose.getTranslation();
+					float[] t2 = (arCoreCorrection.compose(pose)).getTranslation();
+					float[] t3 = previousAnchorPose.getTranslation();
+					float[] t4 = pose.getTranslation();
+					Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2]));
+				
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								String msg = String.format("ARCore localization has been suppressed "
+										+ "because of high speed detected (%f m/s) causing a jump! You can change "
+										+ "ARCore localization filtering speed in Settings->Mapping if you are "
+										+ "indeed moving as fast.", speed);
+								if(!mToast.getView().isShown())
+								{
+									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
+								}
+								else
+								{
+									mToast.setText(msg);
+								}
+								previousAnchorPose = null;
+							}
+						}
+					});
+				}
+				odomPose = arCoreCorrection.compose(pose);
+			}
+			previousAnchorPose = pose;
+			previousAnchorTimeStamp = frame.getTimestamp();
+			
 			double stamp = (double)frame.getTimestamp()/10e8;
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp));
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp);
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
 			
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
@@ -679,7 +800,7 @@ public class ARCoreSharedCamera {
 		        camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
    
 		        float[] viewMatrix = new float[16];
-		        camera.getViewMatrix(viewMatrix, 0);
+		        arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
 		        float[] quat = new float[4];
 		        rotationMatrixToQuaternion(viewMatrix, quat);
 
@@ -698,7 +819,7 @@ public class ARCoreSharedCamera {
 
 					RTABMapLib.postOdometryEventDepth(
 							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
 							fl[0], fl[1], pp[0], pp[1], 
 							depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
 							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
@@ -716,7 +837,7 @@ public class ARCoreSharedCamera {
 				{
 					RTABMapLib.postOdometryEvent(
 							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
 							fl[0], fl[1], pp[0], pp[1], 
 							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
 							stamp, 

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -1236,7 +1236,10 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							if(cameraStartSucess && mCameraDriver == 3)
 							{
 								synchronized (this) {
-									mArCoreCamera = new ARCoreSharedCamera(getActivity());
+									SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
+									String arCoreLocalizationFiltering = sharedPref.getString(getString(R.string.pref_key_arcore_localization_filtering_speed), getString(R.string.pref_default_arcore_localization_filtering_speed));
+									mArCoreCamera = new ARCoreSharedCamera(getActivity(), Float.parseFloat(arCoreLocalizationFiltering));
+									mArCoreCamera.setToast(mToast);
 									mProgressDialog.setTitle("");
 									mProgressDialog.setMessage(message);
 									mProgressDialog.show();

app/android/src/com/introlab/rtabmap/Renderer.java

@@ -91,17 +91,23 @@ public class Renderer implements GLSurfaceView.Renderer {
 		synchronized (this) {
 			if(mActivity.nativeApplication != 0)
 			{
+				int step = 0;
 				try
 				{
 					if(mCamera!=null)
 					{
+						step=1;
 						mCamera.updateGL();
+						
 					}
 					
+					step=2;
+					
 					final int value = RTABMapLib.render(mActivity.nativeApplication);
 		
 					if(mTextManager!=null)
 					{
+						step=3;
 						if(mTextChanged)
 						{
 							mTextChanged = false;
@@ -125,7 +131,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 						Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
 						mTextManager.Draw(mvp);
 					}
-		
+					step=4;
 					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
 					{
 						mActivity.runOnUiThread(new Runnable() {
@@ -142,7 +148,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 							public void run() {
 								if(mToast!=null)
 								{
-									mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+									mToast.makeText(mActivity.getApplicationContext(), String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
 								}
 							}
 						});
@@ -153,7 +159,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 							public void run() {
 								if(mToast!=null)
 								{
-									mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+									mToast.makeText(mActivity.getApplicationContext(), String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
 								}
 							}
 						});
@@ -161,11 +167,14 @@ public class Renderer implements GLSurfaceView.Renderer {
 				}
 				catch(final Exception e)
 				{
+					final int stepF = step;
 					mActivity.runOnUiThread(new Runnable() {
 						public void run() {
 							if(mToast!=null)
 							{
-								mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+								String msg = String.format("Rendering error! (exception=%s) step=%d", e.getMessage(), stepF);
+								Log.e("RTABMapActivity", msg);
+								mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
 							}
 						}
 		

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -222,6 +222,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
+        ((Preference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -284,6 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
+        	if(key.compareTo(getString(R.string.pref_key_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));