Tango #57: Global optimization / Post-processing on pause

app/android/jni/CameraTango.cpp

@@ -379,7 +379,6 @@ void CameraTango::poseReceived(const Transform & pose)
 
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	LOGE("Tango event: %s:%s", key, value);
 	this->post(new CameraTangoEvent(type, key, value));
 }
 

app/android/jni/RTABMapApp.cpp

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/ParamEvent.h>
 #include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Optimizer.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/io/obj_io.h>
@@ -68,13 +69,14 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
 	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(), uNumber2Str(graphOptimization_?rtabmap::Parameters::defaultOptimizerIterations():0)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // PnP
 
 	return parameters;
 }
@@ -82,6 +84,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 RTABMapApp::RTABMapApp() :
 		camera_(0),
 		rtabmapThread_(0),
+		rtabmap_(0),
 		logHandler_(0),
 		mapCloudShown_(true),
 		odomCloudShown_(true),
@@ -138,6 +141,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 		rtabmapThread_->close(false);
 	    delete rtabmapThread_;
 	    rtabmapThread_ = 0;
+	    rtabmap_ = 0;
 	}
 
 	if(logHandler_ == 0)
@@ -168,26 +172,42 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 		rtabmapThread_->close(false);
 		delete rtabmapThread_;
 		rtabmapThread_ = 0;
+		rtabmap_ = 0;
 	}
 
 	//Rtabmap
-	rtabmap::Rtabmap * rtabmap = new rtabmap::Rtabmap();
+	rtabmap_ = new rtabmap::Rtabmap();
 	rtabmap::ParametersMap parameters = getRtabmapParameters();
 
-	rtabmap->init(parameters, databasePath);
+	rtabmap_->init(parameters, databasePath);
-	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap);
+	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
 
 	// Generate all meshes
 	std::map<int, rtabmap::Signature> signatures;
 	std::map<int, rtabmap::Transform> poses;
 	std::multimap<int, rtabmap::Link> links;
-	rtabmap->get3DMap(
+	rtabmap_->get3DMap(
 			signatures,
 			poses,
 			links,
 			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);
@@ -793,6 +813,48 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	return success;
 }
 
+int RTABMapApp::postProcessing(bool graphOptimizationOnly)
+{
+	int detectedLoopClosures = 0;
+	if(rtabmap_)
+	{
+		std::map<int, rtabmap::Transform> poses;
+		std::multimap<int, rtabmap::Link> links;
+		if(graphOptimizationOnly)
+		{
+			rtabmap_->getGraph(poses, links, true, true);
+		}
+		else
+		{
+			detectedLoopClosures = rtabmap_->detectMoreLoopClosures();
+
+			std::map<int, rtabmap::Signature> signatures;
+			rtabmap_->getGraph(poses, links, false, true, &signatures);
+
+			if(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())
+		{
+			boost::mutex::scoped_lock  lock(rtabmapMutex_);
+			rtabmap::Statistics stats;
+			stats.setPoses(poses);
+			stats.setConstraints(links);
+			rtabmapEvents_.push_back(stats);
+
+			rtabmap_->setOptimizedPoses(poses);
+		}
+	}
+	return detectedLoopClosures;
+}
+
 void RTABMapApp::handleEvent(UEvent * event)
 {
 	if(camera_ && camera_->isRunning())

app/android/jni/RTABMapApp.h

@@ -128,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
   void resetMapping();
   void save();
   bool exportMesh(const std::string & filePath);
+  int postProcessing(bool graphOptimizationOnly);
 
  protected:
   virtual void handleEvent(UEvent * event);
@@ -138,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
  private:
   rtabmap::CameraTango * camera_;
   rtabmap::RtabmapThread * rtabmapThread_;
+  rtabmap::Rtabmap * rtabmap_;
   LogHandler * logHandler_;
 
   bool mapCloudShown_;

app/android/jni/jni_interface.cpp

@@ -212,6 +212,15 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 	return app.exportMesh(filePathC);
 }
 
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
+		JNIEnv* env, jobject, bool graphOptimizationOnly)
+{
+	return app.postProcessing(graphOptimizationOnly);
+}
+
+
+
 #ifdef __cplusplus
 }
 #endif

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

