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,7 +354,8 @@ 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);
 	}
-	ArSession_setCameraTextureName(arSession_, textureId_);
+	if(textureId_!=0)
+		ArSession_setCameraTextureName(arSession_, textureId_);
 
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -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_);
 	}
-	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,9 +155,9 @@ 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_);
+    	glGenTextures(1, &textureId_);
     }
 
     if(texCoord)
@@ -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,34 +67,32 @@ 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)
-		{
-			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-		}
-		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else if(color->format == 35)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", color->format);
-		}
 
-		if(!tangoImage.empty())
+	cv::Mat tangoImage;
-		{
+	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+	{
-		}
+		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == 35)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else
+	{
+		LOGE("Not supported color format : %d.", color->format);
+	}
+
+	if(!tangoImage.empty())
+	{
+		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
 	}
 }
 
@@ -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,20 +456,248 @@ 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;
+				cv::Mat tangoImage = tangoColor_;
+				cv::Mat rgb;
+				double cloudStamp = timestamp;
+				double rgbStamp = tangoColorStamp_;
+				int tangoColorType = tangoColorType_;
+
+				tangoColor_ = cv::Mat();
+				tangoColorStamp_ = 0.0;
+				tangoColorType_ = 0;
+
+				LOGD("tangoColorType=%d", tangoColorType);
+				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
+				}
+				else if(tangoColorType == 35)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+				}
+				else
+				{
+					LOGE("Not supported color format : %d.", tangoColorType);
+					data_ = SensorData();
+					return;
+				}
+
+				//for(int i=0; i<rgb.cols; ++i)
+				//{
+				//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
+				//}
+
+				CameraModel model = model_;
+
+				if(colorCamera_)
+				{
+					if(decimation_ > 1)
+					{
+						rgb = util2d::decimate(rgb, decimation_);
+						model = model.scaled(1.0/double(decimation_));
+					}
+				}
+				else
+				{
+					//UTimer t;
+					cv::Mat rgbRect;
+					cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
+					rgb = rgbRect;
+					//LOGD("Rectification time=%fs", t.ticks());
+				}
+
+				// Querying the depth image's frame transformation based on the depth image's
+				// timestamp.
+				cv::Mat depth;
+
+				// Calculate the relative pose from color camera frame at timestamp
+				// color_timestamp t1 and depth
+				// camera frame at depth_timestamp t0.
+				Transform colorToDepth;
+				TangoPoseData pose_color_image_t1_T_depth_image_t0;
+				if (TangoSupport_calculateRelativePose(
+						rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+				{
+					colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
+				}
+				else
+				{
+					LOGE(
+						"SynchronizationApplication: Could not find a valid relative pose at "
+						"time for color and "
+						" depth cameras.");
+				}
+
+				if(colorToDepth.getNormSquared() > 100000)
+				{
+					LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+					colorToDepth.setNull();
+				}
+				cv::Mat scan;
+				if(!colorToDepth.isNull())
+				{
+					// The Color Camera frame at timestamp t0 with respect to Depth
+					// Camera frame at timestamp t1.
+					//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+					LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
+
+					int pixelsSet = 0;
+					int depthSizeDec = colorCamera_?8:1;
+					depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
+					CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
+					std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
+					int oi=0;
+					int closePoints = 0;
+					float closeROI[4];
+					closeROI[0] = depth.cols/4;
+					closeROI[1] = 3*(depth.cols/4);
+					closeROI[2] = depth.rows/4;
+					closeROI[3] = 3*(depth.rows/4);
+					unsigned short minDepthValue=10000;
+					for(unsigned int i=0; i<cloud.total(); ++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_)
+						{
+							scanData.at(oi++) = pt;
+						}
+
+						int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+						// get the coordinate on image plane.
+						pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+						pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+						pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+						pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
+						unsigned short depth_value(pt.z * 1000.0f);
+
+						if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+						   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+						   depth_value < 600)
+						{
+							++closePoints;
+							if(depth_value < minDepthValue)
+							{
+								minDepthValue = depth_value;
+							}
+						}
+
+						bool pixelSet = false;
+						if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+						   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+						   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixelSet)
+						{
+							pixelsSet += 1;
+						}
+					}
+
+					if(closePoints > 100)
+					{
+						this->post(new CameraInfoEvent(0, "TooClose", ""));
+					}
+
+					if(oi)
+					{
+						scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+					}
+					//LOGD("pixels depth set= %d", pixelsSet);
+				}
+				else
+				{
+					LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
+				}
+
+				if(!rgb.empty() && !depth.empty())
+				{
+					depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
+
+					Transform odom = getPoseAtTimestamp(rgbStamp);
+
+					//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
+					//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
+					//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
+
+					// adjust origin
+					if(!getOriginOffset().isNull())
+					{
+						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());
+
+					Transform scanLocalTransform = model.localTransform();
+
+					if(rawScanPublished_)
+					{
+						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_.setGroundTruth(odom);
+				}
+				else
+				{
+					LOGE("Could not get depth and rgb images!?!");
+					data_ = SensorData();
+					return;
+				}
+
 				if(notify)
 				{
-					//LOGD("Cloud: Release semaphore");
 					dataReady_.release();
 				}
+				LOGD("process cloud received %fs", timer.ticks());
 			}
 		}
 	}
