Fixed occlusion image origin offset (iOS and android). Android: disabled maximum odom image rate for faster feedback in First-Person. Min/Max range parameters also filter scans. Fixed background renderer not showing image (ARCore Java driver).

app/android/jni/CameraARCore.cpp

@@ -393,6 +393,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							   /*near=*/0.1f, /*far=*/100.f,
 							   glm::value_ptr(projectionMatrix_));
 
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -407,6 +413,22 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 
+		Transform poseArCore = pose;
+		if(pose.isNull())
+		{
+			LOGE("CameraARCore: Pose is null");
+		}
+		else
+		{
+			this->poseReceived(pose);
+			// adjust origin
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
+			info->odomPose = pose;
+		}
+
 		// Get calibration parameters
 		float fx,fy, cx, cy;
 		int32_t width, height;
@@ -527,7 +549,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 #endif
 							if(pointCloudData && points>0)
 							{
-								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
+								scan = scanFromPointCloudData(pointCloudData, points, poseArCore, model, rgb, &kpts, &kpts3);
 							}
 						}
 						else
@@ -556,20 +578,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 	ArCamera_release(ar_camera);
 
-	if(pose.isNull())
-	{
-		LOGE("CameraARCore: Pose is null");
-	}
-	else
-	{
-		this->poseReceived(pose);
-        // adjust origin
-        if(!getOriginOffset().isNull())
-        {
-            pose = getOriginOffset() * pose;
-        }
-		info->odomPose = pose;
-	}
 	return data;
 
 }
@@ -622,6 +630,12 @@ void CameraARCore::capturePoseOnly()
 							   /*near=*/0.1f, /*far=*/100.f,
 							   glm::value_ptr(projectionMatrix_));
 
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -638,6 +652,11 @@ void CameraARCore::capturePoseOnly()
 		{
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
+
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
 		}
 
 		int32_t is_depth_supported = 0;

app/android/jni/CameraMobile.cpp

@@ -154,7 +154,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     {
         glGenTextures(1, &textureId_);
     }
-    
+
     if(texCoord)
     {
         memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
@@ -162,7 +162,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     }
     
     LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