@@ -509,13 +509,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final String key,
 		  final String value)
   {
-	  Log.i(TAG, String.format("tangoEventCallback()"));
+	  if(mItemPause != null && !mItemPause.isChecked())
-
+	  {
-	  runOnUiThread(new Runnable() {
+		  runOnUiThread(new Runnable() {
-		    public void run() {
+			    public void run() {
-		    	tangoEventUI(type, key, value);
+			    	tangoEventUI(type, key, value);
-		    } 
+			    } 
-		});
+			});
+	  }
   }
   
   private boolean CheckTangoCoreVersion(int minVersion) {
@@ -574,6 +575,57 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  {
     		  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
     	  {

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

@@ -59,6 +59,7 @@ public class RTABMapLib
     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 String getStatus();
     public static native int getTotalNodes();

corelib/include/rtabmap/core/DBDriver.h

@@ -119,6 +119,7 @@ public:
 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures) const;
 	void getNodeData(int signatureId, SensorData & data) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
@@ -169,6 +170,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;

corelib/include/rtabmap/core/Memory.h

@@ -147,11 +147,14 @@ public:
 			Transform & groundTruth,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
 	void getNodeWords(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
 			std::multimap<int, cv::Point3f> & words3,
 			std::multimap<int, cv::Mat> & wordsDescriptors);
+	void getNodeCalibration(int nodeId,
+			std::vector<CameraModel> & models,
+			StereoCameraModel & stereoModel);
 	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
@@ -182,6 +185,7 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
+	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(

corelib/include/rtabmap/core/Rtabmap.h

@@ -115,6 +115,7 @@ public:
 	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);
+	void setOptimizedPoses(const std::map<int, Transform> & poses);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
@@ -125,6 +126,7 @@ public:
 			bool optimized,
 			bool global,
 			std::map<int, Signature> * signatures = 0);
+	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1);
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success

corelib/src/DBDriver.cpp

@@ -546,6 +546,31 @@ void DBDriver::getNodeData(
 	}
 }
 
+bool DBDriver::getCalibration(
+		int signatureId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel) const
+{
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
+		stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getCalibrationQuery(signatureId, models, stereoModel);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
+}
+
 bool DBDriver::getNodeInfo(
 		int signatureId,
 		Transform & pose,

corelib/src/DBDriverSqlite3.cpp

@@ -1009,6 +1009,158 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 	}
 }
 
+bool DBDriverSqlite3::getCalibrationQuery(
+		int signatureId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel) const
+{
+	bool found = false;
+	if(_ppDb && signatureId)
+	{
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
+		{
+			query << "SELECT calibration "
+				  << "FROM Data "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.7.0") >= 0)
+		{
+			query << "SELECT local_transform, fx, fy, cx, cy "
+				  << "FROM Depth "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else
+		{
+			query << "SELECT local_transform, constant "
+				  << "FROM Depth "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+		Transform localTransform;
+		StereoCameraModel stereoModel;
+		std::vector<CameraModel> models;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		if(rc == SQLITE_ROW)
+		{
+			found = true;
+			int index = 0;
+
+			// calibration
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
+				// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+				if(dataSize > 0 && data)
+				{
+					float * dataFloat = (float*)data;
+					if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						int max = cameraCount*(4+localTransform.size());
+						for(int i=0; i<max; i+=4+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									(double)dataFloat[i],
+									(double)dataFloat[i+1],
+									(double)dataFloat[i+2],
+									(double)dataFloat[i+3],
+									localTransform));
+						}
+					}
+					else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+					{
+						UDEBUG("Loading calibration of a stereo camera");
+						memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+						stereoModel = StereoCameraModel(
+								dataFloat[0],  // fx
+								dataFloat[1],  // fy
+								dataFloat[2],  // cx
+								dataFloat[3],  // cy
+								dataFloat[4], // baseline
+								localTransform);
+					}
+					else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						int max = cameraCount*(6+localTransform.size());
+						for(int i=0; i<max; i+=6+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									(double)dataFloat[i],
+									(double)dataFloat[i+1],
+									(double)dataFloat[i+2],
+									(double)dataFloat[i+3],
+									localTransform));
+							models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+							UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+									dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+									localTransform.prettyPrint().c_str());
+						}
+					}
+					else
+					{
+						UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+					}
+				}
+
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
+			{
+				double fx = sqlite3_column_double(ppStmt, index++);
+				double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+				double cx = sqlite3_column_double(ppStmt, index++);
+				double cy = sqlite3_column_double(ppStmt, index++);
+				if(fyOrBaseline < 1.0)
+				{
+					//it is a baseline
+					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+				}
+				else
+				{
+					models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+				}
+			}
+			else
+			{
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				float fx = 1.0f/depthConstant;
+				float fy = 1.0f/depthConstant;
+				float cx = 0.0f;
+				float cy = 0.0f;
+				models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	return found;
+}
+
 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		Transform & pose,
 		int & mapId,

