Tango: Added ICP refining option, mesh vs texture vs point cloud options, show/hide grid option, drift correction option

app/android/jni/CameraTango.cpp

@@ -37,7 +37,7 @@ namespace rtabmap {
 
 #define nullptr 0
 const int kVersionStringLength = 128;
-const int holeSize = 10;
+const int holeSize = 1;
 const float maxDepthError = 0.10;
 
 // Callbacks
@@ -558,6 +558,8 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			poseDepth.setNull();
 		}
 
+		int scanDownsampling = 10;
+		cv::Mat scan;
 		if(!poseDepth.isNull() && !poseColor.isNull())
 		{
 			// The Color Camera frame at timestamp t0 with respect to Depth
@@ -568,11 +570,18 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			int pixelsSet = 0;
 			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
 			CameraModel depthModel = model_.scaled(1.0f/8.0f);
+			std::vector<cv::Point3f> scanData(cloud.total());
+			int oi=0;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
-				cv::Vec3f & p = cloud.at<cv::Vec3f>(i);
+				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)
+				{
+					scanData.at(oi++) = pt;
+				}
+
 				int pixel_x, pixel_y;
 				// get the coordinate on image plane.
 				pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
@@ -591,6 +600,10 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 					}
 				}
 			}
+			if(oi)
+			{
+				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+			}
 			LOGI("pixels depth set= %d", pixelsSet);
 		}
 		else
@@ -607,7 +620,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			//Rotate in RTAB-Map's coordinate
 			Transform odom = rtabmap_world_T_opengl_world * poseColorOpenGL * depth_camera_T_opengl_camera * model.localTransform().inverse();
 
-			data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			data.setGroundTruth(odom);
 		}
 		else

app/android/jni/RTABMapApp.cpp

@@ -76,13 +76,23 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
+
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("false")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), std::string("6")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
 
 	return parameters;
 }
@@ -95,13 +105,14 @@ RTABMapApp::RTABMapApp() :
 		odomCloudShown_(true),
 		graphOptimization_(true),
 		nodesFiltering_(false),
+		driftCorrection_(false),
 		localizationMode_(false),
 		trajectoryMode_(false),
 		autoExposure_(false),
 		fullResolution_(false),
 		maxCloudDepth_(0.0),
 		meshTrianglePix_(1),
-		meshAngleToleranceDeg_(10.0),
+		meshAngleToleranceDeg_(15.0),
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
@@ -109,7 +120,6 @@ RTABMapApp::RTABMapApp() :
 		renderingFPS_(0.0f)
 
 {
-
 }
 
 RTABMapApp::~RTABMapApp() {
@@ -422,7 +432,7 @@ int RTABMapApp::Render()
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
 							LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
-							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
 
 							if(cloud->size() && indices->size())
 							{
@@ -525,7 +535,7 @@ int RTABMapApp::Render()
 				if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), 1, maxCloudDepth_);
 					if(cloud->size())
 					{
 						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
@@ -599,9 +609,9 @@ void RTABMapApp::setOdomCloudShown(bool shown)
 	odomCloudShown_ = shown;
 	main_scene_.setTraceVisible(shown);
 }
-void RTABMapApp::setMeshRendering(bool enabled)
+void RTABMapApp::setMeshRendering(bool enabled, bool withTexture)
 {
-	main_scene_.setMeshRendering(enabled);
+	main_scene_.setMeshRendering(enabled, withTexture);
 }
 void RTABMapApp::setLocalizationMode(bool enabled)
 {
@@ -643,9 +653,22 @@ void RTABMapApp::setNodesFiltering(bool enabled)
 	nodesFiltering_ = enabled;
 	setGraphOptimization(graphOptimization_); // this will resend the graph if paused
 }
+void RTABMapApp::setDriftCorrection(bool enabled)
+{
+	driftCorrection_ = enabled;
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0")));
+	this->post(new rtabmap::ParamEvent(parameters));
+}
 void RTABMapApp::setGraphVisible(bool visible)
 {
 	main_scene_.setGraphVisible(visible);
+	main_scene_.setTraceVisible(visible);
+}
+void RTABMapApp::setGridVisible(bool visible)
+{
+	main_scene_.setGridVisible(visible);
 }
 
 void RTABMapApp::setAutoExposure(bool enabled)
