rtabmap: refining neighbor links uses computeIcpTransform() instead of computeTransform() (to ignore Visual registration, which adds more errors most of the time). When database is in RAM, we can close under a different name than the one used when the database was loaded.

Tango: Added Append Mode option (only start a new map after being localized, default true) and the old map is still shown for convenience until localization. Increased minInliers to 25. Increased ICP correspondenceRatio to 0.5. Decreased time to open a database. Saving doesn't close the database anymore and overwrite only if we save with the same name. Removed ICP refining from standard optimization.

app/android/AndroidManifest.xml.in

@@ -14,7 +14,7 @@
     <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->
-    <uses-sdk android:minSdkVersion="17" />
+    <uses-sdk android:minSdkVersion="18" />
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 

app/android/jni/RTABMapApp.cpp

@@ -78,6 +78,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
@@ -87,21 +88,21 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	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::kVisMinInliers(), std::string("25")));
 	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.1")));
 	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::kRegStrategy(), std::string(driftCorrection_?"2":"0")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("6")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("10")));
 	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::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.3")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
 
 	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
@@ -130,6 +131,7 @@ RTABMapApp::RTABMapApp() :
 		trajectoryMode_(false),
 		autoExposure_(true),
 		fullResolution_(false),
+		appendMode_(true),
 		maxCloudDepth_(0.0),
 		meshTrianglePix_(1),
 		meshAngleToleranceDeg_(15.0),
@@ -481,8 +483,15 @@ int RTABMapApp::Render()
 					{
 						// uncompress
 						rtabmap::SensorData data = jter->second.sensorData();
-						cv::Mat tmpA,tmpB;
+						cv::Mat tmpA,depth;
-						data.uncompressData(&tmpA, &tmpB);
+						data.uncompressData(&tmpA, &depth);
+
+						// do post-processing bilateral filtering now
+						UTimer t;
+						depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
+						data.setDepthOrRightRaw(depth);
+						LOGI("Bilateral filtering of %d, time=%fs", jter->first, t.ticks());
+
 						uInsert(bufferedSensorData, std::make_pair(jter->first, data));
 						uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
 
@@ -583,10 +592,19 @@ int RTABMapApp::Render()
 									inserted.first->second.texture = data.imageRaw();
 									inserted.first->second.cameraModel = data.cameraModels()[0];
 
+									if(notifyDataLoaded)
+									{
+										// gain compensation is done, so don't compress the texture yet
+										inserted.first->second.texture = data.imageRaw(); // keep raw
+										// mesh will be added in gain compensation below
+									}
+									else
+									{
 										main_scene_.addMesh(id, inserted.first->second, iter->second);
-
 										inserted.first->second.texture = data.imageCompressed(); // keep compressed
 									}
+
+								}
 								else
 								{
 									LOGE("Not mesh could be created for node %d", id);
@@ -615,6 +633,8 @@ int RTABMapApp::Render()
 			}
 		}
 
+		if(poses.size())
+		{
 			//update cloud visibility
 			std::set<int> addedClouds = main_scene_.getAddedClouds();
 			for(std::set<int>::const_iterator iter=addedClouds.begin();
@@ -630,6 +650,7 @@ int RTABMapApp::Render()
 				}
 			}
 		}
+	}
 	else
 	{
 		main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_);
@@ -667,10 +688,10 @@ int RTABMapApp::Render()
 
 	if(notifyDataLoaded || gainCompensationOnNextRender_>0)
 	{
+		UTimer tGainCompensation;
 		LOGI("Gain compensation...");
 		boost::mutex::scoped_lock  lock(meshesMutex_);
-		if(createdMeshes_.size() > 1)
+
-		{
 		rtabmap::GainCompensator compensator;
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
 		for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
@@ -697,12 +718,19 @@ int RTABMapApp::Render()
 			}
 		}
 
+		if(clouds.size() > 1 && links.size())
+		{
 			compensator.feed(clouds, links);
+			LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
+		}
+
 		for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 		{
 			cv::Mat compressedImage = iter->second.texture;
-				iter->second.texture = rtabmap::uncompressImage(compressedImage);
+			iter->second.texture = compressedImage.rows == 1 ? rtabmap::uncompressImage(compressedImage) : compressedImage;
 			if(!iter->second.cloud->empty())
+			{
+				if(clouds.size() > 1 && links.size())
 				{
 					compensator.apply(iter->first, iter->second.cloud);
 					if(!iter->second.texture.empty())
@@ -710,12 +738,11 @@ int RTABMapApp::Render()
 						compensator.apply(iter->first, iter->second.texture);
 					}
 				}
+			}
 