corelib/src/DBDriverSqlite3.h

@@ -83,6 +83,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;

corelib/src/Memory.cpp

@@ -2066,113 +2066,7 @@ Transform Memory::computeTransform(
 
 	if(fromS && toS)
 	{
-		// make sure we have all data needed
+		return computeTransform(*fromS, *toS, guess, info);
-		// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
-		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS->sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty()))
-		{
-			getNodeData(fromS->id());
-		}
-		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) ||
-		   (_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) ||
-		   (_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty()))
-		{
-			getNodeData(toS->id());
-		}
-		// uncompress only what we need
-		cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
-		fromS->sensorData().uncompressData(
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
-				_registrationPipeline->isScanRequired()?&laserBuf:0,
-				_registrationPipeline->isUserDataRequired()?&userBuf:0);
-		toS->sensorData().uncompressData(
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-				(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
-				_registrationPipeline->isScanRequired()?&laserBuf:0,
-				_registrationPipeline->isUserDataRequired()?&userBuf:0);
-
-
-		// compute transform fromId -> toId
-		std::vector<int> inliersV;
-		if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
-		{
-			Signature tmpFrom = *fromS;
-			Signature tmpTo = *toS;
-
-			// make a guess fast with known correspondences (if there are)
-			RegistrationVis regVis(parameters_);
-			if(tmpFrom.getWords().size() &&
-				tmpTo.getWords().size() &&
-				tmpFrom.getWords3().size() &&
-				tmpTo.getWords3().size())
-			{
-				UDEBUG("");
-				// Remove descriptors, this will avoid recomputation of the correspondences in regVis
-				tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-				// set back descriptors
-				tmpFrom.setWordsDescriptors(fromS->getWordsDescriptors());
-				tmpTo.setWordsDescriptors(toS->getWordsDescriptors());
-			}
-
-			if(_reextractLoopClosureFeatures)
-			{
-				UDEBUG("");
-				tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
-				tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
-				tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
-				tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
-				tmpTo.setWords3(std::multimap<int, cv::Point3f>());
-				tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
-			}
-
-			if(guess.isNull())
-			{
-				if(!_registrationPipeline->isImageRequired())
-				{
-					UDEBUG("");
-					// no visual in the pipeline, make visual registration for guess
-					guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-				}
-				else
-				{
-					UDEBUG("");
-					guess.setIdentity();
-				}
-			}
-
-			if(!guess.isNull())
-			{
-				UDEBUG("");
-				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
-
-				if(!transform.isNull())
-				{
-					UDEBUG("");
-					// verify if it is a 180 degree transform, well verify > 90
-					float x,y,z, roll,pitch,yaw;
-					transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					if(fabs(roll) > CV_PI/2 ||
-					   fabs(pitch) > CV_PI/2 ||
-					   fabs(yaw) > CV_PI/2)
-					{
-						transform.setNull();
-						std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-								roll, pitch, yaw);
-						UINFO(msg.c_str());
-						if(info)
-						{
-							info->rejectedMsg = msg;
-						}
-					}
-				}
-			}
-		}
 	}
 	else
 	{
@@ -2186,6 +2080,125 @@ Transform Memory::computeTransform(
 	return transform;
 }
 
+// compute transform fromId -> toId
+Transform Memory::computeTransform(
+		Signature & fromS,
+		Signature & toS,
+		Transform guess,
+		RegistrationInfo * info) const
+{
+	Transform transform;
+
+	// make sure we have all data needed
+	// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
+	{
+		fromS.sensorData() = getNodeData(fromS.id());
+	}
+	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) ||
+	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
+	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
+	{
+		toS.sensorData() = getNodeData(toS.id());
+	}
+	// uncompress only what we need
+	cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
+	fromS.sensorData().uncompressData(
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			_registrationPipeline->isScanRequired()?&laserBuf:0,
+			_registrationPipeline->isUserDataRequired()?&userBuf:0);
+	toS.sensorData().uncompressData(
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
+			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			_registrationPipeline->isScanRequired()?&laserBuf:0,
+			_registrationPipeline->isUserDataRequired()?&userBuf:0);
+
+
+	// compute transform fromId -> toId
+	std::vector<int> inliersV;
+	if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
+	{
+		Signature tmpFrom = fromS;
+		Signature tmpTo = toS;
+
+		// make a guess fast with known correspondences (if there are)
+		RegistrationVis regVis(parameters_);
+		if(tmpFrom.getWords().size() &&
+			tmpTo.getWords().size() &&
+			tmpFrom.getWords3().size() &&
+			tmpTo.getWords3().size())
+		{
+			UDEBUG("");
+			// Remove descriptors, this will avoid recomputation of the correspondences in regVis
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			// set back descriptors
+			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
+			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
+		}
+
+		if(_reextractLoopClosureFeatures)
+		{
+			UDEBUG("");
+			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+		}
+
+		if(guess.isNull())
+		{
+			if(!_registrationPipeline->isImageRequired())
+			{
+				UDEBUG("");
+				// no visual in the pipeline, make visual registration for guess
+				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			}
+			else
+			{
+				UDEBUG("");
+				guess.setIdentity();
+			}
+		}
+
+		if(!guess.isNull())
+		{
+			UDEBUG("");
+			transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+
+			if(!transform.isNull())
+			{
+				UDEBUG("");
+				// verify if it is a 180 degree transform, well verify > 90
+				float x,y,z, roll,pitch,yaw;
+				transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				if(fabs(roll) > CV_PI/2 ||
+				   fabs(pitch) > CV_PI/2 ||
+				   fabs(yaw) > CV_PI/2)
+				{
+					transform.setNull();
+					std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+							roll, pitch, yaw);
+					UINFO(msg.c_str());
+					if(info)
+					{
+						info->rejectedMsg = msg;
+					}
+				}
+			}
+		}
+	}
+	return transform;
+}
+
 // compute transform fromId -> toId
 Transform Memory::computeIcpTransform(
 		int fromId,
@@ -2868,45 +2881,24 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
 	return image;
 }
 
-SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoadedDataInMemory)
+SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
 {
 	UDEBUG("nodeId=%d", nodeId);
 	SensorData r;
 	Signature * s = this->_getSignature(nodeId);
 	if(s && !s->sensorData().imageCompressed().empty())
 	{
-		if(keepLoadedDataInMemory && uncompressedData)
-		{
-			s->sensorData().uncompressData();
-		}
 		r = s->sensorData();
-		if(!keepLoadedDataInMemory && uncompressedData)
-		{
-			r.uncompressData();
-		}
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		if(s && keepLoadedDataInMemory)
+		_dbDriver->getNodeData(nodeId, r);
-		{
+	}
-			std::list<Signature*> signatures;
+
-			signatures.push_back(s);
+	if(uncompressedData)
-			_dbDriver->loadNodeData(signatures);
+	{
-			if(uncompressedData)
+		r.uncompressData();
-			{
-				s->sensorData().uncompressData();
-			}
-			r = s->sensorData();
-		}
-		else
-		{
-			_dbDriver->getNodeData(nodeId, r);
-			if(uncompressedData)
-			{
-				r.uncompressData();
-			}
-		}
 	}
 
 	return r;
@@ -2951,6 +2943,24 @@ void Memory::getNodeWords(int nodeId,
 	}
 }
 
+void Memory::getNodeCalibration(int nodeId,
+		std::vector<CameraModel> & models,
+		StereoCameraModel & stereoModel)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	Signature * s = this->_getSignature(nodeId);
+	if(s)
+	{
+		models = s->sensorData().cameraModels();
+		stereoModel = s->sensorData().stereoCameraModel();
+	}
+	else if(_dbDriver)
+	{
+		// load from database
+		_dbDriver->getCalibration(nodeId, models, stereoModel);
+	}
+}
+
 SensorData Memory::getSignatureDataConst(int locationId) const
 {
 	UDEBUG("");

corelib/src/OptimizerG2O.cpp

@@ -752,7 +752,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		UASSERT(optimizer.verifyInformationMatrices());
 
-		UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%d)", iterations(), robustKernel?1:0);
+		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
 
 		int it = 0;
 		UTimer timer;

corelib/src/Rtabmap.cpp