@@ -907,49 +930,64 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 
 int RTABMapApp::postProcessing(int approach)
 {
+	LOGI("postProcessing(%d)", approach);
 	int returnedValue = 0;
 	if(rtabmap_)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
-		if(approach == 2 || approach == 0)
-		{
-			if(approach == 2)
-			{
-				// detect more loop closures
-				returnedValue = rtabmap_->detectMoreLoopClosures();
-			}
 
-			if(returnedValue >= 0)
+		// detect more loop closures
+		if(approach == -1 || approach == 2)
+		{
+			// detect more loop closures
+			returnedValue = rtabmap_->detectMoreLoopClosures(0.5f, M_PI/6.0f, approach == -1?3:1);
+		}
+
+		// ICP refining
+		if(returnedValue >=0 && ((approach == -1 && !driftCorrection_) || approach == 3))
+		{
+			rtabmap::ParametersMap parameters;
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string("1"))); // ICP
+			rtabmap_->parseParameters(parameters);
+			int r = rtabmap_->refineLinks();
+			if(approach == 3 )
+			{
+				returnedValue = r;
+			}
+			// reset back default registration (visual)
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0"))); // Visual
+			rtabmap_->parseParameters(parameters);
+		}
+
+		// graph optimization
+		if(returnedValue >=0)
+		{
+			if (approach == 1)
+			{
+				if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
+				{
+					std::map<int, rtabmap::Signature> signatures;
+					rtabmap_->getGraph(poses, links, false, true, &signatures);
+
+					rtabmap::ParametersMap param;
+					param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
+					param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
+					rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
+					poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
+					delete sba;
+				}
+				else
+				{
+					LOGE("g2o not available!");
+				}
+			}
+			else
 			{
 				// simple graph optmimization
 				rtabmap_->getGraph(poses, links, true, true);
 			}
 		}
-		else if (approach == 1)
-		{
-			if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
-			{
-				std::map<int, rtabmap::Signature> signatures;
-				rtabmap_->getGraph(poses, links, false, true, &signatures);
-
-				rtabmap::ParametersMap param;
-				param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
-                param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
-				rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
-				poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
-				delete sba;
-			}
-			else
-			{
-				LOGE("g2o not available!");
-			}
-		}
-		else
-		{
-			LOGE("Invalid approach %d (should be 0 (graph optimization), 1 (sba) or 2 (detect more loop closures))", approach);
-			returnedValue = -1;
-		}
 
 		if(poses.size())
 		{

app/android/jni/RTABMapApp.h

@@ -112,12 +112,14 @@ class RTABMapApp : public UEventsHandler {
   void setPausedMapping(bool paused);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
-  void setMeshRendering(bool enabled);
+  void setMeshRendering(bool enabled, bool withTexture);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
   void setNodesFiltering(bool enabled);
+  void setDriftCorrection(bool enabled);
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
   void setMaxCloudDepth(float value);
@@ -145,6 +147,7 @@ class RTABMapApp : public UEventsHandler {
   bool odomCloudShown_;
   bool graphOptimization_;
   bool nodesFiltering_;
+  bool driftCorrection_;
   bool localizationMode_;
   bool trajectoryMode_;
   bool autoExposure_;

app/android/jni/jni_interface.cpp

@@ -133,9 +133,9 @@ Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jobject, bool enabled, bool withTexture)
 {
-	return app.setMeshRendering(enabled);
+	return app.setMeshRendering(enabled, withTexture);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
@@ -162,12 +162,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
 	return app.setNodesFiltering(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDriftCorrection(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setDriftCorrection(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 		JNIEnv*, jobject, bool visible)
 {
 	return app.setGraphVisible(visible);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
+		JNIEnv*, jobject, bool visible)
+{
+	return app.setGridVisible(visible);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
 		JNIEnv*, jobject, bool enabled)
 {

app/android/jni/point_cloud_drawable.cpp

@@ -180,11 +180,11 @@ void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 	pose_ = glmFromTransform(pose);
 }
 
-void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize) {
+void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize, bool textureRendering) {
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		if(meshRendering && textures_)
+		if(meshRendering && textureRendering && textures_)
 		{
 			glUseProgram(texture_shader_program_);
 

app/android/jni/point_cloud_drawable.h

@@ -60,7 +60,7 @@ class PointCloudDrawable {
   // @param view_mat: view matrix from current render camera.
   // @param model_mat: model matrix for this point cloud frame.
   // @param vertices: all vertices in this point cloud frame.
-  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f);
+  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f, bool textureRendering = false);
 
  private:
   // Vertex buffer of the point cloud geometry.

app/android/jni/scene.cpp

@@ -109,6 +109,7 @@ Scene::Scene() :
 		trace_(0),
 		graph_(0),
 		graphVisible_(true),
+		gridVisible_(true),
 		traceVisible_(true),
 		currentPose_(0),
 		cloud_shader_program_(0),
@@ -116,6 +117,7 @@ Scene::Scene() :
 		graph_shader_program_(0),
 		mapRendering_(true),
 		meshRendering_(true),
+		meshRenderingTexture_(true),
 		pointSize_(3.0f) {}
 
 Scene::~Scene() {DeleteResources();}
@@ -275,9 +277,11 @@ int Scene::Render() {
 	  }
   }
 
-
+  	if(gridVisible_)
-	grid_->Render(gesture_camera_->GetProjectionMatrix(),
+  	{
-				  gesture_camera_->GetViewMatrix());
+  		grid_->Render(gesture_camera_->GetProjectionMatrix(),
+  					  gesture_camera_->GetViewMatrix());
+  	}
 
 	bool frustumCulling = true;
 	int cloudDrawn=0;
@@ -327,7 +331,7 @@ int Scene::Render() {
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				++cloudDrawn;
-				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
 			}
 		}
 	}
@@ -338,7 +342,7 @@ int Scene::Render() {
 			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
 			}
 		}
 	}
