Tango #57: Increased version to 0.11.3, updated Post-Processing actions, added mesh rendering actions, fixed point cloud rendering

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

app/android/AndroidManifest.xml.in

@@ -3,7 +3,7 @@
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
         android:versionCode="1"
-        android:versionName="0.11.2">
+        android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />

app/android/CMakeLists.txt

@@ -14,11 +14,20 @@ if(NOT ANDROID_EXECUTABLE)
   message(FATAL_ERROR "Can not find android command line tool: android")
 endif()
 
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
   "${CMAKE_CURRENT_BINARY_DIR}/AndroidManifest.xml"
   COPYONLY)
   
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/info.txt.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/res/raw/info.txt")
+  
+
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
   "${CMAKE_CURRENT_BINARY_DIR}/ant.properties"

app/android/info.txt.in

@@ -1,5 +1,5 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
-Version 0.11.2<br>
+Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
 Copyright 2016<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>

app/android/jni/RTABMapApp.cpp

@@ -50,8 +50,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/obj_io.h>
 
 const int kVersionStringLength = 128;
-const float meshAngleTolerance = 0.1745; // 10 degrees
-const int meshTrianglePixels = 1;
 
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
@@ -86,7 +84,6 @@ RTABMapApp::RTABMapApp() :
 		rtabmapThread_(0),
 		rtabmap_(0),
 		logHandler_(0),
-		mapCloudShown_(true),
 		odomCloudShown_(true),
 		graphOptimization_(true),
 		localizationMode_(false),
@@ -94,6 +91,8 @@ RTABMapApp::RTABMapApp() :
 		autoExposure_(false),
 		fullResolution_(false),
 		maxCloudDepth_(0.0),
+		meshTrianglePix_(1),
+		meshAngleToleranceDeg_(10.0),
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
@@ -193,21 +192,6 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 			true,
 			true);
 
-	if(poses.size() > 1 &&
-		rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
-	{
-		rtabmap::ParametersMap param;
-		param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
-		rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
-		poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
-		delete sba;
-
-		if(poses.size())
-		{
-			rtabmap_->setOptimizedPoses(poses);
-		}
-	}
-
 	clearSceneOnNextRender_ = true;
 	rtabmap::Statistics stats;
 	stats.setSignatures(signatures);
@@ -421,7 +405,7 @@ int RTABMapApp::Render()
 								filter.setInputCloud(cloud);
 								filter.filter(*output);
 
-								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
+								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 								std::vector<pcl::Vertices> outputPolygons;
 
@@ -464,7 +448,7 @@ int RTABMapApp::Render()
 			iter!=addedClouds.end();
 			++iter)
 		{
-			if(*iter > 0 && (!mapCloudShown_ || poses.find(*iter) == poses.end()))
+			if(*iter > 0 && poses.find(*iter) == poses.end())
 			{
 				main_scene_.setCloudVisible(*iter, false);
 			}
@@ -485,22 +469,24 @@ int RTABMapApp::Render()
 			}
 		}
 
+		main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
+
 		//just process the last one
 		if(set && !event.pose().isNull())
 		{
-			main_scene_.setCloudVisible(-1, false);
 			if(odomCloudShown_ && !trajectoryMode_)
 			{
 				if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
 				{
-					LOGI("Creating Odom cloud (rgb=%dx%d depth=%dx%d)",
-							event.data().imageRaw().cols, event.data().imageRaw().rows,
-							event.data().depthRaw().cols, event.data().depthRaw().rows);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
 					if(cloud->size())
 					{
-						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleTolerance, false, meshTrianglePixels);
+						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
+													event.data().imageRaw().cols, event.data().imageRaw().rows,
+													event.data().depthRaw().cols, event.data().depthRaw().rows,
+													(int)cloud->width, (int)cloud->height);
+						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
 						main_scene_.setCloudVisible(-1, true);
 					}
@@ -551,7 +537,7 @@ void RTABMapApp::setPausedMapping(bool paused)
 }
 void RTABMapApp::setMapCloudShown(bool shown)
 {
-	mapCloudShown_ = shown;
+	main_scene_.setMapRendering(shown);
 }
 void RTABMapApp::setOdomCloudShown(bool shown)
 {
@@ -618,6 +604,16 @@ void RTABMapApp::setMaxCloudDepth(float value)
 	maxCloudDepth_ = value;
 }
 