@@ -468,7 +705,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(this->isRunning() && !tangoImage.empty())
+	if(!tangoImage.empty())
 	{
 		//LOGD("RGB received! %fs", timestamp);
 
@@ -532,7 +769,6 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	}
 	else
 	{
-
 		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
 	}
 
@@ -543,254 +779,112 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	//LOGI("Capturing image...");
 
-	SensorData data;
+	if(textureId_ == 0)
-	if(!dataReady_.acquire(1, 2000))
 	{
-		if(this->isRunning())
+		glGenTextures(1, &textureId_);
-		{
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
-			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-			boost::mutex::scoped_lock  lock(dataMutex_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-			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
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
 	{
-		cv::Mat cloud;
+		double video_overlay_timestamp;
-		cv::Mat tangoImage;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
-		cv::Mat rgb;
-		double cloudStamp = 0.0;
-		double rgbStamp = 0.0;
-		int tangoColorType = 0;
 
+		if (status == TANGO_SUCCESS)
 		{
-			boost::mutex::scoped_lock  lock(dataMutex_);
+			if(info)
-			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)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
-		}
-		else if(tangoColorType == 35)
-		{
-			cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", tangoColorType);
-			return data;
-		}
-
-		//for(int i=0; i<rgb.cols; ++i)
-		//{
-		//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
-		//}
-
-		CameraModel model = model_;
-
-		if(colorCamera_)
-		{
-			if(decimation_ > 1)
 			{
-				rgb = util2d::decimate(rgb, decimation_);
+				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-				model = model.scaled(1.0/double(decimation_));
 			}
-		}
-		else
-		{
-			//UTimer t;
-			cv::Mat rgbRect;
-			cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
-			rgb = rgbRect;
-			//LOGD("Rectification time=%fs", t.ticks());
-		}
 
-		// Querying the depth image's frame transformation based on the depth image's
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
-		// timestamp.
+			  if (rotation > 3) {
-		cv::Mat depth;
+				  rotation -= 4;
+			  }
 
-		// Calculate the relative pose from color camera frame at timestamp
+			TangoDoubleMatrixTransformData matrix_transform;
-		// color_timestamp t1 and depth
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
-		// camera frame at depth_timestamp t0.
+					video_overlay_timestamp,
-		Transform colorToDepth;
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
-		TangoPoseData pose_color_image_t1_T_depth_image_t0;
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
-		if (TangoSupport_calculateRelativePose(
+					TANGO_SUPPORT_ENGINE_OPENGL,
-				rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					TANGO_SUPPORT_ENGINE_OPENGL,
-			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					static_cast<TangoSupportRotation>(rotation),
-			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+					&matrix_transform);
-		{
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
-			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
-		}
-		else
-		{
-			LOGE(
-				"SynchronizationApplication: Could not find a valid relative pose at "
-				"time for color and "
-				" depth cameras.");
-		}
-
-		if(colorToDepth.getNormSquared() > 100000)
-		{
-			LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
-			colorToDepth.setNull();
-		}
-		cv::Mat scan;
-		if(!colorToDepth.isNull())
-		{
-			// The Color Camera frame at timestamp t0 with respect to Depth
-			// Camera frame at timestamp t1.
-			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
-			LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
-
-			int pixelsSet = 0;
-			int depthSizeDec = colorCamera_?8:1;
-			depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
-			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
-			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
-			int oi=0;
-			int closePoints = 0;
-			float closeROI[4];
-			closeROI[0] = depth.cols/4;
-			closeROI[1] = 3*(depth.cols/4);
-			closeROI[2] = depth.rows/4;
-			closeROI[3] = 3*(depth.rows/4);
-			unsigned short minDepthValue=10000;
-			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
-				float * p = cloud.ptr<float>(0,i);
+				// Get projection matrix
-				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);
 
-				if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+				if (ret == TANGO_SUCCESS) {
-				{
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
-					scanData.at(oi++) = pt;
+					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);
 
-				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
-				// get the coordinate on image plane.
+					if(!getOriginOffset().isNull())
-				pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
-				pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
-				pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
-				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
-				unsigned short depth_value(pt.z * 1000.0f);
-
-				if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
-				   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
-				   depth_value < 600)
-				{
-					++closePoints;
-					if(depth_value < minDepthValue)
 					{
-						minDepthValue = depth_value;
+						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
 					}
-				}
 
-				bool pixelSet = false;
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
-				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
-				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
+
-				   depth_value)
+					switch(rotation)
-				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
-					if(depthPixel == 0 || depthPixel > depth_value)
 					{
-						depthPixel = depth_value;
+					case ROTATION_90:
-						pixelSet = true;
+						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;
 				}
-				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				else
-				   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
-				   depth_value)
 				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
-					if(depthPixel == 0 || depthPixel > depth_value)
-					{
-						depthPixel = depth_value;
-						pixelSet = true;
-					}
 				}
-				if(pixelSet)
-				{
-					pixelsSet += 1;
-				}
-			}
-
-			if(closePoints > 100)
-			{
-				this->post(new CameraInfoEvent(0, "TooClose", ""));
-			}
-
-			if(oi)
-			{
-				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
-			}
-			//LOGD("pixels depth set= %d", pixelsSet);
-		}
-		else
-		{
-			LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
-		}
-
-		if(!rgb.empty() && !depth.empty())
-		{
-			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
-
-			Transform odom = getPoseAtTimestamp(rgbStamp);
-
-			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
-			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
-			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
-
-			// adjust origin
-			if(!getOriginOffset().isNull())
-			{
-				odom = getOriginOffset() * odom;
-			}
-
-			//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());
-
-			Transform scanLocalTransform = model.localTransform();
-
-			if(rawScanPublished_)
-			{
-				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);
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
 			}