@@ -400,6 +404,11 @@ void Scene::setGraphVisible(bool visible)
 	graphVisible_ = visible;
 }
 
+void Scene::setGridVisible(bool visible)
+{
+	gridVisible_ = visible;
+}
+
 void Scene::setTraceVisible(bool visible)
 {
 	traceVisible_ = visible;

app/android/jni/scene.h

@@ -94,6 +94,7 @@ class Scene {
   		const std::multimap<int, rtabmap::Link> & links);
 
   void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
 
   void addCloud(
@@ -109,7 +110,7 @@ class Scene {
   std::set<int> getAddedClouds() const;
 
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
-  void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
+  void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
 
  private:
@@ -129,6 +130,7 @@ class Scene {
   tango_gl::Trace* trace_;
   GraphDrawable * graph_;
   bool graphVisible_;
+  bool gridVisible_;
   bool traceVisible_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
@@ -142,6 +144,7 @@ class Scene {
 
   bool mapRendering_;
   bool meshRendering_;
+  bool meshRenderingTexture_;
   float pointSize_;
 };
 

app/android/res/menu/optionmenu.xml

@@ -8,11 +8,15 @@
     <group android:id="@+id/group_actions">
        <item android:id="@+id/post_processing" android:title="Post-Processing...">
           <menu>
-	         <group android:id="@+id/group_post_processing">
+             <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
-	            <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+             <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
-	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
+	            <menu >
-	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-	         </group>   
+	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
+	              <item android:id="@+id/icp_refining" android:title="ICP Refining" />
+	              <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	            </menu>  
+	         </item>
           </menu>
        </item>
        <item android:id="@+id/open" android:title="Open"/>
@@ -31,14 +35,23 @@
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
-            <item android:id="@+id/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
+            <item android:id="@+id/menu_rendering" android:checkable="false" android:title="Mesh Rendering..." >
+           			<menu>
+           			   <group android:checkableBehavior="single">
+	           			   <item android:id="@+id/point_cloud" android:title="Point Cloud" />
+				           <item android:id="@+id/mesh" android:title="Mesh" />
+				           <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
+			           </group>
+			           <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
+            		   <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
+            		   <item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
+			       </menu>
+            </item>
             <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
             <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
+            <item android:id="@+id/grid_visible" android:checked="true" android:title="Grid Visible" />
             <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
-            <item android:id="@+id/max_depth" android:checkable="false" android:title="Cloud/Mesh Max Depth..." />
-            <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
-            <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
         </group>   
         </menu>
 	    </item>
@@ -49,6 +62,7 @@
 	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
 	           <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
 	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
+	           <item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" />
 	           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
 	           <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
 		           <menu >

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

@@ -74,6 +74,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private MenuItem mItemExport;
   private MenuItem mItemLocalizationMode;
   private MenuItem mItemTrajectoryMode;
+  private MenuItem mItemRenderingPointCloud;
+  private MenuItem mItemRenderingMesh;
+  private MenuItem mItemRenderingTextureMesh;
+  
   
   private String mOpenedDatabasePath = "";
   private String mTempDatabasePath = "";
@@ -81,7 +85,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private String mWorkingDirectory = "";
   
   private int mMaxDepthIndex = 5;
-  private int mMeshAngleToleranceIndex = 1;
+  private int mMeshAngleToleranceIndex = 2;
   private int mMeshTriangleSizeIndex = 0;
   
   private int mParamUpdateRateHzIndex = 1;
@@ -316,6 +320,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  mItemExport = menu.findItem(R.id.export);
 	  mItemLocalizationMode = menu.findItem(R.id.localization_mode);
 	  mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
+	  mItemRenderingPointCloud = menu.findItem(R.id.point_cloud);
+	  mItemRenderingMesh = menu.findItem(R.id.mesh);
+	  mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
 	  mItemSave.setEnabled(false);
 	  mItemExport.setEnabled(false);
 	  mItemOpen.setEnabled(false);
@@ -652,6 +659,33 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
     	  }
       } 
+      else if (itemId == R.id.post_processing_standard)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while optimizing..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int loopDetected = RTABMapLib.postProcessing(-1);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(loopDetected >= 0)
+								{
+									mTotalLoopClosures+=loopDetected;
+									mToast.makeText(getActivity(), String.format("Optimization done!"), mToast.LENGTH_SHORT).show();
+								}
+								else if(loopDetected < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Optimization failed!"), mToast.LENGTH_SHORT).show();
+								}
+					    	}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
       else if (itemId == R.id.detect_more_loop_closures)
       {
 			mProgressDialog.setTitle("Post-Processing");
@@ -679,6 +713,32 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			});
 			workingThread.start();
       }