@@ -1639,27 +1639,29 @@ bool Rtabmap::process(
 			++iter)
 		{
 			const Signature * s = _memory->getSignature(iter->second);
-			UASSERT(s!=0);
+			if(s!=0)
-			// If there is a change of direction, better to be retrieving
-			// ALL nearest signatures than only newest neighbors
-			const std::map<int, Link> & links = s->getLinks();
-			for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
-				jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
-				++jter)
 			{
-				if(_memory->getSignature(jter->first) == 0)
+				// If there is a change of direction, better to be retrieving
+				// ALL nearest signatures than only newest neighbors
+				const std::map<int, Link> & links = s->getLinks();
+				for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
+					jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++jter)
 				{
-					UINFO("retrieval of node %d on local map", jter->first);
+					if(_memory->getSignature(jter->first) == 0)
-					retrievalLocalIds.push_back(jter->first);
+					{
+						UINFO("retrieval of node %d on local map", jter->first);
+						retrievalLocalIds.push_back(jter->first);
+					}
 				}
-			}
+				if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
-			if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
-			{
-				if(immunizedLocations.insert(s->id()).second)
 				{
-					++immunizedLocally;
+					if(immunizedLocations.insert(s->id()).second)
+					{
+						++immunizedLocally;
+					}
+					UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 				}
-				UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 			}
 		}
 		// well, if the maximum retrieved is not reached, look for neighbors in database
@@ -2685,6 +2687,11 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
 	}
 }
 
+void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
+{
+	_optimizedPoses = poses;
+}
+
 void Rtabmap::dumpData() const
 {
 	UDEBUG("");
@@ -3236,6 +3243,20 @@ void Rtabmap::getGraph(
 							label,
 							odomPose,
 							groundTruth)));
+
+				std::multimap<int, cv::KeyPoint> words;
+				std::multimap<int, cv::Point3f> words3;
+				std::multimap<int, cv::Mat> wordsDescriptors;
+				_memory->getNodeWords(iter->first, words, words3, wordsDescriptors);
+				signatures->at(iter->first).setWords(words);
+				signatures->at(iter->first).setWords3(words3);
+				signatures->at(iter->first).setWordsDescriptors(wordsDescriptors);
+
+				std::vector<CameraModel> models;
+				StereoCameraModel stereoModel;
+				_memory->getNodeCalibration(iter->first, models, stereoModel);
+				signatures->at(iter->first).sensorData().setCameraModels(models);
+				signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
 			}
 		}
 	}
@@ -3249,6 +3270,119 @@ void Rtabmap::getGraph(
 	}
 }
 
+int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations)
+{
+	UASSERT(iterations>0);
+
+	if(_graphOptimizer->iterations() <= 0)
+	{
+		UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
+		return 0;
+	}
+	if(!_rgbdSlamMode)
+	{
+		UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
+		return 0;
+	}
+
+	std::list<Link> loopClosuresAdded;
+	std::multimap<int, int> checkedLoopClosures;
+
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	this->getGraph(poses, links, true, true);
+
+	for(int n=0; n<iterations; ++n)
+	{
+		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
+				n+1, iterations, clusterRadius, clusterAngle);
+
+		std::multimap<int, int> clusters = graph::radiusPosesClustering(
+				poses,
+				clusterRadius,
+				clusterAngle);
+
+		UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
+
+		int i=0;
+		std::set<int> addedLinks;
+		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
+		{
+			int from = iter->first;
+			int to = iter->second;
+			if(iter->first < iter->second)
+			{
+				from = iter->second;
+				to = iter->first;
+			}
+
+			bool alreadyChecked = false;
+			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() &&
+				   rtabmap::graph::findLink(links, from, to) == links.end())
+				{
+					checkedLoopClosures.insert(std::make_pair(from, to));
+
+					RegistrationInfo info;
+					Transform t = _memory->computeTransform(from, to, Transform(), &info);
+
+					if(!t.isNull())
+					{
+						UINFO("Added new loop closure between %d and %d.", from, to);
+						addedLinks.insert(from);
+						addedLinks.insert(to);
+						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
+						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
+						UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
+					}
+				}
+			}
+		}
+		UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
+		if(addedLinks.size() == 0)
+		{
+			break;
+		}
+
+		if(n+1 < iterations)
+		{
+			UINFO("Optimizing graph with new links (%d nodes, %d constraints)...",
+					(int)poses.size(), (int)links.size());
+			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
+			poses = _graphOptimizer->optimize(fromId, poses, links, 0);
+			if(poses.size() == 0)
+			{
+				UERROR("Optimization failed! Rejecting all loop closures...");
+				loopClosuresAdded.clear();
+				break;
+			}
+			UINFO("Optimizing graph with new links... done!");
+		}
+	}
+	UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size());
+
+	if(loopClosuresAdded.size())
+	{
+		for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
+		{
+			_memory->addLink(*iter);
+		}
+	}
+	return (int)loopClosuresAdded.size();
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);