-    
+
     if(textureId_ != 0 && texCoord != 0)
     {
         cv::Mat rgbImage;
@@ -200,16 +200,16 @@ void CameraMobile::spinOnce()
 	if(!this->isRunning())
 	{
 		bool ignoreFrame = false;
-		float rate = 10.0f; // maximum 10 FPS for image data
+		//float rate = 10.0f; // maximum 10 FPS for image data
 		double now = UTimer::now();
-		if(rate>0.0f)
+		/*if(rate>0.0f)
 		{
 			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
 				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
 			{
 				ignoreFrame = true;
 			}
-		}
+		}*/
 
 		if(!ignoreFrame)
 		{

app/android/jni/RTABMapApp.cpp

@@ -504,7 +504,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							else
 							{
 								//scan
-								cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255);
+								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
 								indices->resize(cloud->size());
 								for(unsigned int i=0; i<cloud->size(); ++i)
 								{
@@ -1241,7 +1241,9 @@ int RTABMapApp::Render()
 #ifdef RTABMAP_ARCORE
 				if(cameraDriver_ == 1)
 				{
-					if(main_scene_.background_renderer_ == 0)
+					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);
@@ -1250,6 +1252,11 @@ int RTABMapApp::Render()
 					{
 						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)
 					{
@@ -1261,7 +1268,7 @@ int RTABMapApp::Render()
 							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*occlusionModel.localTransform());
+							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;
@@ -1276,7 +1283,9 @@ int RTABMapApp::Render()
 #endif
                 if(cameraDriver_ == 3)
                 {
-                    if(main_scene_.background_renderer_ == 0)
+                    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);
@@ -1301,7 +1310,7 @@ int RTABMapApp::Render()
                             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*occlusionModel.localTransform());
+                            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;
@@ -1794,7 +1803,7 @@ int RTABMapApp::Render()
 										else
 										{
 											//scan
-											cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255);
+											cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
 											indices->resize(cloud->size());
 											for(unsigned int i=0; i<cloud->size(); ++i)
 											{
@@ -2009,7 +2018,7 @@ int RTABMapApp::Render()
 							else
 							{
 								//scan
-								cloud = rtabmap::util3d::laserScanToPointCloudRGB(odomEvent.data().laserScanRaw(), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
+								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
 								indices->resize(cloud->size());
 								for(unsigned int i=0; i<cloud->size(); ++i)
 								{
@@ -3241,7 +3250,7 @@ bool RTABMapApp::exportMesh(
                     else if(!data.laserScanRaw().empty())
                     {
                         //scan
-                        cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255);
+                        cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
                         indices->resize(cloud->size());
                         for(unsigned int i=0; i<cloud->size(); ++i)
                         {
@@ -3271,7 +3280,7 @@ bool RTABMapApp::exportMesh(
                         else if(!data.laserScanRaw().empty())
                         {
                             //scan
-                            cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255);
+                            cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
                             indices->resize(cloud->size());
                             for(unsigned int i=0; i<cloud->size(); ++i)
                             {
@@ -3910,8 +3919,13 @@ void RTABMapApp::postOdometryEvent(
                     
                     if(!outputDepth.empty())
                     {
+                    	rtabmap::Transform poseWithOriginOffset = pose;
+                    	if(!camera_->getOriginOffset().isNull())
+                    	{
+                    		poseWithOriginOffset = camera_->getOriginOffset() * pose;
+                    	}
                         rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
-                        depthModel.setLocalTransform(mapToOdom_*pose*model.localTransform());
+                        depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
                     }
                     

app/android/jni/background_renderer.cc

@@ -51,6 +51,8 @@ const std::string kFragmentShaderBlendingOES =
     "precision mediump float;\n"
     "varying vec2 v_TexCoord;\n"
     "uniform samplerExternalOES sTexture;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform vec2 uScreenScale;\n"
     "uniform bool uRedUnknown;\n"
     "void main() {\n"
     "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
@@ -120,8 +122,19 @@ const std::string kFragmentShaderBlending =
 
 std::vector<GLuint> BackgroundRenderer::shaderPrograms_;
 
+BackgroundRenderer::~BackgroundRenderer()
+{
+	for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
+	{
+		glDeleteShader(shaderPrograms_[i]);
+	}
+	shaderPrograms_.clear();
+}
+
 void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
 {
+	LOGI("textureId=%d", textureId);
+
   texture_id_ = textureId;
 #ifdef __ANDROID__
   oes_ = oes;
@@ -175,19 +188,19 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
   GLuint screenScale_handle = glGetUniformLocation(program, "uRedUnknown");
   glUniform1i(screenScale_handle, redUnknown);
 
-  GLuint attribute_vertices_ = glGetAttribLocation(program, "a_Position");
+  GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
-  GLuint attribute_uvs_ = glGetAttribLocation(program, "a_TexCoord");
+  GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
-    
-  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
-  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
 
-  glEnableVertexAttribArray(attribute_vertices_);
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
-  glEnableVertexAttribArray(attribute_uvs_);
+  glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
+
+  glEnableVertexAttribArray(attributeVertices);
+  glEnableVertexAttribArray(attributeUvs);
 
   glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
 
-  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attributeVertices);
-  glDisableVertexAttribArray(attribute_uvs_);
+  glDisableVertexAttribArray(attributeUvs);
 
   glUseProgram(0);
   glDepthMask(GL_TRUE);

app/android/jni/background_renderer.h

@@ -51,7 +51,7 @@ public:
 
  public:
   BackgroundRenderer() = default;
-  ~BackgroundRenderer() = default;
+  ~BackgroundRenderer();
 
   // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
   // other methods below.

app/android/res/values/strings.xml

@@ -65,7 +65,7 @@
     <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
     <string name="pref_default_rendering_texture_decimation">4</string>
     <string name="pref_key_blending">pref_key_blending</string>
-    <string name="pref_default_blending">false</string>
+    <string name="pref_default_blending">true</string>
     <string name="pref_key_background_color">pref_key_background_color</string>
     <string name="pref_default_background_color">0.2</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>

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

@@ -307,8 +307,6 @@ public class ARCoreSharedCamera {
 		}
 		mTOFImageReader.stopBackgroundThread();
 	}
-	
-	private long mPreviousTime = 0;
 
     /**
      * Return true if the given array contains the given integer.
@@ -335,8 +333,6 @@ public class ARCoreSharedCamera {
 		close();
 		
 		startBackgroundThread();
-		
-		mPreviousTime = System.currentTimeMillis();
 
 		if(cameraTextureId == -1)
 		{
@@ -641,13 +637,6 @@ public class ARCoreSharedCamera {
 			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));
 			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp);
 			
-			int rateMs = 100; // send images at most 10 Hz (to save battery)
-			if(System. currentTimeMillis() - mPreviousTime < rateMs)
-			{
-				return;
-			}
-			mPreviousTime = System. currentTimeMillis();
-			
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
@@ -683,7 +672,7 @@ public class ARCoreSharedCamera {
 				frame.transformCoordinates2d(
 				        Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES,
 				        QUAD_COORDS,
-				        Coordinates2d.TEXTURE_NORMALIZED,
+				        Coordinates2d.IMAGE_NORMALIZED,
 				        texCoord);
 				
 				float[] p = new float[16];

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

@@ -1075,6 +1075,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		setAndroidOrientation();
 
+		updateCameraDriverSettings();
 		updatePreferences();
 
 		if(mState == State.STATE_MAPPING || mState == State.STATE_CAMERA)