+      else if (itemId == R.id.icp_refining)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while refining links..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int linksRefined = RTABMapLib.postProcessing(3);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(linksRefined >= 0)
+								{
+									mToast.makeText(getActivity(), String.format("Refining done! %d link(s) refined.", linksRefined), mToast.LENGTH_SHORT).show();
+								}
+								else if(linksRefined < 0)
+								{
+									mToast.makeText(getActivity(), String.format("Refining failed!"), mToast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
       else if (itemId == R.id.global_graph_optimization)
       {
 			mProgressDialog.setTitle("Post-Processing");
@@ -743,10 +803,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		  mLayoutDebug.setVisibility(LinearLayout.VISIBLE);
     	  }
       }
-      else if(itemId == R.id.mesh_rendering)
+      else if(itemId == R.id.mesh || itemId == R.id.texture_mesh || itemId == R.id.point_cloud)
       {
-    	  item.setChecked(!item.isChecked());
+    	  item.setChecked(true);
-    	  RTABMapLib.setMeshRendering(item.isChecked());
+    	  RTABMapLib.setMeshRendering(
+    			  mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), 
+    			  mItemRenderingTextureMesh.isChecked());
       }
       else if(itemId == R.id.map_shown)
       {
@@ -778,11 +840,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setNodesFiltering(item.isChecked());
       }
+      else if(itemId == R.id.drift_correction)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setDriftCorrection(item.isChecked());
+      }
       else if(itemId == R.id.graph_visible)
       {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setGraphVisible(item.isChecked());
       }
+      else if(itemId == R.id.grid_visible)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setGridVisible(item.isChecked());
+      }
       else if(itemId == R.id.auto_exposure)
       {
     	  item.setChecked(!item.isChecked());

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

@@ -58,12 +58,14 @@ public class RTABMapLib
     public static native void setPausedMapping(boolean paused);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled);