+void RTABMapApp::setMeshAngleTolerance(float value)
+{
+	meshAngleToleranceDeg_ = value;
+}
+
+void RTABMapApp::setMeshTriangleSize(int value)
+{
+	 meshTrianglePix_ = value;
+}
+
 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
 {
 	if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
@@ -763,6 +759,14 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 
 				UINFO("Saving obj to %s.", filePath.c_str());
 				success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
+				if(success)
+				{
+					UINFO("Saved obj to %s!", filePath.c_str());
+				}
+				else
+				{
+					UERROR("Failed saving obj to %s!", filePath.c_str());
+				}
 			}
 		}
 	}
@@ -806,39 +810,64 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
 			mesh.polygons = mergedPolygons;
 
-			UINFO("Saving to %s.", filePath.c_str());
+			UINFO("Saving ply to %s.", filePath.c_str());
 			success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
+			if(success)
+			{
+				UINFO("Saved ply to %s!", filePath.c_str());
+			}
+			else
+			{
+				UERROR("Failed saving ply to %s!", filePath.c_str());
+			}
 		}
 	}
 	return success;
 }
 
-int RTABMapApp::postProcessing(bool graphOptimizationOnly)
+int RTABMapApp::postProcessing(int approach)
 {
-	int detectedLoopClosures = 0;
+	int returnedValue = 0;
 	if(rtabmap_)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
-		if(graphOptimizationOnly)
+		if(approach == 2 || approach == 0)
 		{
-			rtabmap_->getGraph(poses, links, true, true);
+			if(approach == 2)
+			{
+				// detect more loop closures
+				returnedValue = rtabmap_->detectMoreLoopClosures();
+			}
+
+			if(returnedValue >= 0)
+			{
+				// simple graph optmimization
+				rtabmap_->getGraph(poses, links, true, true);
+			}
 		}
-		else
+		else if (approach == 1)
 		{
-			detectedLoopClosures = rtabmap_->detectMoreLoopClosures();
-
-			std::map<int, rtabmap::Signature> signatures;
-			rtabmap_->getGraph(poses, links, false, true, &signatures);
-
 			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(), "10"));
+				param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
 				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())
@@ -851,8 +880,12 @@ int RTABMapApp::postProcessing(bool graphOptimizationOnly)
 
 			rtabmap_->setOptimizedPoses(poses);
 		}
+		else
+		{
+			returnedValue = -1;
+		}
 	}
-	return detectedLoopClosures;
+	return returnedValue;
 }
 
 void RTABMapApp::handleEvent(UEvent * event)

app/android/jni/RTABMapApp.h