-			info->odomPose = odom;
 		}
 		else
 		{
-			LOGE("Could not get depth and rgb images!?!");
+			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,36 +1218,30 @@ 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);
+					camera_->spinOnce();
 				}
+#ifdef DEBUG_RENDERING_PERFORMANCE
+				LOGW("Camera spinOnce %fs", time.ticks());
 #endif
-                if(cameraDriver_ == 1 || cameraDriver_ == 2)
-                {
-                    camera_->spinOnce();
-                }
 
-#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ != 2)
-				if(cameraDriver_ == 1)
 				{
-					if(main_scene_.background_renderer_ == 0 &&
+					if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 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);
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
 					}
-					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
+					if(camera_->uvsInitialized())
 					{
-						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
+						uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
-						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+						((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
 						if(graphOptimization_ && !mapToOdom_.isIdentity())
 						{
 							rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
@@ -1261,12 +1251,12 @@ int RTABMapApp::Render()
 					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
 					{
 						rtabmap::CameraModel occlusionModel;
-						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
+						cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
 
 						if(occlusionModel.isValidForProjection())
 						{
 							pcl::IndicesPtr indices(new std::vector<int>);
-                            int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
+							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>());
@@ -1274,54 +1264,15 @@ int RTABMapApp::Render()
 							occlusionMesh.indices = indices;
 							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
 						}
-						else
+						else if(!occlusionImage.empty())
 						{
 							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
 						}
 					}
 				}
+#ifdef DEBUG_RENDERING_PERFORMANCE
+				LOGW("Update background and occlusion mesh %fs", time.ticks());
 #endif
-                if(cameraDriver_ == 3)
-                {
-                    if(main_scene_.background_renderer_ == 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);
-                    }
-                    if(((rtabmap::CameraMobile*)camera_)->uvsInitialized())
-                    {
-                        uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
-                        ((rtabmap::CameraMobile*)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::CameraMobile*)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 if(!occlusionImage.empty())
-                        {
-                            UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
-                        }
-                    }
-                }
 			}
 		}
 
@@ -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

@@ -97,11 +97,18 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
-		     <com.introlab.rtabmap.CustomSwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
                 android:key="@string/pref_key_depth_from_motion"
                 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

@@ -11,23 +11,6 @@
        <item android:id="@+id/new_scan" android:title="New Scan"/>
        <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>
@@ -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.
@@ -66,6 +71,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;
@@ -83,6 +94,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;
@@ -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 =
@@ -355,6 +372,32 @@ public class ARCoreSharedCamera {
 				Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
 				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();
@@ -371,11 +414,11 @@ public class ARCoreSharedCamera {
 		sharedCamera = sharedSession.getSharedCamera();
 		// Store the ID of the camera used by ARCore.
 		cameraId = sharedSession.getCameraConfig().getCameraId();
-	
+
 		Log.d(TAG, "Shared camera ID: " + cameraId);
 
 		mTOFAvailable = false;
-		
+
 		// Store a reference to the camera system service.
 		cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
 		
@@ -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,14 +641,15 @@ public class ARCoreSharedCamera {
 
 	public void setDisplayGeometry(int rotation, int width, int height)
 	{
-		sharedSession.setDisplayGeometry(rotation, width, height);
+		if(sharedSession!=null)
+			sharedSession.setDisplayGeometry(rotation, width, height);
 	}
 	
 	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */	
 	// 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));