-				// If we do bilateral filtering, do it right now as the texture is uncompressed
+			if(notifyDataLoaded)
-				if((notifyDataLoaded || bilateralFilteringOnNextRender_) &&
-					iter->second.cloud->size() && smoothMesh(iter->first, iter->second))
 			{
-					main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
+				main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world* poses.at(iter->first));
 			}
 			else
 			{
@@ -724,8 +751,8 @@ int RTABMapApp::Render()
 
 			iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
 		}
-			bilateralFilteringOnNextRender_ = false;
+		LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
-		}
+
 		gainCompensationOnNextRender_ = 0;
 		notifyDataLoaded = true;
 	}
@@ -929,6 +956,17 @@ void RTABMapApp::setFullResolution(bool enabled)
 	}
 }
 
+void RTABMapApp::setAppendMode(bool enabled)
+{
+	if(appendMode_ != enabled)
+	{
+		appendMode_ = enabled;
+		rtabmap::ParametersMap parameters;
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
+		this->post(new rtabmap::ParamEvent(parameters));
+	}
+}
+
 void RTABMapApp::setDataRecorderMode(bool enabled)
 {
 	if(dataRecorderMode_ != enabled)
@@ -979,9 +1017,12 @@ void RTABMapApp::resetMapping()
 	UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
 }
 
-void RTABMapApp::save()
+void RTABMapApp::save(const std::string & databasePath)
 {
-	UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdClose));
+	rtabmapThread_->join(true);
+	rtabmap_->close(true, databasePath);
+	rtabmap_->init(getRtabmapParameters(), databasePath);
+	rtabmapThread_->start();
 }
 
 bool RTABMapApp::exportMesh(const std::string & filePath)