@@ -123,12 +123,14 @@ class RTABMapApp : public UEventsHandler {
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
   void setMaxCloudDepth(float value);
+  void setMeshAngleTolerance(float value);
+  void setMeshTriangleSize(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
   void save();
   bool exportMesh(const std::string & filePath);
-  int postProcessing(bool graphOptimizationOnly);
+  int postProcessing(int approach);
 
  protected:
   virtual void handleEvent(UEvent * event);
@@ -142,7 +144,6 @@ class RTABMapApp : public UEventsHandler {
   rtabmap::Rtabmap * rtabmap_;
   LogHandler * logHandler_;
 
-  bool mapCloudShown_;
   bool odomCloudShown_;
   bool graphOptimization_;
   bool localizationMode_;
@@ -150,6 +151,8 @@ class RTABMapApp : public UEventsHandler {
   bool autoExposure_;
   bool fullResolution_;
   float maxCloudDepth_;
+  int meshTrianglePix_;
+  float meshAngleToleranceDeg_;
 
   rtabmap::ParametersMap mappingParameters_;
 

app/android/jni/jni_interface.cpp

@@ -179,6 +179,18 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 {
 	return app.setMaxCloudDepth(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMeshAngleTolerance(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshTriangleSize(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)
@@ -214,9 +226,9 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
-		JNIEnv* env, jobject, bool graphOptimizationOnly)
+		JNIEnv* env, jobject, int approach)
 {
-	return app.postProcessing(graphOptimizationOnly);
+	return app.postProcessing(approach);
 }
 
 

app/android/jni/point_cloud_drawable.cpp

@@ -46,7 +46,8 @@ PointCloudDrawable::PointCloudDrawable(
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
+		cloud_shader_program_(cloudShaderProgram),
+		texture_shader_program_(textureShaderProgram)
 {
 	UASSERT(!cloud->empty());
 
@@ -57,7 +58,7 @@ PointCloudDrawable::PointCloudDrawable(
 		return;
 	}
 
-	if(textureShaderProgram)
+	if(!cloud->is_dense && !image.empty())
 	{
 		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
 		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
@@ -74,16 +75,19 @@ PointCloudDrawable::PointCloudDrawable(
 	std::vector<float> vertices;
 	if(textures_)
 	{
-		vertices = std::vector<float>(cloud->size()*5);
+		vertices = std::vector<float>(cloud->size()*6);
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
-			vertices[i*5] = cloud->at(i).x;
+			vertices[i*6] = cloud->at(i).x;
-			vertices[i*5+1] = cloud->at(i).y;
+			vertices[i*6+1] = cloud->at(i).y;
-			vertices[i*5+2] = cloud->at(i).z;
+			vertices[i*6+2] = cloud->at(i).z;
+
+			// rgb
+			vertices[i*6+3] = cloud->at(i).rgb;
 
 			// texture uv
-			vertices[i*5+3] = float(i % cloud->width)/float(cloud->width); //u
+			vertices[i*6+4] = float(i % cloud->width)/float(cloud->width); //u
-			vertices[i*5+4] = float(i/cloud->width)/float(cloud->height);  //v
+			vertices[i*6+5] = float(i/cloud->width)/float(cloud->height);  //v
 		}
 	}
 	else
@@ -180,47 +184,60 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		glUseProgram(shader_program_);
+		if(meshRendering && textures_)
-
-		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
-		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
-		glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
-
-		if(textures_)
 		{
+			glUseProgram(texture_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(texture_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
 			// Texture activate unit 0
 			glActiveTexture(GL_TEXTURE0);
 			// Bind the texture to this unit.
 			glBindTexture(GL_TEXTURE_2D, textures_);
 			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
-			GLuint texture_handle = glGetUniformLocation(shader_program_, "u_Texture");
+			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
 			glUniform1i(texture_handle, 0);
 
-			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
-			GLint attribute_texture = glGetAttribLocation(shader_program_, "a_TexCoordinate");
+			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
 
 			glEnableVertexAttribArray(attribute_vertex);
 			glEnableVertexAttribArray(attribute_texture);
 			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), 0);
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
-			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 
 			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
 		}
 		else // point cloud or colored mesh
 		{
-			GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+			glUseProgram(cloud_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
 			glUniform1f(point_size_handle_, pointSize);
 
-			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
-			GLint attribute_color = glGetAttribLocation(shader_program_, "color");
+			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
 
 			glEnableVertexAttribArray(attribute_vertex);
 			glEnableVertexAttribArray(attribute_color);
 			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
+			if(textures_)
-			glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			{
-
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 6*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
+			else
+			{
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
 			if(meshRendering && polygons_.size())
 			{
 				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());

app/android/jni/point_cloud_drawable.h

@@ -71,7 +71,8 @@ class PointCloudDrawable {
   glm::mat4 pose_;
   bool visible_;
 
-  GLuint shader_program_;
+  GLuint cloud_shader_program_;
+  GLuint texture_shader_program_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -114,6 +114,7 @@ Scene::Scene() :
 		cloud_shader_program_(0),
 		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		mapRendering_(true),
 		meshRendering_(true),
 		pointSize_(3.0f) {}
 
@@ -280,7 +281,7 @@ int Scene::Render() {
 
 	bool frustumCulling = true;
 	int cloudDrawn=0;
-	if(frustumCulling)
+	if(mapRendering_ && frustumCulling)
 	{
 		//Use camera frustum to cull nodes that don't need to be drawn
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -334,8 +335,11 @@ int Scene::Render() {
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			++cloudDrawn;
+			if(mapRendering_ || iter->first < 0)
-			iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			{
+				++cloudDrawn;
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			}
 		}
 	}
 
@@ -420,8 +424,8 @@ void Scene::addCloud(
 	//create
 	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
 	PointCloudDrawable * drawable = new PointCloudDrawable(
-			cloud->is_dense || image.empty()?cloud_shader_program_:0,
+			cloud_shader_program_,
-			cloud->is_dense || image.empty()?0:texture_mesh_shader_program_,
+			texture_mesh_shader_program_,
 			cloud,
 			polygons,
 			image);

app/android/jni/scene.h

@@ -108,6 +108,7 @@ class Scene {
   bool hasCloud(int id) const;
   std::set<int> getAddedClouds() const;
 
+  void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
   void setPointSize(float size) {pointSize_ = size;}
 
@@ -139,6 +140,7 @@ class Scene {
   GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool mapRendering_;
   bool meshRendering_;
   float pointSize_;
 };

app/android/res/menu/optionmenu.xml

@@ -6,6 +6,15 @@
     </group>
     
     <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>   
+          </menu>
+       </item>
        <item android:id="@+id/open" android:title="Open"/>
        <item android:id="@+id/save" android:title="Save"/>
        <item android:id="@+id/export" android:title="Export...">
@@ -17,10 +26,8 @@
           </menu>
        </item>
        <item android:id="@+id/reset" android:title="Reset"/>
-       <item android:id="@+id/about" android:title="About"/>
-    </group>
        
-    <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options..." android:orderInCategory="2">
+       <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
@@ -28,22 +35,28 @@
             <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/max_depth" android:checkable="false" android:title="Max Depth..." />
+            <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>
+	    </item>
-    <item android:id="@+id/menu_mapping_settings" android:title="Mapping Options..." android:orderInCategory="3">
+	    <item android:id="@+id/menu_mapping_settings" android:title="Mapping Options...">
-       <menu >
+	       <menu >
-       <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
+	       <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
-           <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
+	           <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
-           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
+	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
-           <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
+	           <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
-           <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
+	           <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
-           <item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
+	           <item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
-           <item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
+	           <item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
-           <item android:id="@+id/features" android:checkable="false" android:title="Max Features Extracted..." />
+	           <item android:id="@+id/loop_threshold" android:checkable="false" android:title="Loop Closure Threshold..." />
-           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
+	           <item android:id="@+id/features" android:checkable="false" android:title="Max Features Extracted..." />
-       </group>   
+	           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
-       </menu>
+	       </group>   
-   </item>
+	       </menu>
+	   </item>
+       
+       <item android:id="@+id/about" android:title="About"/>
+    </group>
 </menu>

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

@@ -30,6 +30,7 @@ import android.view.MenuInflater;
 import android.view.MotionEvent;
 import android.view.View;
 import android.view.View.OnClickListener;
+import android.view.WindowManager;
 import android.widget.EditText;
 import android.widget.LinearLayout;
 import android.widget.TextView;
@@ -66,6 +67,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private MenuItem mItemPause;
   private MenuItem mItemSave;
   private MenuItem mItemOpen;
+  private MenuItem mItemPostProcessing;
   private MenuItem mItemExport;
   private MenuItem mItemLocalizationMode;
   private MenuItem mItemTrajectoryMode;
@@ -76,10 +78,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private String mWorkingDirectory = "";
   
   private int mMaxDepthIndex = 5;
+  private int mMeshAngleToleranceIndex = 1;
+  private int mMeshTriangleSizeIndex = 0;
   
   private int mParamUpdateRateHzIndex = 1;
   private int mParamTimeThrMsIndex = 1;
-  private int mParamMaxFeaturesIndex = 2;
+  private int mParamMaxFeaturesIndex = 4;
+  private int mParamLoopThrMsIndex = 1;
   
   private LinearLayout mLayoutDebug;
   
@@ -95,6 +100,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     Display display = getWindowManager().getDefaultDisplay();
     display.getSize(mScreenSize);
     
+    getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);
+
     // Setting content view of this activity.
     setContentView(R.layout.activity_rtabmap);
 
@@ -188,6 +195,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	    mItemPause.setChecked(false);
     	    mItemSave.setEnabled(false);
     		mItemExport.setEnabled(false);
+    		mItemPostProcessing.setEnabled(false);
         }
 
         if(RTABMapLib.onResume()!=0)
@@ -274,12 +282,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  mItemPause = menu.findItem(R.id.pause);
 	  mItemSave = menu.findItem(R.id.save);
 	  mItemOpen = menu.findItem(R.id.open);
+	  mItemPostProcessing = menu.findItem(R.id.post_processing);
 	  mItemExport = menu.findItem(R.id.export);
 	  mItemLocalizationMode = menu.findItem(R.id.localization_mode);
 	  mItemTrajectoryMode = menu.findItem(R.id.trajectory_mode);
 	  mItemSave.setEnabled(false);
 	  mItemExport.setEnabled(false);
 	  mItemOpen.setEnabled(false);
+	  mItemPostProcessing.setEnabled(false);
 	  
 	  return true;
   }
@@ -315,9 +325,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  if(loopClosureId > 0)
 	  {
 		  ++mTotalLoopClosures;
-		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
 		  Toast.makeText(this, "Loop closure detected!", Toast.LENGTH_SHORT).show();
 	  }
+	  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
   }
   
   // called from jni
@@ -443,6 +453,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				{
 					mItemPause.setChecked(false);
 					mItemOpen.setEnabled(false);
+					mItemPostProcessing.setEnabled(false);
 					mItemSave.setEnabled(false);
 					mItemExport.setEnabled(false);
 					RTABMapLib.setPausedMapping(false); // resume mapping
@@ -570,62 +581,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  mItemSave.setEnabled(item.isChecked());
 		  mItemExport.setEnabled(item.isChecked());
     	  mItemOpen.setEnabled(item.isChecked());
+    	  mItemPostProcessing.setEnabled(item.isChecked());
     	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
     	  if(item.isChecked())
     	  {
     		  RTABMapLib.setPausedMapping(true);
     		  ((TextView)findViewById(R.id.status)).setText("Paused");
-    		  
-    		  // Post-processing
-    		  new AlertDialog.Builder(getActivity())
-              .setTitle("Post-Processing")
-              .setMessage("Do you want to detect more loop closures and do a bundle adjustement? Otherwise, a simple global graph optimization is done.")
-              .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-                  public void onClick(DialogInterface dialog, int which) {
-	                	dialog.dismiss();
-	                  	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(false);
-						    	runOnUiThread(new Runnable() {
-						    		public void run() {
-										mProgressDialog.dismiss();
-										if(loopDetected > 0)
-										{
-											Toast.makeText(getActivity(), String.format("%d new loop closure(s) added.", loopDetected), Toast.LENGTH_SHORT).show();
-										}
-						    		}
-						    	});
-						    } 
-						});
-						workingThread.start();
-                  }
-              })
-              .setNegativeButton("No", new DialogInterface.OnClickListener() {
-                  public void onClick(DialogInterface dialog, int which) {
-                      dialog.dismiss();
-                      mProgressDialog.setTitle("Post-Processing");
-              	      mProgressDialog.setMessage(String.format("Please wait while optimizing..."));
-              	      mProgressDialog.show();
-              	      
-						Thread workingThread = new Thread(new Runnable() {
-						    public void run() {
-						    	RTABMapLib.postProcessing(true);
-						    	runOnUiThread(new Runnable() {
-						    		public void run() {
-								    	mProgressDialog.dismiss();
-						    		}
-						    	});
-						    } 
-						});
-						workingThread.start();
-                  }
-              })
-              .show();
-    		  
     	  }
     	  else
     	  {
@@ -633,6 +594,84 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
     	  }
       } 
+      else if (itemId == R.id.detect_more_loop_closures)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Please wait while detecting more loop closures..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int loopDetected = RTABMapLib.postProcessing(2);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(loopDetected >= 0)
+								{
+									Toast.makeText(getActivity(), String.format("Detection done! %d new loop closure(s) added.", loopDetected), Toast.LENGTH_SHORT).show();
+								}
+								else if(loopDetected < 0)
+								{
+									Toast.makeText(getActivity(), String.format("Detection failed!"), Toast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
+      else if (itemId == R.id.global_graph_optimization)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Global graph optimization..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int value = RTABMapLib.postProcessing(0);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(value >= 0)
+								{
+									Toast.makeText(getActivity(), String.format("Optimization done!"), Toast.LENGTH_SHORT).show();
+								}
+								else if(value < 0)
+								{
+									Toast.makeText(getActivity(), String.format("Optimization failed!"), Toast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
+      else if (itemId == R.id.sba)
+      {
+			mProgressDialog.setTitle("Post-Processing");
+			mProgressDialog.setMessage(String.format("Bundle adjustement..."));
+			mProgressDialog.show();
+			
+			Thread workingThread = new Thread(new Runnable() {
+				    public void run() {
+				    	final int value = RTABMapLib.postProcessing(1);
+				    	runOnUiThread(new Runnable() {
+				    		public void run() {
+								mProgressDialog.dismiss();
+								if(value >= 0)
+								{
+									Toast.makeText(getActivity(), String.format("Optimization done!"), Toast.LENGTH_SHORT).show();
+								}
+								else if(value < 0)
+								{
+									Toast.makeText(getActivity(), String.format("Optimization failed!"), Toast.LENGTH_SHORT).show();
+								}
+				    		}
+				    	});
+				    } 
+			});
+			workingThread.start();
+      }
       else if(itemId == R.id.debug)
       {
     	  item.setChecked(!item.isChecked());
@@ -703,7 +742,45 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		        if(which >=0 && which < 6)
 		        {
 			        mMaxDepthIndex = which;
-			        RTABMapLib.setMaxCloudDepth(which < 5?Integer.parseInt(values[which]):0);
+			        RTABMapLib.setMaxCloudDepth(which < 5?Float.parseFloat(values[which]):0);
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.mesh_angle_tolerance)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Angle Tolerance (deg)");
+		  final String[] values = {"5", "10", "15", "20", "25", "30"};
+		  builder.setSingleChoiceItems(values, mMeshAngleToleranceIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 6)
+		        {
+		        	mMeshAngleToleranceIndex = which;
+			        RTABMapLib.setMeshAngleTolerance(Float.parseFloat(values[which]));
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.mesh_triangle_size)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Mesh Triangle Size (pixels)");
+		  final String[] values = {"2", "3", "4", "5", "6"};
+		  builder.setSingleChoiceItems(values, mMeshTriangleSizeIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 5)
+		        {
+		        	mMeshTriangleSizeIndex = which;
+			        RTABMapLib.setMeshTriangleSize(Integer.parseInt(values[which]));
 		        }
 		    }
 		  });
@@ -735,7 +812,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  // get double
 		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
-		  builder.setTitle("Time Threshold (ms, 0 means no limit)");
+		  builder.setTitle("Time Threshold (ms)");
 		  final String[] values = {"400", "700", "1400", "No Limit"};
 		  builder.setSingleChoiceItems(values, mParamTimeThrMsIndex, new DialogInterface.OnClickListener() {
 		    @Override
@@ -758,15 +835,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  // get double
 		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
 		  builder.setTitle("Max Features");
-		  final String[] values = {"Disabled", "100", "200", "300", "400", "No Limit"};
+		  final String[] values = {"Disabled", "100", "200", "300", "400", "500", "600", "700", "800", "900", "1000", "No Limit"};
 		  builder.setSingleChoiceItems(values, mParamMaxFeaturesIndex, new DialogInterface.OnClickListener() {
 		    @Override
 		    public void onClick(DialogInterface dialog, int which) {
 		        dialog.dismiss();
-		        if(which >=0 && which < 6)
+		        if(which >=0 && which < 12)
 		        {
 		        	mParamMaxFeaturesIndex = which;
-			        if(RTABMapLib.setMappingParameter("Kp/MaxFeatures", which==0?"-1":which==5?"0":values[which]) != 0)
+			        if(RTABMapLib.setMappingParameter("Kp/MaxFeatures", which==0?"-1":which==11?"0":values[which]) != 0)
 			        {
 			        	Toast.makeText(getActivity(),"Failed to set parameter \"Kp/MaxFeatures\"!", Toast.LENGTH_LONG).show();
 			        }
@@ -775,6 +852,28 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  });
 		  builder.show();
       }
+      else if(itemId == R.id.loop_threshold)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Loop Closure Threshold");
+		  final String[] values = {"Disabled", "0.11", "0.20", "0.30", "0.40", "0.50", "0.60", "0.70", "0.80", "0.90"};
+		  builder.setSingleChoiceItems(values, mParamTimeThrMsIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 10)
+		        {
+		        	mParamLoopThrMsIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/LoopThr", which==0?"1":values[which]) != 0)
+			        {
+			        	Toast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/LoopThr\"!", Toast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
       else if (itemId == R.id.save)
       {
     	  if(mOpenedDatabasePath.isEmpty())
@@ -810,6 +909,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 										//disable gui actions
 										mItemSave.setEnabled(false);
 										mItemOpen.setEnabled(false);
+										mItemPostProcessing.setEnabled(false);
 										mItemExport.setEnabled(false);
 				                    }
 				                })
@@ -832,6 +932,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							  //disable gui actions
 							  mItemSave.setEnabled(false);
 							  mItemOpen.setEnabled(false);
+							  mItemPostProcessing.setEnabled(false);
 							  mItemExport.setEnabled(false);
 						  }
 					  }
@@ -849,6 +950,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		  //disable gui actions
 			  mItemSave.setEnabled(false);
 			  mItemOpen.setEnabled(false);
+			  mItemPostProcessing.setEnabled(false);
 			  mItemExport.setEnabled(false);
     	  }
       }
@@ -880,6 +982,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
       {
     	  final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
+    	  final boolean isOBJ = itemId == R.id.export_obj;
     	  
     	  AlertDialog.Builder builder = new AlertDialog.Builder(this);
 		  builder.setTitle(String.format("File Name (*%s):", extension));
@@ -917,7 +1020,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                		    		public void run() {
 					                		    	if(success)
 							                    	{
-							                    		Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+					                		    		if(isOBJ)
+					                		    		{	
+					                		    			Toast.makeText(getActivity(), String.format("Mesh \"%s\" (with textures \"%s/\" and \"%s\") successfully exported!", path, fileName, fileName + ".mtl"), Toast.LENGTH_LONG).show();
+						                		    	}
+					                		    		else
+					                		    		{
+					                		    			Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+					                		    		}
 							                    	}
 							                    	else
 							                    	{
@@ -953,7 +1063,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	                		    		public void run() {
 			                		    	if(success)
 					                    	{
-					                    		Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+			                		    		if(isOBJ)
+			                		    		{	
+			                		    			Toast.makeText(getActivity(), String.format("Mesh \"%s\" (with textures \"%s/\" and \"%s\") successfully exported!", path, fileName, fileName + ".mtl"), Toast.LENGTH_LONG).show();
+			                		    		}
+			                		    		else
+			                		    		{
+			                		    			Toast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), Toast.LENGTH_LONG).show();
+			                		    		}
 					                    	}
 					                    	else
 					                    	{
@@ -986,7 +1103,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
                 	  if(!mItemTrajectoryMode.isChecked())
                 	  {
 	                	  mProgressDialog.setTitle("Loading");
-	                	  mProgressDialog.setMessage(String.format("Please wait while loading \"%s\"...", files[which]));
+	                	  mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", files[which]));
 	                	  mProgressDialog.show();
                 	  }
 

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

@@ -54,12 +54,14 @@ public class RTABMapLib
     public static native void setAutoExposure(boolean enabled);
     public static native void setFullResolution(boolean enabled);
     public static native void setMaxCloudDepth(float value);
+    public static native void setMeshAngleTolerance(float value);
+    public static native void setMeshTriangleSize(int value);
     public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();
     public static native void save();
     public static native boolean exportMesh(String filePath);
-    public static native int postProcessing(boolean graphOptimizationOnly);
+    public static native int postProcessing(int approach);
     
     public static native String getStatus();
     public static native int getTotalNodes();

corelib/include/rtabmap/core/DBDriver.h

@@ -94,7 +94,7 @@ public:
 	// Mutex-protected methods of abstract versions below
 
 	bool openConnection(const std::string & url, bool overwritten = false);
-	void closeConnection();
+	void closeConnection(bool save = true);
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
@@ -136,7 +136,7 @@ protected:
 
 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
-	virtual void disconnectDatabaseQuery() = 0;
+	virtual void disconnectDatabaseQuery(bool save = true) = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;

corelib/include/rtabmap/core/Memory.h

@@ -91,7 +91,7 @@ public:
 	int cleanup();
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLink(const Link & link);
+	bool addLink(const Link & link, bool addInDatabase = false);
 	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
 	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
 	void removeAllVirtualLinks();

corelib/src/DBDriver.cpp

@@ -61,14 +61,24 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
 {
 }
 
-void DBDriver::closeConnection()
+void DBDriver::closeConnection(bool save)
 {
 	UDEBUG("isRunning=%d", this->isRunning());
 	this->join(true);
 	UDEBUG("");
-	this->emptyTrashes();
+	if(save)
+	{
+		this->emptyTrashes();
+	}
+	else
+	{
+		_trashesMutex.lock();
+		_trashSignatures.clear();
+		_trashVisualWords.clear();
+		_trashesMutex.unlock();
+	}
 	_dbSafeAccessMutex.lock();
-	this->disconnectDatabaseQuery();
+	this->disconnectDatabaseQuery(save);
 	_dbSafeAccessMutex.unlock();
 	UDEBUG("");
 }

corelib/src/DBDriverSqlite3.cpp

@@ -381,7 +381,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 
 	return true;
 }
-void DBDriverSqlite3::disconnectDatabaseQuery()
+void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -398,7 +398,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery()
 			}
 		}
 
-		if(_dbInMemory)
+		if(save && _dbInMemory)
 		{
 			UTimer timer;
 			timer.start();
@@ -1049,8 +1049,6 @@ bool DBDriverSqlite3::getCalibrationQuery(
 		const void * data = 0;
 		int dataSize = 0;
 		Transform localTransform;
-		StereoCameraModel stereoModel;
-		std::vector<CameraModel> models;
 
 		// Process the result if one
 		rc = sqlite3_step(ppStmt);

corelib/src/DBDriverSqlite3.h

@@ -48,8 +48,8 @@ public:
 	void setTempStore(int tempStore);
 
 private:
-	virtual bool connectDatabaseQuery(const std::string & url, bool overwirtten = false);
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
-	virtual void disconnectDatabaseQuery();
+	virtual void disconnectDatabaseQuery(bool save = true);
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;

corelib/src/Memory.cpp

@@ -313,7 +313,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 		if(_dbDriver)
 		{
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
-			_dbDriver->closeConnection();
+			_dbDriver->closeConnection(false);
 			delete _dbDriver;
 			_dbDriver = 0;
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
@@ -2327,7 +2327,7 @@ Transform Memory::computeIcpTransformMulti(
 	return t;
 }
 
-bool Memory::addLink(const Link & link)
+bool Memory::addLink(const Link & link, bool addInDatabase)
 {
 	UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
 
@@ -2378,9 +2378,8 @@ bool Memory::addLink(const Link & link)
 				}
 			}
 		}
-		return true;
 	}
-	else
+	else if(!addInDatabase)
 	{
 		if(!fromS)
 		{
@@ -2390,8 +2389,27 @@ bool Memory::addLink(const Link & link)
 		{
 			UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to());
 		}
+		return false;
 	}
-	return false;
+	else if(fromS)
+	{
+		UDEBUG("Add link between %d and %d (db)", link.from(), link.to());
+		fromS->addLink(link);
+		_dbDriver->addLink(link.inverse());
+	}
+	else if(toS)
+	{
+		UDEBUG("Add link between %d (db) and %d", link.from(), link.to());
+		_dbDriver->addLink(link);
+		toS->addLink(link.inverse());
+	}
+	else
+	{
+		UDEBUG("Add link between %d (db) and %d (db)", link.from(), link.to());
+		_dbDriver->addLink(link);
+		_dbDriver->addLink(link.inverse());
+	}
+	return true;
 }
 
 void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)

corelib/src/OptimizerG2O.cpp

@@ -612,6 +612,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				else
 				{
 					UERROR("Missing calibration for node %d", iter->first);
+					return optimizedPoses;
 				}
 			}
 			else
@@ -699,6 +700,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				{
 					delete e;
 					UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
+					return optimizedPoses;
 				}
 			}
 		}

corelib/src/Rtabmap.cpp

@@ -3277,12 +3277,12 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 	if(_graphOptimizer->iterations() <= 0)
 	{
 		UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
-		return 0;
+		return -1;
 	}
 	if(!_rgbdSlamMode)
 	{
 		UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
-		return 0;
+		return -1;
 	}
 
 	std::list<Link> loopClosuresAdded;
@@ -3317,21 +3317,11 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 				to = iter->first;
 			}
 
-			bool alreadyChecked = false;
+			if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
-			for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
-				!alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
-				++jter)
-			{
-				if(to == jter->second)
-				{
-					alreadyChecked = true;
-				}
-			}
-
-			if(!alreadyChecked)
 			{
 				// only add new links and one per cluster per iteration
 				if(addedLinks.find(from) == addedLinks.end() &&
+				   addedLinks.find(to) == addedLinks.end() &&
 				   rtabmap::graph::findLink(links, from, to) == links.end())
 				{
 					checkedLoopClosures.insert(std::make_pair(from, to));
@@ -3371,7 +3361,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 			{
 				UERROR("Optimization failed! Rejecting all loop closures...");
 				loopClosuresAdded.clear();
-				break;
+				return -1;
 			}
 			UINFO("Optimizing graph with new links... done!");
 		}
@@ -3382,7 +3372,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 	{
 		for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
 		{
-			_memory->addLink(*iter);
+			_memory->addLink(*iter, true);
 		}
 	}
 	return (int)loopClosuresAdded.size();