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() {
 	  }
   }
 
-
-	grid_->Render(gesture_camera_->GetProjectionMatrix(),
-				  gesture_camera_->GetViewMatrix());
+  	if(gridVisible_)
+  	{
+  		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/detect_more_loop_closures" android:title="Detect More Loop Closures" />
-	            <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
-	            <item android:id="@+id/sba" android:title="Bundle Adjustement" />
-	         </group>   
+             <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
+             <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
+	            <menu >
+	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
+	              <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());
-    	  RTABMapLib.setMeshRendering(item.isChecked());
+    	  item.setChecked(true);
+    	  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);