+    public static native void setMeshRendering(boolean enabled, boolean withTexture);
     public static native void setLocalizationMode(boolean enabled);
     public static native void setTrajectoryMode(boolean enabled);
     public static native void setGraphOptimization(boolean enabled);
     public static native void setNodesFiltering(boolean enabled);
+    public static native void setDriftCorrection(boolean enabled);
     public static native void setGraphVisible(boolean visible);
+    public static native void setGridVisible(boolean visible);
     public static native void setAutoExposure(boolean enabled);
     public static native void setFullResolution(boolean enabled);
     public static native void setMaxCloudDepth(float value);

corelib/include/rtabmap/core/Memory.h

@@ -93,8 +93,7 @@ public:
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link, bool addInDatabase = false);
-	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
+	void updateLink(const Link & link, bool updateInDatabase = false);
-	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
 	void removeAllVirtualLinks();
 	void removeVirtualLinks(int signatureId);
 	std::map<int, int> getNeighborsId(

corelib/include/rtabmap/core/Rtabmap.h

@@ -128,6 +128,7 @@ public:
 			bool global,
 			std::map<int, Signature> * signatures = 0);
 	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
+	int refineLinks();
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success

corelib/include/rtabmap/core/util3d.h

@@ -141,7 +141,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap(),
+		const ParametersMap & stereoParameters = ParametersMap(),
 		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 /**
@@ -164,7 +164,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap(),
+		const ParametersMap & stereoParameters = ParametersMap(),
 		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(

corelib/src/Memory.cpp

@@ -2422,54 +2422,63 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
 	return true;
 }
 
-void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
+void Memory::updateLink(const Link & link, bool updateInDatabase)
 {
-	Signature * fromS = this->_getSignature(fromId);
+	Signature * fromS = this->_getSignature(link.from());
-	Signature * toS = this->_getSignature(toId);
+	Signature * toS = this->_getSignature(link.to());
 
-	if(fromS->hasLink(toId) && toS->hasLink(fromId))
+	if(fromS && toS)
 	{
-		Link::Type type = fromS->getLinks().at(toId).type();
+		if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
-		fromS->removeLink(toId);
-		toS->removeLink(fromId);
-
-		fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
-		toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
-
-		if(type!=Link::kVirtualClosure)
 		{
-			_linksChanged = true;
+			Link::Type oldType = fromS->getLinks().at(link.to()).type();
+
+			fromS->removeLink(link.to());
+			toS->removeLink(link.from());
+
+			fromS->addLink(link);
+			toS->addLink(link.inverse());
+
+			if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
+			{
+				_linksChanged = true;
+			}
 		}
+		else
+		{
+			UERROR("fromId=%d and toId=%d are not linked!", link.from(), link.to());
+		}
+	}
+	else if(!updateInDatabase)
+	{
+		if(!fromS)
+		{
+			UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from());
+		}
+		if(!toS)
+		{
+			UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
+		}
+	}
+	else if(fromS)
+	{
+		UDEBUG("Update link between %d and %d (db)", link.from(), link.to());
+		fromS->removeLink(link.to());
+		fromS->addLink(link);
+		_dbDriver->updateLink(link.inverse());
+	}
+	else if(toS)
+	{
+		UDEBUG("Update link between %d (db) and %d", link.from(), link.to());
+		toS->removeLink(link.from());
+		toS->addLink(link.inverse());
+		_dbDriver->updateLink(link);
 	}
 	else
 	{
-		UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
+		UDEBUG("Update link between %d (db) and %d (db)", link.from(), link.to());
-	}
+		_dbDriver->updateLink(link);
-}
+		_dbDriver->updateLink(link.inverse());
-
-void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance)
-{
-	Signature * fromS = this->_getSignature(fromId);
-	Signature * toS = this->_getSignature(toId);
-
-	if(fromS->hasLink(toId) && toS->hasLink(fromId))
-	{
-		Link::Type type = fromS->getLinks().at(toId).type();
-		fromS->removeLink(toId);
-		toS->removeLink(fromId);
-
-		cv::Mat infMatrix  = covariance.inv();
-		fromS->addLink(Link(fromId, toId, type, transform, infMatrix));
-		toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix));
-
-		if(type!=Link::kVirtualClosure)
-		{
-			_linksChanged = true;
-		}
-	}
-	else
-	{
-		UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
 	}
 }
 

corelib/src/Rtabmap.cpp

@@ -1012,7 +1012,7 @@ bool Rtabmap::process(
 				{
 					// set small variance
 					UDEBUG("Set small variance. The robot is not moving.");
-					_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
+					_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
 				}
 			}
 			else
@@ -1034,7 +1034,7 @@ bool Rtabmap::process(
 								guess.prettyPrint().c_str(),
 								t.prettyPrint().c_str());
 						UASSERT(info.variance > 0.0);
-						_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
+						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.variance, info.variance));
 
 						if(_optimizeFromGraphEnd)
 						{
@@ -1058,7 +1058,7 @@ bool Rtabmap::process(
 						if(info.variance > 0)
 						{
 							double sqrtVar = sqrt(info.variance);
-							_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
+							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrtVar, sqrtVar));
 						}
 					}
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1821,7 +1821,7 @@ bool Rtabmap::process(
 				UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
 
 				// segment poses by paths, only one detection per path
-				std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
+				std::map<int, std::map<int, Transform> > nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
 				UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
 
 				for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
@@ -1886,13 +1886,21 @@ bool Rtabmap::process(
 				// 2) compare locally with nearest locations by scan matching
 				//
 				UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
-				if( !signature->sensorData().laserScanCompressed().empty() &&
+				if( _proximityMaxNeighbors > 0 &&
+					!signature->sensorData().laserScanCompressed().empty() &&
 					(_memory->isIncremental() || lastProximitySpaceClosureId == 0))
 				{
 					// In localization mode, no need to check local loop
 					// closures if we are already localized by at least one
 					// local visual closure above.
 
+					// Parse again with if different (normally, maxNeighbors would be smaller than MaxGraphDepth)
+					if(_proximityMaxNeighbors != _proximityMaxGraphDepth)
+					{
+						nearestPaths = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxNeighbors);
+						UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
+					}
+
 					proximitySpacePaths = (int)nearestPaths.size();
 					for(std::map<int, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
 							iter!=nearestPaths.rend() &&
@@ -3412,6 +3420,52 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 	return (int)loopClosuresAdded.size();
 }
 
+int Rtabmap::refineLinks()
+{
+	if(!_rgbdSlamMode)
+	{
+		UERROR("Refining links can be done only in RGBD-SLAM mode.");
+		return -1;
+	}
+
+	std::list<Link> linksRefined;
+
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	std::map<int, Signature> signatures;
+	this->getGraph(poses, links, false, true, &signatures);
+
+	int i=0;
+	for(std::multimap<int, Link>::iterator iter=links.begin(); iter!= links.end(); ++iter)
+	{
+		int from = iter->second.from();
+		int to = iter->second.to();
+
+		UASSERT(signatures.find(from) != signatures.end());
+		UASSERT(signatures.find(to) != signatures.end());
+
+		RegistrationInfo info;
+		// use signatures instead of IDs because some signatures may not be in WM
+		Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), iter->second.transform(), &info);
+
+		if(!t.isNull())
+		{
+			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.variance, info.variance));
+			UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
+		}
+	}
+	UINFO("Total refined %d links.", (int)linksRefined.size());
+
+	if(linksRefined.size())
+	{
+		for(std::list<Link>::iterator iter=linksRefined.begin(); iter!=linksRefined.end(); ++iter)
+		{
+			_memory->updateLink(*iter, true);
+		}
+	}
+	return (int)linksRefined.size();
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);

corelib/src/util3d.cpp

@@ -699,7 +699,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices,
-		const ParametersMap & parameters,
+		const ParametersMap & stereoParameters,
 		const std::vector<float> & roiRatios)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -815,7 +815,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		}
 
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), stereoParameters),
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
@@ -839,7 +839,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices,
-		const ParametersMap & parameters,
+		const ParametersMap & stereoParameters,
 		const std::vector<float> & roiRatios)
 {
 	UASSERT(!sensorData.imageRaw().empty());
@@ -961,7 +961,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				maxDepth,
 				minDepth,
 				validIndices,
-				parameters);
+				stereoParameters);
 
 		if(cloud->size())
 		{