@@ -1079,7 +1120,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				UDirectory::makeDir(textureDirectory);
 				for(unsigned int i=0;i<textures.size(); ++i)
 				{
-					cv::Mat rawImage = rtabmap::uncompressImage(textures[i]);
+					cv::Mat rawImage = textures[i].rows>1?textures[i]:rtabmap::uncompressImage(textures[i]);
 					std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
 					cv::imwrite(texFile, rawImage);
 
@@ -1161,11 +1202,11 @@ int RTABMapApp::postProcessing(int approach)
 		if(approach == -1 || approach == 2)
 		{
 			// detect more loop closures
-			returnedValue = rtabmap_->detectMoreLoopClosures(0.5f, M_PI/6.0f, approach == -1?3:1);
+			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
 		}
 
 		// ICP refining
-		if(returnedValue >=0 && ((approach == -1 && !driftCorrection_) || approach == 3))
+		if(returnedValue >=0 && approach == 3)
 		{
 			rtabmap::ParametersMap parameters;
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string("1"))); // ICP
@@ -1228,7 +1269,7 @@ int RTABMapApp::postProcessing(int approach)
 		{
 			boost::mutex::scoped_lock  lock(renderingMutex_);
 			// filter polygons
-			if(approach == -1 || approach == 4)
+			if(approach == 4)
 			{
 				filterPolygonsOnNextRender_ = true;
 			}
@@ -1330,11 +1371,6 @@ void RTABMapApp::handleEvent(UEvent * event)
 		status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
 		status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
 
-		if(status_.first == rtabmap::RtabmapEventInit::kClosed)
-		{
-			clearSceneOnNextRender_ = true;
-		}
-
 		// Call JAVA callback with init msg
 		bool success = false;
 		if(jvm && RTABMapActivity)

app/android/jni/RTABMapApp.h

@@ -122,6 +122,7 @@ class RTABMapApp : public UEventsHandler {
   void setGridVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
+  void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
   void setMeshAngleTolerance(float value);
@@ -129,7 +130,7 @@ class RTABMapApp : public UEventsHandler {
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
-  void save();
+  void save(const std::string & databasePath);
   bool exportMesh(const std::string & filePath);
   int postProcessing(int approach);
 
@@ -154,6 +155,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool autoExposure_;
   bool fullResolution_;
+  bool appendMode_;
   float maxCloudDepth_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;

app/android/jni/jni_interface.cpp

@@ -192,6 +192,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
 	return app.setFullResolution(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setAppendMode(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -234,9 +240,11 @@ Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_save(
-		JNIEnv* env, jobject)
+		JNIEnv* env, jobject, jstring databasePath)
 {
-	return app.save();
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	return app.save(databasePathC);
 }
 
 JNIEXPORT bool JNICALL

app/android/res/menu/optionmenu.xml

@@ -65,6 +65,7 @@
 	           <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/graph_optimization" android:checked="true" android:title="Optimized Graph" />
+	           <item android:id="@+id/append" android:checked="true" android:title="Append Mode" />
 	           <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="HD Mode" />

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

@@ -87,7 +87,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   
   private String mOpenedDatabasePath = "";
   private String mTempDatabasePath = "";
-  private String mNewDatabasePath = "";
   private String mWorkingDirectory = "";
   
   private int mMaxDepthIndex = 5;
@@ -179,7 +178,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     
     mOpenedDatabasePath = "";
     mTempDatabasePath = "";
-    mNewDatabasePath = "";
     mWorkingDirectory = "";
     mTotalLoopClosures = 0;
     
@@ -436,108 +434,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  
 	  ((TextView)findViewById(R.id.status)).setText(
 			  status == 1 && msg.isEmpty()?mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":"Mapping":msg);
-	  
-	  /*0=kInitializing,
-		1=kInitialized,
-		2=kClosing,
-		3=kClosed,
-		4=kInfo,
-		5=kError*/
-	  
-		if(status == 3)
-		{
-			msg = "";
-			if(!mNewDatabasePath.isEmpty())
-			{
-				boolean removed = true;
-				File outputFile = new File(mNewDatabasePath);
-				if(outputFile.exists())
-				{
-					removed = outputFile.delete();
-				}
-				if(removed)
-				{
-					File tempFile = new File(mTempDatabasePath);
-					if(tempFile.renameTo(outputFile))
-					{
-						msg = String.format("Database saved to \"%s\". Tip: You can open a saved database with \"Open\".", mNewDatabasePath);
-						
-						Intent intent = new Intent(this, RTABMapActivity.class);
-						// use System.currentTimeMillis() to have a unique ID for the pending intent
-						PendingIntent pIntent = PendingIntent.getActivity(this, (int) System.currentTimeMillis(), intent, 0);
-
-						// build notification
-						// the addAction re-use the same intent to keep the example short
-						Notification n  = new Notification.Builder(this)
-						        .setContentTitle(getString(R.string.app_name))
-						        .setContentText(mNewDatabasePath + " saved!")
-						        .setSmallIcon(R.drawable.ic_launcher)
-						        .setContentIntent(pIntent)
-						        .setAutoCancel(true).build();
-						    
-						  
-						NotificationManager notificationManager = 
-						  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
-
-						notificationManager.notify(0, n); 
-					}
-					else
-					{
-						msg = String.format("Failed to rename temporary database from \"%s\" to \"%s\".",
-								mTempDatabasePath, mNewDatabasePath);
-					}
-				}
-				else
-				{
-					msg = String.format("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
-							mNewDatabasePath, mTempDatabasePath);
-				}
-			}
-			else if(!mOpenedDatabasePath.isEmpty())
-			{
-				msg = String.format("Database \"%s\" updated.", mOpenedDatabasePath);
-			}
-
-			if(!msg.isEmpty())
-			{
-				mToast.makeText(this, msg, mToast.LENGTH_LONG).show();
-			}
-			
-			mOpenedDatabasePath = "";
-			mNewDatabasePath = "";
-			
-			//restart a new scan by default
-			RTABMapLib.openDatabase(mTempDatabasePath);
-			
-			((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
-			((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
-			mTotalLoopClosures = 0;
-			((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
-			
-			if(mItemSave!=null)
-			{
-				if(mItemPause.isChecked())
-				{
-					mItemPause.setChecked(false);
-					mItemOpen.setEnabled(false);
-					mItemPostProcessing.setEnabled(false);
-					mItemSave.setEnabled(false);
-					mItemExport.setEnabled(false);
-					mItemDataRecorderMode.setEnabled(false);
-					RTABMapLib.setPausedMapping(false); // resume mapping
-				}
-			}
-			mProgressDialog.dismiss();
-		}
   }
   
   //called from jni
@@ -938,6 +834,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setFullResolution(item.isChecked());
       }
+      else if(itemId == R.id.append)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setAppendMode(item.isChecked());
+      }
       else if(itemId == R.id.max_depth)
       {
     	  // get double
@@ -1101,15 +1002,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  builder.show();
       }
       else if (itemId == R.id.save)
-      {
-    	  if(mOpenedDatabasePath.isEmpty())
       {
 		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
 		  builder.setTitle("RTAB-Map Database Name (*.db):");
 		  final EditText input = new EditText(this);
 		  input.setInputType(InputType.TYPE_CLASS_TEXT); 
+		  if(mOpenedDatabasePath.isEmpty())
+		  {
 			  String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
 			  input.setText(timeStamp);
+		  }
+		  else
+		  {
+			  File f = new File(mOpenedDatabasePath);
+			  String name = f.getName();
+			  input.setText(name.substring(0,name.lastIndexOf(".")));
+		  }
 		  input.setSelectAllOnFocus(true);
 		  input.selectAll();
 		  builder.setView(input);
@@ -1129,19 +1037,59 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                .setMessage("Do you want to overwrite the existing file?")
 			                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 			                    public void onClick(DialogInterface dialog, int which) {
-				                    	mNewDatabasePath = mWorkingDirectory + fileName + ".db";
 			                    	
+			                    	final String newDatabasePath = mWorkingDirectory + fileName + ".db";
 			                    	mProgressDialog.setTitle("Saving");
-				                  	    mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
+			                    	if(mOpenedDatabasePath.equals(newDatabasePath))
+			                    	{
+			                    		mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePath));
+			                    	}
+			                    	else
+			                    	{
+			                    		mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
+			                    	}
 			                  	    mProgressDialog.show();
 			                    	
-										RTABMapLib.save(); // send save event
+			                  	  Thread saveThread = new Thread(new Runnable() {
-										//disable gui actions
+			                		    public void run() {
-										mItemSave.setEnabled(false);
+			                		    	RTABMapLib.save(newDatabasePath); // save
-										mItemOpen.setEnabled(false);
+			                		    	runOnUiThread(new Runnable() {
-										mItemPostProcessing.setEnabled(false);
+			                		    		public void run() {
-										mItemExport.setEnabled(false);
+				                		    		if(mOpenedDatabasePath.equals(newDatabasePath))
-										mItemDataRecorderMode.setEnabled(false);
+				        							{
+				        								mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
+				        							}
+				        							else
+				        							{
+				        								mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
+				        							
+				        								Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+				        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+				        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+				        								// build notification
+				        								// the addAction re-use the same intent to keep the example short
+				        								Notification n  = new Notification.Builder(getActivity())
+				        								        .setContentTitle(getString(R.string.app_name))
+				        								        .setContentText(newDatabasePath + " saved!")
+				        								        .setSmallIcon(R.drawable.ic_launcher)
+				        								        .setContentIntent(pIntent)
+				        								        .setAutoCancel(true).build();
+				        								    
+				        								  
+				        								NotificationManager notificationManager = 
+				        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+				        								notificationManager.notify(0, n); 
+				        							}
+					                		    	mProgressDialog.dismiss();
+			                		    		}
+			                		    	});
+			                		    } 
+			                		});
+					        	    saveThread.start();
+									
+									mOpenedDatabasePath = newDatabasePath;
 								}
 			                })
 			                .setNegativeButton("No", new DialogInterface.OnClickListener() {
@@ -1153,40 +1101,64 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					  }
 					  else
 					  {
-							  mNewDatabasePath = mWorkingDirectory + fileName + ".db";
+						    final String newDatabasePath = mWorkingDirectory + fileName + ".db";
-							  
 	                        mProgressDialog.setTitle("Saving");
-				        	  mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
+	                        if(mOpenedDatabasePath.equals(newDatabasePath))
+	                        {
+	                        	mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
+	                        }
+	                        else
+	                        {
+	                   	  		mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
+	                        }
 			        	    mProgressDialog.show();
 			        	    
-							  RTABMapLib.save(); // send save event
+			        	    Thread saveThread = new Thread(new Runnable() {
-							  //disable gui actions
+	                		    public void run() {
-							  mItemSave.setEnabled(false);
+	                		    	RTABMapLib.save(newDatabasePath); // save
-							  mItemOpen.setEnabled(false);
+	                		    	runOnUiThread(new Runnable() {
-							  mItemPostProcessing.setEnabled(false);
+	                		    		public void run() {
-							  mItemExport.setEnabled(false);
+		                		    		if(mOpenedDatabasePath.equals(newDatabasePath))
-							  mItemDataRecorderMode.setEnabled(false);
+		        							{
+		        								mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
+		        							}
+		        							else
+		        							{
+		        								mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
+		        							
+		        								Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+		        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+		        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+		        								// build notification
+		        								// the addAction re-use the same intent to keep the example short
+		        								Notification n  = new Notification.Builder(getActivity())
+		        								        .setContentTitle(getString(R.string.app_name))
+		        								        .setContentText(newDatabasePath + " saved!")
+		        								        .setSmallIcon(R.drawable.ic_launcher)
+		        								        .setContentIntent(pIntent)
+		        								        .setAutoCancel(true).build();
+		        								    
+		        								  
+		        								NotificationManager notificationManager = 
+		        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+		        								notificationManager.notify(0, n); 
+		        							}
+			                		    	mProgressDialog.dismiss();
+	                		    		}
+	                		    	});
+	                		    } 
+	                		});
+			        	    saveThread.start();
+			        	    
+			        	    mOpenedDatabasePath = newDatabasePath;
 					  }
 				  }
 			  }
 		  });
 		  builder.show();
       }
-    	  else
-    	  {
-    		  mProgressDialog.setTitle("Saving");
-        	  mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
-        	  mProgressDialog.show();
-        	  
-    		  RTABMapLib.save(); // send save event
-    		  //disable gui actions
-			  mItemSave.setEnabled(false);
-			  mItemOpen.setEnabled(false);
-			  mItemPostProcessing.setEnabled(false);
-			  mItemExport.setEnabled(false);
-			  mItemDataRecorderMode.setEnabled(false);
-    	  }
-      }
       else if(itemId == R.id.reset)
       {
     	  ((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
@@ -1276,8 +1248,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final EditText input = new EditText(this);
 		  input.setInputType(InputType.TYPE_CLASS_TEXT);        
 		  builder.setView(input);
+		  if(mOpenedDatabasePath.isEmpty())
+		  {
 			  String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
 			  input.setText(timeStamp);
+		  }
+		  else
+		  {
+			  File f = new File(mOpenedDatabasePath);
+			  String name = f.getName();
+			  input.setText(name.substring(0,name.lastIndexOf(".")));
+		  }
 		  input.setSelectAllOnFocus(true);
 		  input.selectAll();
 		  builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@@ -1333,6 +1314,24 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					                		    		{
 					                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
 					                		    		}
+					                		    		Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+				        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+				        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+				        								// build notification
+				        								// the addAction re-use the same intent to keep the example short
+				        								Notification n  = new Notification.Builder(getActivity())
+				        								        .setContentTitle(getString(R.string.app_name))
+				        								        .setContentText(path + " exported!")
+				        								        .setSmallIcon(R.drawable.ic_launcher)
+				        								        .setContentIntent(pIntent)
+				        								        .setAutoCancel(true).build();
+				        								    
+				        								  
+				        								NotificationManager notificationManager = 
+				        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+				        								notificationManager.notify(0, n); 
 							                    	}
 							                    	else
 							                    	{
@@ -1376,6 +1375,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                		    		{
 			                		    			mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
 			                		    		}
+			                		    		
+			                		    		Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+		        								// use System.currentTimeMillis() to have a unique ID for the pending intent
+		        								PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+		        								// build notification
+		        								// the addAction re-use the same intent to keep the example short
+		        								Notification n  = new Notification.Builder(getActivity())
+		        								        .setContentTitle(getString(R.string.app_name))
+		        								        .setContentText(path + " exported!")
+		        								        .setSmallIcon(R.drawable.ic_launcher)
+		        								        .setContentIntent(pIntent)
+		        								        .setAutoCancel(true).build();
+		        								    
+		        								  
+		        								NotificationManager notificationManager = 
+		        								  (NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+		        								notificationManager.notify(0, n);
 					                    	}
 					                    	else
 					                    	{

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

@@ -68,6 +68,7 @@ public class RTABMapLib
     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 setAppendMode(boolean enabled);
     public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);
     public static native void setMeshAngleTolerance(float value);
@@ -75,7 +76,7 @@ public class RTABMapLib
     public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();
-    public static native void save();
+    public static native void save(String outputDatabasePath);
     public static native boolean exportMesh(String filePath);
     public static native int postProcessing(int approach);
     

corelib/include/rtabmap/core/DBDriver.h

@@ -102,7 +102,7 @@ public:
 	// Mutex-protected methods of abstract versions below
 
 	bool openConnection(const std::string & url, bool overwritten = false);
-	void closeConnection(bool save = true);
+	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
@@ -146,7 +146,7 @@ protected:
 
 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
-	virtual void disconnectDatabaseQuery(bool save = true) = 0;
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 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

@@ -80,7 +80,7 @@ public:
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
 			bool postInitClosingEvents = false);
-	void close(bool databaseSaved = true, bool postInitClosingEvents = false);
+	void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId(std::map<int, int> * reducedIds = 0);

corelib/include/rtabmap/core/Rtabmap.h

@@ -67,7 +67,14 @@ public:
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
 
-	void close(bool databaseSaved = true);
+	/**
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
 
 	const std::string & getWorkingDir() const {return _wDir;}
 	bool isRGBDMode() const { return _rgbdSlamMode; }

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -61,7 +61,7 @@ public:
 	enum Cmd {
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
-			kCmdClose,            // params: [bool] database saved (default true)
+			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin

corelib/include/rtabmap/core/RtabmapThread.h

@@ -82,8 +82,14 @@ public:
 	void setDataBufferSize(unsigned int bufferSize);
 	void createIntermediateNodes(bool enabled);
 
-	// this will delete rtabmap object if set
+	/**
-	void close(bool databaseSaved);
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved, const std::string & databasePath = "");
 
 protected:
 	virtual void handleEvent(UEvent * anEvent);

corelib/src/DBDriver.cpp

@@ -61,7 +61,7 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
 {
 }
 
-void DBDriver::closeConnection(bool save)
+void DBDriver::closeConnection(bool save, const std::string & outputUrl)
 {
 	UDEBUG("isRunning=%d", this->isRunning());
 	this->join(true);
@@ -78,7 +78,7 @@ void DBDriver::closeConnection(bool save)
 		_trashesMutex.unlock();
 	}
 	_dbSafeAccessMutex.lock();
-	this->disconnectDatabaseQuery(save);
+	this->disconnectDatabaseQuery(save, outputUrl);
 	_dbSafeAccessMutex.unlock();
 	UDEBUG("");
 }

corelib/src/DBDriverSqlite3.cpp

@@ -390,7 +390,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 
 	return true;
 }
-void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
+void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -411,11 +411,23 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
 		{
 			UTimer timer;
 			timer.start();
-			UINFO("Saving database to %s ...",  this->getUrl().c_str());
+			std::string outputFile = this->getUrl();
-			rc = loadOrSaveDb(_ppDb, this->getUrl(), 1); // Save memory to file
+			if(!outputUrl.empty())
+			{
+				outputFile = outputUrl;
+			}
+			UINFO("Saving database to %s ...",  outputFile.c_str());
+			rc = loadOrSaveDb(_ppDb, outputFile, 1); // Save memory to file
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 			ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
 		}
+		else if(save && !outputUrl.empty() && outputUrl.compare(this->getUrl()) != 0)
+		{
+			UWARN("Output database path (%s) is different than the opened database "
+					"path (%s). Exporting to a different path is only available "
+					"when database is in memory (%s=true). Opened database path is overwritten.",
+					outputUrl.c_str(), this->getUrl().c_str(), Parameters::kDbSqlite3InMemory().c_str());
+		}
 
 		// Then close (delete) the database connection
 		UINFO("Disconnecting database %s...", this->getUrl().c_str());

corelib/src/DBDriverSqlite3.h

@@ -49,7 +49,7 @@ public:
 
 private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
-	virtual void disconnectDatabaseQuery(bool save = true);
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;

corelib/src/Memory.cpp

@@ -304,7 +304,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	return success;
 }
 
-void Memory::close(bool databaseSaved, bool postInitClosingEvents)
+void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
 {
 	UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
 	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
@@ -342,7 +342,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 			_dbDriver->emptyTrashes();
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
-			_dbDriver->closeConnection();
+			_dbDriver->closeConnection(true, ouputDatabasePath);
 			delete _dbDriver;
 			_dbDriver = 0;
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));

corelib/src/Rtabmap.cpp

@@ -320,7 +320,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 	this->init(param, databasePath);
 }
 
-void Rtabmap::close(bool databaseSaved)
+void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 {
 	UINFO("databaseSaved=%d", databaseSaved?1:0);
 	_highestHypothesis = std::make_pair(0,0.0f);
@@ -354,7 +354,7 @@ void Rtabmap::close(bool databaseSaved)
 	}
 	if(_memory)
 	{
-		_memory->close(databaseSaved, true);
+		_memory->close(databaseSaved, true, ouputDatabasePath);
 		delete _memory;
 		_memory = 0;
 	}
@@ -1024,7 +1024,7 @@ bool Rtabmap::process(
 				{
 					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
 					RegistrationInfo info;
-					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
+					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())
 					{
 						UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",

corelib/src/RtabmapThread.cpp

@@ -117,12 +117,12 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
 	_createIntermediateNodes = enabled;
 }
 
-void RtabmapThread::close(bool databaseSaved)
+void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
 {
 	this->join(true);
 	if(_rtabmap)
 	{
-		_rtabmap->close(databaseSaved);
+		_rtabmap->close(databaseSaved, ouputDatabasePath);
 		delete _rtabmap;
 		_rtabmap = 0;
 	}
@@ -241,7 +241,7 @@ void RtabmapThread::mainLoop()
 			UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
 			this->clearBufferedData();
 		}
-		_rtabmap->close(uStr2Bool(parameters.at("saved")));
+		_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
 		break;
 	case kStateDumpingMemory:
 		_rtabmap->dumpData();
@@ -409,6 +409,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 				UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
 				ParametersMap param;
 				param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
+				param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
 				pushNewState(kStateClose, param);
 			}
 			else if(cmd == RtabmapEventCmd::kCmdResetMemory)

corelib/src/util2d.cpp

@@ -1742,15 +1742,15 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 	UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
 			depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
 
-	cv::Mat output = depth.clone();
+	cv::Mat output = cv::Mat::zeros(depth.size(), CV_32FC1);
 
 	float base_max = -std::numeric_limits<float>::max ();
 	float base_min = std::numeric_limits<float>::max ();
 	bool found_finite = false;
-	for (int x = 0; x < output.cols; ++x)
+	for (int x = 0; x < depth.cols; ++x)
-		for (int y = 0; y < output.rows; ++y)
+		for (int y = 0; y < depth.rows; ++y)
 		{
-			float z = depth.type()==CV_32FC1?output.at<float>(y, x):float(output.at<unsigned short>(y, x))/1000.0f;
+			float z = depth.type()==CV_32FC1?depth.at<float>(y, x):float(depth.at<unsigned short>(y, x))/1000.0f;
 			if (z > 0.0f && uIsFinite(z))
 			{
 				if (base_max < z)
@@ -1783,7 +1783,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 		const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
 		for (int y = 0; y < depth.rows; ++y)
 		{
-			float v = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
+			float v = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
 			if((v > 0 && uIsFinite(v)))
 			{
 				float z = v - base_min;
@@ -1832,7 +1832,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 	for (int x = 0; x < depth.cols; ++x)
 	  for (int y = 0; y < depth.rows; ++y)
 	  {
-		  float z = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
+		  float z = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
 		  if(z > 0 && uIsFinite(z))
 		  {
 			  z -= base_min;
@@ -1844,20 +1844,12 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 			  {
 				  v = 0.0f;
 			  }
-			  if(depth.type()==CV_32FC1)
+			  if(depth.type()==CV_16UC1 && v>65.5350f)
 			  {
+				  v = 65.5350f;
+			  }
 			  output.at<float>(y,x) = v;
 		  }
-			  else
-			  {
-				  v*=1000.0f;
-				  if(v>65535.0f)
-				  {
-					  v = 65535.0f;
-				  }
-				  output.at<unsigned short>(y,x) = v;
-			  }
-		  }
 	  }
 
 	UDEBUG("End");