Increased version to 0.6.2

Memory management tested on RGBD SLAM
Fixed some crashes
New parameter RGBD/LocalLoopDetectionMaxDiffID
New parameter LccIcp2/VoxelSize
Updated parameter LccIcp/Type (added "No ICP" type)
Updated parameter kLccBowMinInliers (inliers minimum reduced to 1)
TORO optimization: The graph root is explicitly defined as the last node added (fixed a TORO crash when a root could not be found)
TORO optimization: only one constraint between neighbors and loop closures
TORO optimization: added initial tree guess
Map correction/Map transform: now only used in localization mode (map correction is always identity on mapping mode)
New statistics: local loop space diff, last loop closure parent and child ids
Database: Fixed empty signatures with no poses loaded
ICP: fixed transform multiplication order
Dump memory: added 3D words
DatabaseViewer: added Export option (added ExportDialog object)
DataRecorder: Option recording in RAM or Hard drive
GUI: added a dock widget (MapVisibilityWidget) to change visibility of nodes in the 3D map
GUI: fixed progress bar step count when generating the map
Menu option: generate TORO graph (full/current map, optimized/not optimized)
Menu option: download map (full/current map, optimized/not optimized)
Preferences: added Reset all settings button
Preferences: A QGroupBox can be a boolean parameter

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1066 f169173b-cf89-36c8-b27e-44dbe73f0c83

CMakeLists.txt

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

app/src/main.cpp

@@ -61,14 +61,16 @@ int main(int argc, char* argv[])
 	ULOGGER_INFO("Killing threads...");
 	rtabmap->join(true);
 
-	ULOGGER_INFO("Free memory...");
+	ULOGGER_INFO("Closing RTAB-Map core...");
-
-	delete mainWindow;
 
 	//Since we can't put the Rtabmap object in the MainWindow class,
 	//we pop up a message box indicating that the rtabmap object
 	// is being deleted (saving data to the database)
-	QMessageBox * msg = new QMessageBox(QMessageBox::Information, QObject::tr("RTAB-Map is closing..."), QObject::tr("The detector is saving the working memory to database (located in RTAB-Map's working directory)..."), QMessageBox::NoButton);
+	QMessageBox * msg = new QMessageBox(QMessageBox::Information,
+			QObject::tr("RTAB-Map is closing..."),
+			QObject::tr("The detector is saving the working memory to database (located in RTAB-Map's working directory)..."),
+			QMessageBox::NoButton,
+			mainWindow);
 	msg->setEnabled(false);
 	msg->setIconPixmap(QPixmap(":/images/RTAB-Map.ico"));
 	msg->setWindowIcon(QIcon(":/images/RTAB-Map.ico"));
@@ -78,7 +80,10 @@ int main(int argc, char* argv[])
 	UEventsManager::addHandler(&handler);
 	delThread.startDeletion(1); // make sure that app-exec() is called before the deletion of the object
 	app->exec();
-	delete msg;
+
+	ULOGGER_INFO("Closing RTAB-Map gui...");
+	delete mainWindow;
+
 	delete app;
 
 	ULOGGER_INFO("All done! Closing...");

corelib/include/rtabmap/core/Memory.h

@@ -71,7 +71,7 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId, const Transform & transform);
+	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
 	void updateNeighborLink(int fromId, int toId, const Transform & transform);
 	std::map<int, int> getNeighborsId(int signatureId,
 			unsigned int margin,
@@ -87,7 +87,6 @@ public:
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
 	void getPose(int locationId,
-			int targetMapId,
 			Transform & pose,
 			bool lookInDatabase = false) const;
 	std::map<int, Transform> getNeighborLinks(int signatureId,
@@ -119,12 +118,14 @@ public:
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
 	bool isIDsGenerated() const {return _generateIds;}
+	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
+	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
 
 	void setRoi(const std::string & roi);
 
 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
-	virtual void dumpSignatures(const char * fileNameSign) const;
+	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
 
 	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
@@ -142,14 +143,13 @@ public:
 
 	void getMetricConstraints(
 			const std::vector<int> & ids,
-			int targetMapId,
 			std::map<int, Transform> & poses,
 			std::multimap<int, std::pair<int, Transform> > & links,
 			bool lookInDatabase = false);
 	Transform computeVisualTransform(int oldId, int newId) const;
 	Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess);
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const;
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
@@ -201,7 +201,8 @@ private:
 	int _idCount;
 	int _idMapCount;
 	Signature * _lastSignature;
-	int _lastLoopClosureId;
+	int _lastGlobalLoopClosureParentId;
+	int _lastGlobalLoopClosureChildId;
 	bool _memoryChanged; // False by default, become true when Memory::update() is called.
 	int _signaturesAdded;
 
@@ -220,7 +221,6 @@ private:
 	std::vector<float> _roiRatios; // size 4
 
 	// RGBD-SLAM stuff
-	int _icpType;
 	int _bowMinInliers;
 	float _bowInlierDistance;
 	int _bowIterations;
@@ -236,6 +236,7 @@ private:
 	int _icp2MaxIterations;
 	float _icp2MaxFitness;
 	float _icp2CorrespondenceRatio;
+	float _icp2VoxelSize;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -210,6 +210,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=BOW 1=Binary.");
@@ -235,8 +236,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomICP, MaxFitness,               float, 0.01, 	"");
 
 	// Loop closure constraint
-	RTABMAP_PARAM(LccIcp, Enabled,         bool, false, 	"Enable ICP");
+	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 0=ICP 3D, 1=ICP 2D");
-	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=ICP 3D, 1=ICP 2D");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.01, 	"Maximum distance for visual word correspondences.");
@@ -255,7 +255,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
 	RTABMAP_PARAM(LccIcp2, MaxFitness,      float, 1.0, 			"ICP 2D: Maximum fitness to accept the computed transform.");
 	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
-
+	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
 
 public:
 	virtual ~Parameters();

corelib/include/rtabmap/core/Rtabmap.h

@@ -75,7 +75,6 @@ public:
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
 	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
-	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
@@ -87,6 +86,7 @@ public:
 
 	void triggerNewMap();
 	void generateGraph(const std::string & path, int id=0, int margin=5);
+	void generateTOROGraph(const std::string & path, bool optimized, bool full);
 	void resetMemory(bool dbOverwritten = false);
 	void dumpPrediction() const;
 	void dumpData() const;
@@ -101,15 +101,20 @@ public:
 			std::map<int, float> & depthConstants,
 			std::map<int, Transform> & localTransforms,
 			std::map<int, Transform> & poses,
-			Transform & mapCorrection) const;
+			bool optimized,
+			bool full) const;
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int & nearestId) const;
+	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
 
 private:
-	void optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const;
+	void optimizeCurrentMap(int id,
+			bool lookInDatabase,
+			std::map<int, Transform> & optimizedPoses,
+			std::multimap<int, std::pair<int, Transform> > * constraints = 0) const;
+
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
 
@@ -130,11 +135,13 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	int _globalLoopClosureIcpType;
 	int _scanMatchingSize;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localDetectRadius;
 	float _localDetectMaxNeighbors;
+	int _localDetectMaxDiffID;
 	bool _icpEnabled;
 	std::string _databasePath;
 
@@ -160,8 +167,8 @@ private:
 	std::string _wDir;
 
 	std::map<int, Transform> _optimizedPoses;
-	Transform _mapCorrection;
+	Transform _mapCorrection; // for localization mode
-	Transform _mapTransform;
+	Transform _mapTransform; // for localization mode
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -52,11 +52,14 @@ public:
 			kCmdResetMemory,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
-			kCmdGenerateGraph,
+			kCmdGenerateGraph, // params: path
-			kCmdGenerateLocalGraph,
+			kCmdGenerateLocalGraph, // params: path, id, margin
-			kCmdDeleteMemory,
+			kCmdGenerateTOROGraph, // params: path, optimized
+			kCmdGenerateTOROGraphFull, // params: path, optimized
+			kCmdDeleteMemory, // params: path [optional]
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,
+			kCmdPublish3DMapFull,
 			kCmdTriggerNewMap,
 			kCmdPause};
 public:
@@ -75,6 +78,11 @@ public:
 			_cmd(cmd),
 			_strValue(value),
 			_intValue(0){}
+	RtabmapEventCmd(Cmd cmd, const std::string & strValue, int intValue) :
+			UEvent(0),
+			_cmd(cmd),
+			_strValue(strValue),
+			_intValue(intValue){}
 
 	virtual ~RtabmapEventCmd() {}
 	Cmd getCmd() const {return _cmd;}
@@ -139,16 +147,14 @@ public:
 			const std::map<int, std::vector<unsigned char> > & depths2d,
 			const std::map<int, float> & depthConstants,
 			const std::map<int, Transform> & localTransforms,
-			const std::map<int, Transform> & poses,
+			const std::map<int, Transform> & poses) :
-			const Transform & mapCorrection) :
 		UEvent(0),
 		_images(images),
 		_depths(depths),
 		_depths2d(depths2d),
 		_depthConstants(depthConstants),
 		_localTransforms(localTransforms),
-		_poses(poses),
+		_poses(poses)
-		_mapCorrection(mapCorrection)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -159,7 +165,6 @@ public:
 	const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
 	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
-	const Transform & getMapCorrection() const {return _mapCorrection;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -170,7 +175,6 @@ private:
 	std::map<int, float> _depthConstants;
 	std::map<int, Transform> _localTransforms;
 	std::map<int, Transform> _poses;
-	Transform _mapCorrection;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -53,9 +53,12 @@ public:
 		kStateDumpingPrediction,
 		kStateGeneratingGraph,
 		kStateGeneratingLocalGraph,
+		kStateGeneratingTOROGraph,
+		kStateGeneratingTOROGraphFull,
 		kStateDeletingMemory,
 		kStateCleanDataBuffer,
 		kStatePublishingMap,
+		kStatePublishingMapFull,
 		kStateTriggeringMap
 	};
 
@@ -76,7 +79,7 @@ private:
 	void getImage(Image & image);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
-	void publishMap() const;
+	void publishMap(bool optimized, bool full) const;
 
 private:
 	UMutex _stateMutex;

corelib/include/rtabmap/core/Statistics.h

@@ -50,11 +50,14 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
+	RTABMAP_STATS(Loop, Last_loop_closure_child,);
 
-	RTABMAP_STATS(LocalLoop, Scan_matching_success,);
+	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_closure_id,);
 	RTABMAP_STATS(LocalLoop, Space_neighbors,);
+	RTABMAP_STATS(LocalLoop, Space_diff_id,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
@@ -63,7 +66,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
-	RTABMAP_STATS(Memory, Last_loop_closure,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);

corelib/src/DBDriverSqlite3.cpp

@@ -940,74 +940,110 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
-		std::multimap<int, cv::KeyPoint> visualWords;
-		std::multimap<int, pcl::PointXYZ> visualWords3;
 		unsigned int loaded = 0;
 
-		// Prepare the query... Get the map from signature and visual words
+		// Load nodes information
-		query << "SELECT id, map_id, weight, pose, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
+		query << "SELECT id, map_id, weight, pose "
-				 "FROM Map_Node_Word "
+			  << "FROM Node "
-				 "INNER JOIN Node "
+			  << "WHERE id=?;";
-				 "ON Node.id = node_id "
-				 "WHERE node_id = ? ";
-
-		query << " ORDER BY word_id"; // Needed for fast insertion below
-		query << ";";
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-		int id = 0;
-		int mapId = 0;
-		int weight = 0;
-		const void * data = 0;
-		int dataSize = 0;
-		Transform pose;
-		int visualWordId = 0;
-		cv::KeyPoint kpt;
-		pcl::PointXYZ depth;
-
-
 		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 		{
-			ULOGGER_DEBUG("Loading %d...", *iter);
+			ULOGGER_DEBUG("Loading node %d...", *iter);
 			// bind id
 			rc = sqlite3_bind_int(ppStmt, 1, *iter);
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-			id = 0;
+			int id = 0;
-			mapId = 0;
+			int mapId = 0;
-			weight = 0;
+			int weight = 0;
-			data = 0;
+			Transform pose;
-			dataSize = 0;
+			const void * data = 0;
-			pose = Transform();
+			int dataSize = 0;
-			visualWordId = 0;
+
-			kpt = cv::KeyPoint();
+			// Process the result if one
-			visualWords.clear();
+			rc = sqlite3_step(ppStmt);
-			depth = pcl::PointXYZ(0,0,0);
+			if(rc == SQLITE_ROW)
+			{
+				int index = 0;
+				id = sqlite3_column_int(ppStmt, index++); // Signature Id
+				mapId = sqlite3_column_int(ppStmt, index++); // Map Id
+				weight = sqlite3_column_int(ppStmt, index++); // weight
+
+				data = sqlite3_column_blob(ppStmt, index); // pose
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
+				{
+					memcpy(pose.data(), data, dataSize);
+				}
+				rc = sqlite3_step(ppStmt);
+			}
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+			// create the node
+			if(id)
+			{
+				ULOGGER_DEBUG("Creating %d (map=%d)", *iter, mapId);
+				Signature * s = new Signature(
+						id,
+						mapId,
+						std::multimap<int, cv::KeyPoint>(),
+						std::multimap<int, pcl::PointXYZ>(),
+						pose);
+				s->setWeight(weight);
+				s->setSaved(true);
+				nodes.push_back(s);
+				++loaded;
+			}
+			else
+			{
+				UERROR("Signature %d not found in database!", *iter);
+			}
+
+			//reset
+			rc = sqlite3_reset(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+		// Prepare the query... Get the map from signature and visual words
+		std::stringstream query2;
+		query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
+				 "FROM Map_Node_Word "
+				 "WHERE node_id = ? ";
+
+		query2 << " ORDER BY word_id"; // Needed for fast insertion below
+		query2 << ";";
+
+		rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
+			// bind id
+			rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+			int visualWordId = 0;
+			cv::KeyPoint kpt;
+			std::multimap<int, cv::KeyPoint> visualWords;
+			std::multimap<int, pcl::PointXYZ> visualWords3;
+			pcl::PointXYZ depth(0,0,0);
 
 			// Process the result if one
 			rc = sqlite3_step(ppStmt);
 			while(rc == SQLITE_ROW)
 			{
 				int index = 0;
-				if(id==0)
-				{
-					id = sqlite3_column_int(ppStmt, index++); // Signature Id
-					mapId = sqlite3_column_int(ppStmt, index++); // Map Id
-					weight = sqlite3_column_int(ppStmt, index++); // weight
-
-					data = sqlite3_column_blob(ppStmt, index); // pose
-					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
-					{
-						memcpy(pose.data(), data, dataSize);
-					}
-				}
-				else
-				{
-					index = 4;
-				}
 				visualWordId = sqlite3_column_int(ppStmt, index++);
 				kpt.pt.x = sqlite3_column_double(ppStmt, index++);
 				kpt.pt.y = sqlite3_column_double(ppStmt, index++);
@@ -1023,23 +1059,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-			// create the node
+			if(visualWords.size()==0)
-			if(id==0)
 			{
-				UWARN("Empty signature detected! (id=%d)", *iter);
+				UWARN("Empty signature detected! (id=%d)", (*iter)->id());
+			}
+			else
+			{
+				(*iter)->setWords(visualWords);
+				(*iter)->setWords3(visualWords3);
+				ULOGGER_DEBUG("Add %d keypoints and %d 3d points to node %d", visualWords.size(), visualWords3.size(), (*iter)->id());
 			}
-
-			ULOGGER_DEBUG("Creating %d (map=%d) with %d keypoints", *iter, mapId, visualWords.size());
-			Signature * s = new Signature(
-					*iter,
-					mapId,
-					visualWords,
-					visualWords3,
-					pose);
-			s->setWeight(weight);
-			s->setSaved(true);
-			nodes.push_back(s);
-			++loaded;
 
 			//reset
 			rc = sqlite3_reset(ppStmt);
@@ -1079,7 +1108,6 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 		std::string query;
 		std::list<int> ids;
 
-		// Get the map from signature and visual words
 		query = "SELECT n.id "
 				 "FROM Node AS n "
 				 "WHERE n.time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
@@ -1412,6 +1440,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 {
+	UDEBUG("nodes = %d", nodes.size());
 	if(_ppDb && nodes.size())
 	{
 		UTimer timer;

corelib/src/Memory.cpp

@@ -58,7 +58,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
-	_lastLoopClosureId(0),
+	_lastGlobalLoopClosureParentId(0),
+	_lastGlobalLoopClosureChildId(0),
 	_memoryChanged(false),
 	_signaturesAdded(0),
 
@@ -70,8 +71,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
 	_roiRatios(std::vector<float>(4, 0.0f)),
 
-	_icpType(Parameters::defaultLccIcpType()),
-
 	_bowMinInliers(Parameters::defaultLccBowMinInliers()),
 	_bowInlierDistance(Parameters::defaultLccBowInlierDistance()),
 	_bowIterations(Parameters::defaultLccBowIterations()),
@@ -88,7 +87,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()),
 	_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
 	_icp2MaxFitness(Parameters::defaultLccIcp2MaxFitness()),
-	_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio())
+	_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()),
+	_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize())
 {
 	_vwd = new VWDictionary(parameters);
 	this->parseParameters(parameters);
@@ -314,20 +314,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_dbDriver->parseParameters(parameters);
 	}
 
-	// RGB-D SLAM stuff
-	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
-	{
-		int icpType = std::atoi((*iter).second.c_str());
-		if(icpType >= 0 && icpType <= 1)
-		{
-			_icpType = icpType;
-		}
-		else
-		{
-			UERROR("Icp type must be 0 or 1 (value=%d)", icpType);
-		}
-	}
-
 	Parameters::parse(parameters, Parameters::kLccBowMinInliers(), _bowMinInliers);
 	Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance);
 	Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
@@ -343,8 +329,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations);
 	Parameters::parse(parameters, Parameters::kLccIcp2MaxFitness(), _icp2MaxFitness);
 	Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio);
+	Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize);
 
-	UASSERT_MSG(_bowMinInliers >= 8, uFormat("value=%d", _bowMinInliers).c_str());
+	UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
 	UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
 	UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
 	UASSERT_MSG(_bowMaxDepth >= 0.0f, uFormat("value=%f", _bowMaxDepth).c_str());
@@ -359,6 +346,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str());
 	UASSERT_MSG(_icp2MaxFitness > 0.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
 	UASSERT_MSG(_icp2CorrespondenceRatio >=0.0f && _icp2CorrespondenceRatio <=1.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
+	UASSERT_MSG(_icp2VoxelSize >= 0, uFormat("value=%d", _icp2VoxelSize).c_str());
 
 	// Keypoint stuff
 	if(_vwd)
@@ -511,7 +499,8 @@ bool Memory::update(const Image & image, Statistics * stats)
 
 	UDEBUG("totalTimer = %fs", totalTimer.ticks());
 
-	if(stats) stats->addStatistic(Statistics::kMemoryLast_loop_closure(), _lastLoopClosureId);
+	if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_parent(), _lastGlobalLoopClosureParentId);
+	if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_child(), _lastGlobalLoopClosureChildId);
 
 	return true;
 }
@@ -629,7 +618,7 @@ int Memory::getVWDictionarySize() const
 	return _vwd->getVisualWords().size();
 }
 
-void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool lookInDatabase) const
+void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) const
 {
 	const Signature * s = getSignature(locationId);
 	int mapId = -1;
@@ -637,6 +626,11 @@ void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool loo
 	{
 		pose = s->getPose();
 		mapId = s->mapId();
+
+		if(pose.isNull())
+		{
+			UERROR("Pose of %d is null?!?", locationId);
+		}
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
@@ -722,7 +716,7 @@ std::map<int, Transform> Memory::getNeighborLinks(int signatureId, bool ignoreNe
 // maxCheckedInDatabase = -1 means no limit to check in database (default)
 // maxCheckedInDatabase = 0 means don't check in database
 std::map<int, int> Memory::getNeighborsId(int signatureId,
-		unsigned int margin,
+		unsigned int margin, // 0 means infinite margin
 		int maxCheckedInDatabase, // default -1 (no limit)
 		bool incrementMarginOnLoop, // default false
 		bool ignoreLoopIds, // default false
@@ -745,7 +739,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	std::set<int> nextMargin;
 	nextMargin.insert(signatureId);
 	unsigned int m = 0;
-	while(m < margin && nextMargin.size())
+	while((margin == 0 || m < margin) && nextMargin.size())
 	{
 		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
 		nextMargin.clear();
@@ -1001,7 +995,8 @@ void Memory::clear()
 	}
 	UDEBUG("");
 	_lastSignature = 0;
-	_lastLoopClosureId = 0;
+	_lastGlobalLoopClosureParentId = 0;
+	_lastGlobalLoopClosureChildId = 0;
 	_idCount = kIdStart;
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
@@ -1299,10 +1294,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 		bool recentWmImmunized = false;
 		// look for the position of the lastLoopClosureId in WM
 		int currentRecentWmSize = 0;
-		if(_lastLoopClosureId > 0 && _stMem.find(_lastLoopClosureId) == _stMem.end())
+		if(_lastGlobalLoopClosureParentId > 0 && _stMem.find(_lastGlobalLoopClosureParentId) == _stMem.end())
 		{
 			// If set, it must be in WM
-			std::set<int>::const_iterator iter = _workingMem.find(_lastLoopClosureId);
+			std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureParentId);
 			while(iter != _workingMem.end())
 			{
 				++currentRecentWmSize;
@@ -1314,9 +1309,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 			}
 			else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
 			{
-				UERROR("Last loop closure id not found in WM (%d)", _lastLoopClosureId);
+				UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureParentId);
 			}
-			UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastLoopClosureId);
+			UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureParentId);
 		}
 
 		// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
@@ -1328,8 +1323,8 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 
 		for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
 		{
-			if( (recentWmImmunized && *memIter > _lastLoopClosureId) ||
+			if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureParentId) ||
-				*memIter == _lastLoopClosureId)
+				*memIter == _lastGlobalLoopClosureParentId)
 			{
 				// ignore recent memory
 			}
@@ -1403,7 +1398,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 					removableSignatures.push_back(iter->second);
 					addedSignatures.insert(iter->second->id());
 
-					if(iter->second->id() > _lastLoopClosureId)
+					if(iter->second->id() > _lastGlobalLoopClosureParentId)
 					{
 						++recentWmCount;
 						if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
@@ -1413,7 +1408,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 						}
 					}
 				}
-				else if(iter->second->id() < _lastLoopClosureId)
+				else if(iter->second->id() < _lastGlobalLoopClosureParentId)
 				{
 					UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d",
 							iter->first.weight,
@@ -1575,9 +1570,10 @@ void Memory::rejectLoopClosure(int oldId, int newId)
 				break;
 			}
 		}
-		if(newS->getChildLoopClosureIds().size() == 0 && newId == _lastLoopClosureId)
+		if(newS->getChildLoopClosureIds().size() == 0 && newId == _lastGlobalLoopClosureParentId)
 		{
-			_lastLoopClosureId = 0;
+			_lastGlobalLoopClosureParentId = 0;
+			_lastGlobalLoopClosureChildId = 0;
 		}
 	}
 	else
@@ -1627,6 +1623,7 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 		if((int)inliersOld->size() >= _bowMinInliers)
 		{
 			UDEBUG("Correspondences = %d", (int)inliersOld->size());
+
 			int inliersCount = 0;
 			Transform t = util3d::transformFromXYZCorrespondences(
 					inliersOld,
@@ -1656,7 +1653,7 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 }
 
 // compute transform newId -> oldId
-Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
+Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D)
 {
 	Signature * oldS = this->_getSignature(oldId);
 	Signature * newS = this->_getSignature(newId);
@@ -1665,7 +1662,7 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
 	{
 		std::list<Signature*> depthToLoad;
 		std::set<int> added;
-		if(_icpType == 0)
+		if(icp3D)
 		{
 			//Depth required, if not in RAM, load it from LTM
 			if(oldS->getDepth().empty())
@@ -1679,7 +1676,7 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
 				added.insert(newS->id());
 			}
 		}
-		if(_icpType == 1)
+		else
 		{
 			//Depth required, if not in RAM, load it from LTM
 			if(oldS->getDepth2D().size() == 0 && added.find(oldS->id()) == added.end())
@@ -1699,13 +1696,13 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess)
 	Transform t;
 	if(oldS && newS)
 	{
-		t = computeIcpTransform(*oldS, *newS, guess);
+		t = computeIcpTransform(*oldS, *newS, guess, icp3D);
 	}
 	return t;
 }
 
 // get transform from the new to old node
-Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const
+Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const
 {
 	if(guess.isNull())
 	{
@@ -1722,7 +1719,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 	Transform transform;
 
 	// ICP with guess transform
-	if(_icpType == 0)
+	if(icp3D)
 	{
 		UDEBUG("3D ICP");
 		util3d::CompressionThread ctOld(oldS.getDepth(), true);
@@ -1786,7 +1783,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 
 			if(hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
 			{
-				transform = guess * icpT;
+				transform = icpT * guess;
 				transform = transform.inverse();
 			}
 			else
@@ -1800,7 +1797,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 			UERROR("Depths 3D empty?!?");
 		}
 	}
-	else //_icp2DEnabled
+	else // icp 2D
 	{
 		UDEBUG("2D ICP");
 
@@ -1826,6 +1823,14 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 			// 2D
 			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, guess);
+
+			//voxelize
+			if(_icp2VoxelSize > 0.0f)
+			{
+				oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
+				newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
+			}
+
 			double fitness = 0.0f;
 			bool hasConverged = false;
 			Transform icpT;
@@ -1839,10 +1844,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 					   hasConverged,
 					   fitness);
 
-				//pcl::io::savePCDFile("old.pcd", *oldCloud);
+				//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
-				//pcl::io::savePCDFile("newguess.pcd", *newCloud);
+				//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
 				newCloud = util3d::transformPointCloud(newCloud, icpT);
-				//pcl::io::savePCDFile("newicp.pcd", *newCloud);
+				//pcl::io::savePCDFile("lccnewicp.pcd", *newCloud);
 
 				// verify if there are enough correspondences
 				int correspondences = util3d::getCorrespondencesCount(newCloud, oldCloud, _icp2MaxCorrespondenceDistance);
@@ -1864,7 +1869,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 			   (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
 			   correspondencesRatio >= _icp2CorrespondenceRatio)
 			{
-				transform = guess * icpT;
+				transform = icpT * guess;
 				transform = transform.inverse();
 			}
 			else
@@ -1881,6 +1886,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 	return transform;
 }
 
+// poses of newId and oldId must be in "poses"
 Transform Memory::computeScanMatchingTransform(
 		int newId,
 		int oldId,
@@ -1920,9 +1926,9 @@ Transform Memory::computeScanMatchingTransform(
 	}
 
 	//voxelize
-	if(assembledOldClouds->size())
+	if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f)
 	{
-		assembledOldClouds = util3d::voxelize(assembledOldClouds, 0.01);
+		assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize);
 	}
 
 	// get the new cloud
@@ -1932,9 +1938,9 @@ Transform Memory::computeScanMatchingTransform(
 	newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2D()), poses.at(newId));
 
 	//voxelize
-	if(newCloud->size())
+	if(newCloud->size() && _icp2VoxelSize > 0.0f)
 	{
-		newCloud = util3d::voxelize(newCloud, 0.01);
+		newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
 	}
 
 	//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
@@ -1991,7 +1997,7 @@ Transform Memory::computeScanMatchingTransform(
 }
 
 // Transform from new to old
-bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform)
+bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global)
 {
 	ULOGGER_INFO("old=%d, new=%d transform: %s", oldId, newId, transform.prettyPrint().c_str());
 	Signature * oldS = _getSignature(oldId);
@@ -2011,9 +2017,10 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 		oldS->addLoopClosureId(newS->id(), transform.inverse());
 		newS->addChildLoopClosureId(oldS->id(), transform);
 
-		if(_incrementalMemory)
+		if(_incrementalMemory && global)
 		{
-			_lastLoopClosureId = newS->id();
+			_lastGlobalLoopClosureParentId = newS->id();
+			_lastGlobalLoopClosureChildId = oldS->id();
 
 			// udpate weights only if the memory is incremental
 			newS->setWeight(newS->getWeight() + oldS->getWeight());
@@ -2058,7 +2065,8 @@ void Memory::dumpMemory(std::string directory) const
 {
 	UINFO("Dumping memory to directory \"%s\"", directory.c_str());
 	this->dumpDictionary((directory+"DumpMemoryWordRef.txt").c_str(), (directory+"DumpMemoryWordDesc.txt").c_str());
-	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str());
+	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str(), false);
+	this->dumpSignatures((directory + "DumpMemorySign3.txt").c_str(), true);
 	this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());
 }
 
@@ -2070,7 +2078,7 @@ void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc)
 	}
 }
 
-void Memory::dumpSignatures(const char * fileNameSign) const
+void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
 {
 	FILE* foutSign = 0;
 #ifdef _MSC_VER
@@ -2081,7 +2089,14 @@ void Memory::dumpSignatures(const char * fileNameSign) const
 
 	if(foutSign)
 	{
-		fprintf(foutSign, "SignatureID WordsID...\n");
+		if(words3D)
+		{
+			fprintf(foutSign, "SignatureID WordsID... (Max features depth=%f)\n", _bowMaxDepth);
+		}
+		else
+		{
+			fprintf(foutSign, "SignatureID WordsID...\n");
+		}
 		const std::map<int, Signature *> & signatures = this->getSignatures();
 		for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
 		{
@@ -2089,10 +2104,27 @@ void Memory::dumpSignatures(const char * fileNameSign) const
 			const Signature * ss = dynamic_cast<const Signature *>(iter->second);
 			if(ss)
 			{
-				const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
+				if(words3D)
-				for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 				{
-					fprintf(foutSign, "%d ", (*jter).first);
+					const std::multimap<int, pcl::PointXYZ> & ref = ss->getWords3();
+					for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					{
+						//show only valid point according to current parameters
+						if(pcl::isFinite(jter->second) &&
+						   (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0) &&
+						   (_bowMaxDepth <= 0 || jter->second.x <= _bowMaxDepth))
+						{
+							fprintf(foutSign, "%d ", (*jter).first);
+						}
+					}
+				}
+				else
+				{
+					const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
+					for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					{
+						fprintf(foutSign, "%d ", (*jter).first);
+					}
 				}
 			}
 			fprintf(foutSign, "\n");
@@ -2208,9 +2240,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 
 			oldS->addLoopClosureId(newS->id()); // to keep track of the merged location
 
-			if(_lastLoopClosureId == oldS->id())
+			if(_lastGlobalLoopClosureParentId == oldS->id())
 			{
-				_lastLoopClosureId = newS->id();
+				_lastGlobalLoopClosureParentId = newS->id();
 			}
 		}
 		else
@@ -2406,7 +2438,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 						 {
 							 weightNeighbor = _signatures.find(iter->first)->second->getWeight();
 						 }
-						 UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+						 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
 								 id,
 								 weight,
@@ -2427,7 +2459,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 					 {
 						 weightNeighbor = _signatures.find(iter->first)->second->getWeight();
 					 }
-					 UDEBUG("Add loop link from %d to %d", id, iter->first);
+					 //UDEBUG("Add loop link from %d to %d", id, iter->first);
 					 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
 							 id,
 							 weight,
@@ -2446,7 +2478,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 					 {
 						 weightNeighbor = _signatures.find(iter->first)->second->getWeight();
 					 }
-					 UDEBUG("Add child link from %d to %d", id, iter->first);
+					 //UDEBUG("Add child link from %d to %d", id, iter->first);
 					 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
 							 id,
 							 weight,
@@ -2481,7 +2513,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 							 {
 								 _dbDriver->getWeight(iter->first, weightNeighbor);
 							 }
-							 UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+							 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
 							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\";\n",
 									 id,
 									 weight,
@@ -2502,7 +2534,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 						 {
 							 weightNeighbor = _signatures.find(iter->first)->second->getWeight();
 						 }
-						 UDEBUG("Add loop link from %d to %d", id, iter->first);
+						 //UDEBUG("Add loop link from %d to %d", id, iter->first);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
 								 id,
 								 weight,
@@ -2524,7 +2556,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 						 {
 							 weightNeighbor = _signatures.find(iter->first)->second->getWeight();
 						 }
-						 UDEBUG("Add child link from %d to %d", id, iter->first);
+						 //UDEBUG("Add child link from %d to %d", id, iter->first);
 						 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
 								 id,
 								 weight,
@@ -3165,16 +3197,14 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
 
 void Memory::getMetricConstraints(
 		const std::vector<int> & ids,
-		int targetMapId,
 		std::map<int, Transform> & poses,
 		std::multimap<int, std::pair<int, Transform> > & links,
 		bool lookInDatabase)
 {
-	UDEBUG("targetMap = %d", targetMapId);
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
 		Transform pose;
-		this->getPose(ids[i], targetMapId, pose, lookInDatabase);
+		this->getPose(ids[i], pose, lookInDatabase);
 		if(!pose.isNull())
 		{
 			poses.insert(std::make_pair(ids[i], pose));
@@ -3190,9 +3220,23 @@ void Memory::getMetricConstraints(
 			{
 				if(!jter->second.isNull() && uContains(poses, jter->first))
 				{
-					links.insert(std::make_pair(ids[i], *jter));
+					bool edgeAlreadyAdded = false;
+					for(std::multimap<int, std::pair<int, Transform> >::iterator iter = links.lower_bound(jter->first);
+							iter != links.end() && iter->first == jter->first;
+							++iter)
+					{
+						if(iter->second.first == ids[i])
+						{
+							edgeAlreadyAdded = true;
+						}
+					}
+					if(!edgeAlreadyAdded)
+					{
+						links.insert(std::make_pair(ids[i], *jter));
+					}
 				}
 			}
+
 			std::map<int, Transform> loops, children;
 			this->getLoopClosureIds(ids[i], loops, children, lookInDatabase);
 			for(std::map<int, Transform>::iterator jter=children.begin(); jter!=children.end(); ++jter)

corelib/src/Rtabmap.cpp

@@ -79,12 +79,13 @@ Rtabmap::Rtabmap() :
 	_rgbdSlamMode(Parameters::defaultRGBDEnabled()),
 	_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
+	_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
 	_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
 	_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
-	_icpEnabled(Parameters::defaultLccIcpEnabled()),
+	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
 	_databasePath(""),
 	_lcHypothesisId(0),
 	_lcHypothesisValue(0),
@@ -330,7 +331,21 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
-	Parameters::parse(parameters, Parameters::kLccIcpEnabled(), _icpEnabled);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
+
+	// RGB-D SLAM stuff
+	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
+	{
+		int icpType = std::atoi((*iter).second.c_str());
+		if(icpType >= 0 && icpType <= 2)
+		{
+			_globalLoopClosureIcpType = icpType;
+		}
+		else
+		{
+			UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
+		}
+	}
 
 	// By default, we create our strategies if they are not already created.
 	// If they already exists, we check the parameters if a change is requested
@@ -343,7 +358,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		//generate map
 		if(_memory->getLastWorkingSignature())
 		{
-			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, _mapCorrection);
+			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
 		}
 	}
 	else
@@ -482,15 +497,6 @@ std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
 	return std::multimap<int, cv::KeyPoint>();
 }
 
-std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM) const
-{
-	if(_memory)
-	{
-		return _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
-	}
-	return std::map<int, int>();
-}
-
 bool Rtabmap::isInSTM(int locationId) const
 {
 	if(_memory)
@@ -551,6 +557,7 @@ void Rtabmap::triggerNewMap()
 	{
 		int mapId = _memory->incrementMapId();
 		UINFO("New map triggerred, new map = %d", mapId);
+		_optimizedPoses.clear();
 	}
 }
 
@@ -586,6 +593,27 @@ void Rtabmap::generateGraph(const std::string & path, int id, int margin)
 	}
 }
 
+void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool full)
+{
+	if(_memory && _memory->getLastWorkingSignature())
+	{
+		std::map<int, Transform> poses;
+		std::multimap<int, std::pair<int, Transform> > constraints;
+
+		if(optimized)
+		{
+			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses, &constraints);
+		}
+		else
+		{
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, full?-1:0, true);
+			_memory->getMetricConstraints(uKeys(ids), poses, constraints, full);
+		}
+
+		util3d::saveTOROGraph(path, poses, constraints);
+	}
+}
+
 void Rtabmap::resetMemory(bool dbOverwritten)
 {
 	_retrievedId = 0;
@@ -601,7 +629,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 		_memory->init(getDatabasePath(), dbOverwritten);
 		if(_memory->getLastWorkingSignature())
 		{
-			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, _mapCorrection);
+			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses);
 		}
 		if(_bayesFilter)
 		{
@@ -610,7 +638,6 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 	}
 	else if(dbOverwritten)
 	{
-		// FIXME May be not work with other database type.
 		// May be memory should be already created here, and use init above...
 		UINFO("Erasing file : \"%s\"", getDatabasePath().c_str());
 		UFile::erase(getDatabasePath());
@@ -712,10 +739,12 @@ bool Rtabmap::process(const Image & image)
 				Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
 				float x,y,z, roll,pitch,yaw;
 				lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				// TODO Increment map id also if there is a big position change
 				if(!lastPose.isIdentity() && image.pose().isIdentity())
 				{
 					int mapId = _memory->incrementMapId();
 					UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
+					_optimizedPoses.clear();
 				}
 			}
 		}
@@ -774,6 +803,13 @@ bool Rtabmap::process(const Image & image)
 			}
 		}
 
+		// Reset map correction if we are mapping!
+		if(_memory->isIncremental() && !_mapCorrection.isIdentity())
+		{
+			UWARN("Reset map correction because we are now mapping!");
+			_mapCorrection.setIdentity();
+		}
+
 		Transform newPose = _mapCorrection * signature->getPose();
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
 
@@ -785,7 +821,7 @@ bool Rtabmap::process(const Image & image)
 			signature->getDepth2D().size() &&
 			rehearsedId == 0) // don't do it if rehearsal happened
 		{
-			UINFO("Scan matching...");
+			UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
 			const std::set<int> & stm = _memory->getStMem();
 			std::map<int, Transform> poses;
 			int count = _scanMatchingSize;
@@ -794,7 +830,7 @@ bool Rtabmap::process(const Image & image)
 				if(_memory->getSignature(*iter)->mapId() == signature->mapId())
 				{
 					std::map<int, Transform>::iterator jter = _optimizedPoses.find(*iter);
-					UASSERT(jter != _optimizedPoses.end());
+					UASSERT_MSG(jter != _optimizedPoses.end(), uFormat("%d not found in optimized poses!", *iter).c_str());
 					poses.insert(*jter);
 					if(*iter != signature->id())
 					{
@@ -844,9 +880,9 @@ bool Rtabmap::process(const Image & image)
 				{
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
 					Transform transform = _memory->computeVisualTransform(*iter, signature->id());
-					if(!transform.isNull() && _icpEnabled)
+					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 					{
-						transform = _memory->computeIcpTransform(*iter, signature->id(), transform);
+						transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
 					}
 					if(!transform.isNull())
 					{
@@ -855,7 +891,7 @@ bool Rtabmap::process(const Image & image)
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLoopClosureLink(*iter, signature->id(), transform))
+						if(_memory->addLoopClosureLink(*iter, signature->id(), transform, false))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1170,9 +1206,9 @@ bool Rtabmap::process(const Image & image)
 		if(_rgbdSlamMode)
 		{
 			transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
-			if(!transform.isNull() && _icpEnabled)
+			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				transform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform);
+				transform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1182,7 +1218,7 @@ bool Rtabmap::process(const Image & image)
 		}
 		if(!rejectedHypothesis)
 		{
-			rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform);
+			rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, true);
 		}
 
 		if(rejectedHypothesis)
@@ -1219,12 +1255,17 @@ bool Rtabmap::process(const Image & image)
 	   signature->getDepth2D().size())
 	{
 		//============================================================
-		// Scan matching
+		// Scan matching LOCAL LOOP CLOSURE SPACE
 		//============================================================
 		// get all nodes in radius of the current node
 		std::map<int, Transform> poses;
 		int oldId = 0;
-		poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, oldId);
+		poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, oldId);
+
+		// add current node to poses
+		UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
+		poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+
 		localSpaceDetectionPosesCount = poses.size()-1;
 		//The nearest will be the reference for a loop closure transform
 		if(poses.size() &&
@@ -1239,7 +1280,7 @@ bool Rtabmap::process(const Image & image)
 						signature->id(),
 						oldId,
 						t.prettyPrint().c_str());
-				_memory->addLoopClosureLink(oldId, signature->id(), t);
+				_memory->addLoopClosureLink(oldId, signature->id(), t, false);
 			}
 		}
 	}
@@ -1260,7 +1301,7 @@ bool Rtabmap::process(const Image & image)
 		{
 			UINFO("Update map correction: SLAM mode");
 			// SLAM mode!
-			optimizeCurrentMap(signature->id(), false, _optimizedPoses, _mapCorrection);
+			optimizeCurrentMap(signature->id(), false, _optimizedPoses);
 		}
 		else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
 		{
@@ -1270,7 +1311,7 @@ bool Rtabmap::process(const Image & image)
 			if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
 			{
 				// update optimized poses
-				optimizeCurrentMap(_retrievedId, false, _optimizedPoses, _mapCorrection);
+				optimizeCurrentMap(_retrievedId, false, _optimizedPoses);
 			}
 
 			if(_optimizedPoses.find(oldId) == _optimizedPoses.end())
@@ -1340,13 +1381,19 @@ bool Rtabmap::process(const Image & image)
 			statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 
-			statistics_.addStatistic(Statistics::kLocalLoopScan_matching_success(), scanMatchingSuccess?1:0);
+			statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
 			if(localSpaceClosureId)
 			{
 				statistics_.setLocalLoopClosureId(localSpaceClosureId);
+				int d1 = abs(signature->id() - localSpaceClosureId);
+				int d2 = abs(localSpaceClosureId - _memory->getLastGlobalLoopClosureChildId());
+				int d3 = abs(signature->id() - _memory->getLastGlobalLoopClosureParentId());
+				int d = d1<=d2?d1:d2;
+				d = d <= d3?d:d3;
+				statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
 			}
 			if(_lcHypothesisId || localSpaceClosureId)
 			{
@@ -1503,13 +1550,14 @@ bool Rtabmap::process(const Image & image)
 	}
 	_lastProcessTime = totalTime;
 
-	//Removed optimized poses from signatures transferred
+	//Remove optimized poses from signatures transferred
 	for(std::list<int>::iterator iter = signaturesRemoved.begin(); iter!=signaturesRemoved.end(); ++iter)
 	{
 		UDEBUG("removing optimized pose %d...", *iter);
 		_optimizedPoses.erase(*iter);
 	}
 
+
 	timeRealTimeLimitReachedProcess = timer.ticks();
 	ULOGGER_INFO("Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
 
@@ -1710,14 +1758,17 @@ void Rtabmap::dumpData() const
 	}
 }
 
+// fromId must be in _memory and in _optimizedPoses
 std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 		int fromId,
 		int maxNearestNeighbors,
 		float radius,
+		int maxDiffID, // 0 means ignore
 		int & nearestId) const
 {
 	UDEBUG("");
 	const Signature * fromS = _memory->getSignature(fromId);
+	UASSERT(fromS != 0);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	cloud->resize(_optimizedPoses.size());
@@ -1726,8 +1777,9 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 	const std::set<int> & stm = _memory->getStMem();
 	for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 	{
-		// Only locations in Working Memory and in the same map.
+		// Only locations in Working Memory with ID not too far from the last loop closure child id
-		if(stm.find(iter->first) == stm.end() && fromS->mapId() == _memory->getSignature(iter->first)->mapId())
+		bool diffIdOk = maxDiffID == 0 || abs(fromId - iter->first) <= maxDiffID || (abs(iter->first - _memory->getLastGlobalLoopClosureChildId()) <= maxDiffID && abs(fromId - _memory->getLastGlobalLoopClosureParentId()) <= maxDiffID);
+		if(stm.find(iter->first) == stm.end() && diffIdOk)
 		{
 			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 			ids[oi++] = iter->first;
@@ -1737,11 +1789,11 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 	cloud->resize(oi);
 	ids.resize(oi);
 
+	UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
 	Transform fromT = _optimizedPoses.at(fromId);
 
 	nearestId = 0;
 	std::map<int, Transform> poses;
-	poses.insert(*_optimizedPoses.find(fromId)); // add fromId
 	float minDistance = -1;
 	if(cloud->size())
 	{
@@ -1789,6 +1841,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 					}
 				}
 			}
+
 			//if(inliers.size())
 			//{
 			//	pcl::io::savePCDFile("radiusInliers.pcd", inliers);
@@ -1800,29 +1853,130 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 			//	c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
 			//	pcl::io::savePCDFile("radiusNearestPt.pcd", c);
 			//}
+
+			if(nearestId > 0)
+			{
+				// Only take nodes linked with the nearest node
+				std::map<int, int> neighbors = _memory->getNeighborsId(nearestId, maxNearestNeighbors, 0, true, true);
+				for(std::map<int, Transform>::iterator iter = poses.begin(); iter!= poses.end();)
+				{
+					if(!uContains(neighbors, iter->first))
+					{
+						poses.erase(iter++);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+			else if(poses.size())
+			{
+				UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId);
+				poses.clear();
+			}
 		}
 	}
 	UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
 	return poses;
 }
 
-void Rtabmap::optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const
+void Rtabmap::optimizeCurrentMap(
+		int id,
+		bool lookInDatabase,
+		std::map<int, Transform> & optimizedPoses,
+		std::multimap<int, std::pair<int, Transform> > * constraints) const
 {
 	//Optimize the map
+	optimizedPoses.clear();
 	UDEBUG("Optimize map: around location %d", id);
 	if(_memory && id > 0)
 	{
-		std::map<int, int> ids = _memory->getNeighborsId(id, 999, lookInDatabase?-1:0);
+
+		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
+		UDEBUG("ids=%d", (int)ids.size());
+
 		std::map<int, Transform> poses;
 		std::multimap<int, std::pair<int, Transform> > edgeConstraints;
-		_memory->getMetricConstraints(uKeys(ids), _memory->getSignature(id)->mapId(), poses, edgeConstraints, false);
+		_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
-		if(poses.size() > 1 && edgeConstraints.size() > 0)
+		UDEBUG("poses=%d, edgeConstraints=%d", (int)poses.size(), (int)edgeConstraints.size());
+
+		if(constraints)
 		{
-			util3d::optimizeTOROGraph(poses, edgeConstraints, 100, optimizedPoses, mapCorrection);
+			*constraints = edgeConstraints;
+		}
+
+		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
+		int m = 0;
+		int toroId = 1;
+		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
+		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
+		while(ids.size())
+		{
+			for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end();)
+			{
+				if(m == iter->second)
+				{
+					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
+					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
+					++toroId;
+					ids.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+			++m;
+		}
+
+		//
+		std::map<int, Transform> posesToro;
+		std::multimap<int, std::pair<int, Transform> > edgeConstraintsToro;
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
+		}
+		for(std::multimap<int, std::pair<int, Transform> >::iterator iter = edgeConstraints.begin();
+			iter!=edgeConstraints.end();
+			++iter)
+		{
+			edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), std::make_pair(rtabmapToToro.at(iter->second.first), iter->second.second)));
+		}
+
+		if(posesToro.size() > 1 && edgeConstraintsToro.size() > 0)
+		{
+			///UDEBUG("TORO optimize begin");
+			//util3d::saveTOROGraph("toroIdRtabmap.graph", poses, edgeConstraints);
+			//util3d::saveTOROGraph("toroIdToro.graph", posesToro, edgeConstraintsToro);
+			//UDEBUG("graph saved");
+
+			std::map<int, Transform> optimizedPosesToro;
+			Transform mapCorrectionToro;
+			util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, 100, optimizedPosesToro, mapCorrectionToro);
+
+			//UDEBUG("saving optimized graph...");
+			//util3d::saveTOROGraph("toroOptimized.graph", optimizedPosesToro, edgeConstraintsToro);
+			//UDEBUG("TORO optimize end");
+
+			for(std::map<int, Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
+			{
+				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
+			}
 
 			//util3d::saveTOROGraph("toro.graph", poses, edgeConstraints);
 			//util3d::saveTOROGraph("toroOptimized.graph", _optimizedPoses, edgeConstraints);
 		}
+		else if(poses.size() == 1 && edgeConstraints.size() == 0)
+		{
+			optimizedPoses = poses;
+		}
+		else if(poses.size() || edgeConstraints.size())
+		{
+			UFATAL("Poses=%d and edges=%d (poses must "
+				   "not be null if there are edges, and edges must be null if poses <= 1)",
+				   poses.size(), edgeConstraints.size());
+		}
 	}
 }
 
@@ -1936,44 +2090,50 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 		std::map<int, std::vector<unsigned char> > & depths2d,
 		std::map<int, float> & depthConstants,
 		std::map<int, Transform> & localTransforms,
-		std::map<int, Transform> & optimizedPoses,
+		std::map<int, Transform> & poses,
-		Transform & mapCorrection) const
+		bool optimized,
+		bool full) const
 {
-	if(_memory)
+	if(_memory && _memory->getLastWorkingSignature())
 	{
-		//Optimize the map
+		if(optimized)
-		UDEBUG("Optimize map");
-		optimizedPoses = _optimizedPoses;
-		mapCorrection = _mapCorrection;
-
-		std::set<int> ids = _memory->getAllSignatureIds();
-		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			UDEBUG("Adding %d ... ", *iter);
+			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), full, poses);
+		}
+		else
+		{
+			std::multimap<int, std::pair<int, Transform> > constraints;
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, full?-1:0, true);
+			_memory->getMetricConstraints(uKeys(ids), poses, constraints, full);
+		}
+
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			UDEBUG("Adding %d ... ", iter->first);
 			std::vector<unsigned char> image, depth, depth2d;
 			float depthConstant;
 			Transform localTransform;
-			_memory->getImageDepth(*iter, image, depth, depth2d, depthConstant, localTransform);
+			_memory->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
 
 			if(image.size())
 			{
-				images.insert(std::make_pair(*iter, image));
+				images.insert(std::make_pair(iter->first, image));
 			}
 			if(depth.size())
 			{
-				depths.insert(std::make_pair(*iter, depth));
+				depths.insert(std::make_pair(iter->first, depth));
 			}
 			if(depth2d.size())
 			{
-				depths2d.insert(std::make_pair(*iter, depth2d));
+				depths2d.insert(std::make_pair(iter->first, depth2d));
 			}
 			if(depthConstant > 0)
 			{
-				depthConstants.insert(std::make_pair(*iter, depthConstant));
+				depthConstants.insert(std::make_pair(iter->first, depthConstant));
 			}
 			if(!localTransform.isNull())
 			{
-				localTransforms.insert(std::make_pair(*iter, localTransform));
+				localTransforms.insert(std::make_pair(iter->first, localTransform));
 			}
 		}
 	}

corelib/src/RtabmapThread.cpp

@@ -29,6 +29,7 @@
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
 
 namespace rtabmap {
 
@@ -76,7 +77,7 @@ void RtabmapThread::clearBufferedData()
 	_imageMutex.unlock();
 }
 
-void RtabmapThread::publishMap() const
+void RtabmapThread::publishMap(bool optimized, bool full) const
 {
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
@@ -84,7 +85,6 @@ void RtabmapThread::publishMap() const
 	std::map<int, float> depthConstants;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
-	Transform mapCorrection;
 
 	_rtabmap->get3DMap(images,
 			depths,
@@ -92,15 +92,15 @@ void RtabmapThread::publishMap() const
 			depthConstants,
 			localTransforms,
 			poses,
-			mapCorrection);
+			optimized,
+			full);
 
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
 			depthConstants,
 			localTransforms,
-			poses,
+			poses));
-			mapCorrection));
 }
 
 void RtabmapThread::mainLoopKill()
@@ -156,6 +156,12 @@ void RtabmapThread::mainLoop()
 	case kStateGeneratingLocalGraph:
 		_rtabmap->generateGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
 		break;
+	case kStateGeneratingTOROGraph:
+		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
+		break;
+	case kStateGeneratingTOROGraphFull:
+		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
+		break;
 	case kStateDeletingMemory:
 		if(!parameters.at("path").empty())
 		{
@@ -168,7 +174,10 @@ void RtabmapThread::mainLoop()
 		this->clearBufferedData();
 		break;
 	case kStatePublishingMap:
-		this->publishMap();
+		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
+		break;
+	case kStatePublishingMapFull:
+		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
@@ -223,26 +232,47 @@ void RtabmapThread::handleEvent(UEvent* event)
 		}
 		else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
 		{
-			if(!rtabmapEvent->getStr().empty())
+			UASSERT(!rtabmapEvent->getStr().empty());
-			{
+
-				ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
+			ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
-				ParametersMap param;
+			ParametersMap param;
-				param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-				pushNewState(kStateGeneratingGraph, param);
+			pushNewState(kStateGeneratingGraph, param);
-			}
 		}
 		else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
 		{
 			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
-			if(values.size() == 3)
+			UASSERT(values.size() == 3);
-			{
+
-				ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
+			ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
-				ParametersMap param;
+			ParametersMap param;
-				param.insert(ParametersPair("path", *values.begin()));
+			param.insert(ParametersPair("path", *values.begin()));
-				param.insert(ParametersPair("id", *(++values.begin())));
+			param.insert(ParametersPair("id", *(++values.begin())));
-				param.insert(ParametersPair("margin", *values.rbegin()));
+			param.insert(ParametersPair("margin", *values.rbegin()));
-				pushNewState(kStateGeneratingLocalGraph, param);
+			pushNewState(kStateGeneratingLocalGraph, param);
-			}
+
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraph)
+		{
+			UASSERT(!rtabmapEvent->getStr().empty());
+
+			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH");
+			ParametersMap param;
+			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateGeneratingTOROGraph, param);
+
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphFull)
+		{
+			UASSERT(!rtabmapEvent->getStr().empty());
+
+			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_FULL");
+			ParametersMap param;
+			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateGeneratingTOROGraphFull, param);
+
 		}
 		else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
 		{
@@ -259,7 +289,16 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
 		{
 			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
-			pushNewState(kStatePublishingMap);
+			ParametersMap param;
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStatePublishingMap, param);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapFull)
+		{
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP_FULL");
+			ParametersMap param;
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStatePublishingMapFull, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
 		{

corelib/src/util3d.cpp

@@ -1266,12 +1266,12 @@ int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & clo
 	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 	kdTree->setInputCloud(cloud_target);
 	int count = 0;
+	float sqrdMaxDistance = maxDistance * maxDistance;
 	for(unsigned int i=0; i<cloud_source->size(); ++i)
 	{
 		std::vector<int> ind(1);
 		std::vector<float> dist(1);
-		kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist);
+		if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
-		if(dist[0] < maxDistance)
 		{
 			++count;
 		}
@@ -1545,16 +1545,22 @@ void optimizeTOROGraph(
 				return;
 			}
 		}
+		pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
 
-		pg.buildMST(pg.vertices.begin()->first);
+		UDEBUG("Initial guess...");
+		pg.initializeOnTree(); // optional
 
 		pg.initializeTreeParameters();
+		UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
+			   "TORO is not able to find the root of the graph!)");
 		pg.initializeOptimization();
 
+		UDEBUG("TORO iterate begin");
 		for (int i=0; i<toroIterations; i++)
 		{
 			pg.iterate();
 		}
+		UDEBUG("TORO iterate end");
 
 		optimizedPoses.clear();
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

guilib/include/rtabmap/gui/CloudViewer.h

@@ -98,6 +98,7 @@ public:
 	bool removeCloud(const std::string & id); //including mesh
 
 	bool getPose(const std::string & id, Transform & pose); //including meshes
+	bool getCloudVisibility(const std::string & id);
 
 	const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
 

guilib/include/rtabmap/gui/DataRecorder.h

@@ -25,7 +25,7 @@ class RTABMAPGUI_EXP DataRecorder : public QWidget, public UEventsHandler
 	Q_OBJECT
 public:
 	DataRecorder(QWidget * parent = 0);
-	bool init(const QString & path);
+	bool init(const QString & path, bool recordInRAM = true);
 
 	void close();
 

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -54,6 +54,7 @@ public:
 private slots:
 	void openDatabase();
 	void generateGraph();
+	void exportDatabase();
 	void generateLocalGraph();
 	void generate3DMap();
 	void sliderAValueChanged(int);

guilib/include/rtabmap/gui/MainWindow.h

@@ -104,6 +104,7 @@ private slots:
 	void printLoopClosureIds();
 	void generateMap();
 	void generateLocalMap();
+	void generateTOROMap();
 	void deleteMemory();
 	void openWorkingDirectory();
 	void updateEditMenu();
@@ -147,6 +148,7 @@ private slots:
 	void viewMeshes();
 	void resetOdometry();
 	void triggerNewMap();
+	void updateNodeVisibility(int, bool);
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
@@ -231,6 +233,7 @@ private:
 	DetailedProgressDialog * _initProgressDialog;
 
 	QString _graphSavingFileName;
+	QString _toroSavingFileName;
 	QString _autoScreenCaptureFormat;
 
 	QVector<int> _refIds;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -192,7 +192,8 @@ private slots:
 	void resetApply ( QAbstractButton * button );
 	void resetSettings(int panelNumber);
 	void loadConfigFrom();
-	void saveConfigTo();
+	bool saveConfigTo();
+	void resetConfig();
 	void makeObsoleteGeneralPanel();
 	void makeObsoleteCloudRenderingPanel();
 	void makeObsoleteLoggingPanel();

guilib/src/CMakeLists.txt

@@ -16,6 +16,8 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
 	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
 	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
+	./ExportDialog.h
+	./MapVisibilityWidget.h
 )
 
 SET(uis
@@ -25,6 +27,7 @@ SET(uis
     ./ui/consoleWidget.ui
     ./ui/DatabaseViewer.ui
     ./ui/loopClosureViewer.ui
+    ./ui/exportDialog.ui
 )
 
 SET(qrc 
@@ -60,6 +63,8 @@ SET(SRC_FILES
 	./OdometryViewer.cpp
 	./LoopClosureViewer.cpp
 	./DataRecorder.cpp
+	./ExportDialog.cpp
+	./MapVisibilityWidget.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}

guilib/src/CloudViewer.cpp

@@ -473,6 +473,21 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
 	}
 }
 
+bool CloudViewer::getCloudVisibility(const std::string & id)
+{
+	pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
+	pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
+	if(iter != cloudActorMap->end())
+	{
+		return iter->second.actor->GetVisibility() != 0;
+	}
+	else
+	{
+		UERROR("Cannot find actor named \"%s\".", id.c_str());
+	}
+	return false;
+}
+
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);

guilib/src/DataRecorder.cpp

@@ -32,7 +32,7 @@ DataRecorder::DataRecorder(QWidget * parent) :
 	layout->addWidget(imageView_);
 	this->setLayout(layout);
 }
-bool DataRecorder::init(const QString & path)
+bool DataRecorder::init(const QString & path, bool recordInRAM)
 {
 	if(!memory_)
 	{
@@ -40,6 +40,10 @@ bool DataRecorder::init(const QString & path)
 		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 		customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
 		customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true")); // to keep images
+		if(!recordInRAM)
+		{
+			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // to keep images
+		}
 		memory_ = new Memory();
 		if(!memory_->init(path.toStdString(), true, customParameters, false))
 		{

guilib/src/DatabaseViewer.cpp

@@ -35,6 +35,10 @@
 #include "rtabmap/gui/UCv2Qt.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
+#include "rtabmap/gui/DataRecorder.h"
+#include "rtabmap/core/Image.h"
+#include "ExportDialog.h"
+#include "DetailedProgressDialog.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/filters/voxel_grid.h>
@@ -59,6 +63,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 
 	// connect actions with custom slots
 	connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
+	connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
 	connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
 	connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
@@ -130,6 +135,82 @@ bool DatabaseViewer::openDatabase(const QString & path)
 	return false;
 }
 
+void DatabaseViewer::exportDatabase()
+{
+	if(!memory_ || ids_.size() == 0)
+	{
+		return;
+	}
+
+	rtabmap::ExportDialog dialog;
+
+	if(dialog.exec())
+	{
+		if(!dialog.outputPath().isEmpty())
+		{
+			int framesIgnored = dialog.framesIgnored();
+			QString path = dialog.outputPath();
+			rtabmap::DataRecorder recorder;
+			if(recorder.init(path, false))
+			{
+				rtabmap::DetailedProgressDialog progressDialog(this);
+				progressDialog.setMaximumSteps(ids_.size() / (1+framesIgnored) + 1);
+				progressDialog.show();
+
+				for(int i=0; i<ids_.size(); i+=1+framesIgnored)
+				{
+					int id = ids_.at(i);
+					std::vector<unsigned char> compressedRgb, compressedDepth, compressedDepth2d;
+					float tmpDepthConstant;
+					rtabmap::Transform tmpLocalTransform, pose;
+
+					memory_->getImageDepth(id, compressedRgb, compressedDepth, compressedDepth2d, tmpDepthConstant, tmpLocalTransform);
+					if(dialog.isOdomExported())
+					{
+						memory_->getPose(id, pose, true);
+					}
+
+					cv::Mat rgb, depth, depth2d;
+					float depthConstant = 0;
+					rtabmap::Transform localTransform;
+
+					if(dialog.isRgbExported())
+					{
+						rgb = rtabmap::util3d::uncompressImage(compressedRgb);
+					}
+					if(dialog.isDepthExported())
+					{
+						depth = rtabmap::util3d::uncompressImage(compressedDepth);
+						depthConstant = tmpDepthConstant;
+						localTransform = tmpLocalTransform;
+					}
+					if(dialog.isDepth2dExported())
+					{
+						depth2d = rtabmap::util3d::uncompressData(compressedDepth2d);
+					}
+
+					rtabmap::Image data(rgb, depth, depth2d, depthConstant, pose, localTransform, id);
+					recorder.addData(data);
+
+					progressDialog.appendText(tr("Exported node %1").arg(id));
+					progressDialog.incrementStep();
+					QApplication::processEvents();
+				}
+				progressDialog.setValue(progressDialog.maximumSteps());
+				progressDialog.appendText("Export finished!");
+			}
+			else
+			{
+				UERROR("DataRecorder init failed?!");
+			}
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
+		}
+	}
+}
+
 void DatabaseViewer::updateIds()
 {
 	if(!memory_)
@@ -246,7 +327,7 @@ void DatabaseViewer::generate3DMap()
 					{
 						std::map<int, rtabmap::Transform> poses, optimizedPoses;
 						std::multimap<int, std::pair<int, rtabmap::Transform> > edgeConstraints;
-						memory_->getMetricConstraints(uKeys(ids), memory_->getSignature(id)->mapId(), poses, edgeConstraints, true);
+						memory_->getMetricConstraints(uKeys(ids), poses, edgeConstraints, true);
 
 						UINFO("Poses=%d, constraints=%d", poses.size(), edgeConstraints.size());
 

guilib/src/ExportDialog.cpp

@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ExportDialog.h"
+#include "ui_exportDialog.h"
+
+#include <QtGui/QFileDialog>
+
+namespace rtabmap {
+
+ExportDialog::ExportDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_ExportDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
+
+	_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
+}
+
+ExportDialog::~ExportDialog()
+{
+	delete _ui;
+}
+
+void ExportDialog::getPath()
+{
+	QString path = QFileDialog::getSaveFileName(this, tr("Output database path..."), _ui->lineEdit_path->text(), tr("RTAB-Map database (*.db)"));
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_path->setText(path);
+	}
+}
+
+QString ExportDialog::outputPath() const
+{
+	return _ui->lineEdit_path->text();
+}
+
+int ExportDialog::framesIgnored() const
+{
+	return _ui->spinBox_ignored->value();
+}
+
+bool ExportDialog::isRgbExported() const
+{
+	return _ui->checkBox_rgb->isChecked();
+}
+
+bool ExportDialog::isDepthExported() const
+{
+	return _ui->checkBox_depth->isChecked();
+}
+
+bool ExportDialog::isDepth2dExported() const
+{
+	return _ui->checkBox_depth2d->isChecked();
+}
+
+bool ExportDialog::isOdomExported() const
+{
+	return _ui->checkBox_odom->isChecked();
+}
+
+}

guilib/src/ExportDialog.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef EXPORTDIALOG_H_
+#define EXPORTDIALOG_H_
+
+#include <QtGui/QDialog>
+
+class Ui_ExportDialog;
+
+namespace rtabmap {
+
+class ExportDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	ExportDialog(QWidget * parent = 0);
+
+	virtual ~ExportDialog();
+
+	QString outputPath() const;
+	int framesIgnored() const;
+	bool isRgbExported() const;
+	bool isDepthExported() const;
+	bool isDepth2dExported() const;
+	bool isOdomExported() const;
+
+private slots:
+	void getPath();
+
+private:
+	Ui_ExportDialog * _ui;
+};
+
+}
+
+#endif /* ABOUTDIALOG_H_ */

guilib/src/MainWindow.cpp

@@ -151,6 +151,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 		_ui->dockWidget_statsV2->setVisible(false);
 		_ui->dockWidget_console->setVisible(false);
 		_ui->dockWidget_loopClosureViewer->setVisible(false);
+		_ui->dockWidget_mapVisibility->setVisible(false);
 		//_ui->dockWidget_cloudViewer->setVisible(false);
 	}
 
@@ -206,6 +207,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_initProgressDialog->setWindowTitle(tr("Progress dialog"));
 	_initProgressDialog->setMinimumWidth(800);
 
+	connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
+
 	//connect stuff
 	connect(_ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
 	qRegisterMetaType<MainWindow::State>("MainWindow::State");
@@ -222,6 +225,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->menuShow_view->addAction(_ui->dockWidget_console->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_cloudViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
+	_ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
 	_ui->toolBar->setWindowTitle(tr("Control toolbar"));
 	QAction * a = _ui->menuShow_view->addAction("Progress dialog");
@@ -239,6 +243,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
 	connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap()));
 	connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap()));
+	connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap()));
 	connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
 	connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
@@ -391,6 +396,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
 		_ui->dockWidget_console->close();
 		_ui->dockWidget_cloudViewer->close();
 		_ui->dockWidget_loopClosureViewer->close();
+		_ui->dockWidget_mapVisibility->close();
 
 		if(_camera)
 		{
@@ -616,7 +622,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	//Affichage des stats et images
 
 	int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
-	int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), -1);
+	int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
+
 	_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
 	_ui->label_matchId->clear();
 
@@ -688,7 +695,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
-		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopScan_matching_success(), 0.0f);
+		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopOdom_corrected(), 0.0f);
 		_ui->label_matchId->clear();
 		_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
@@ -916,6 +923,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			// update pose only if a odometry is not received
 			updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose());
+			_ui->widget_mapVisibility->setMap(stat.poses());
 			_odometryReceived = false;
 
 			_odometryCorrection = stat.mapCorrection();
@@ -1130,7 +1138,11 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Tr
 			int id = std::atoi(splitted.back().c_str());
 			if(poses.find(id) == poses.end())
 			{
-				_ui->widget_cloudViewer->setCloudVisibility(iter.key(), false);
+				if(_ui->widget_cloudViewer->getCloudVisibility(iter.key()))
+				{
+					UDEBUG("Hide %s", iter.key().c_str());
+					_ui->widget_cloudViewer->setCloudVisibility(iter.key(), false);
+				}
 			}
 		}
 	}
@@ -1170,6 +1182,25 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Tr
 	_ui->widget_cloudViewer->render();
 }
 
+void MainWindow::updateNodeVisibility(int nodeId, bool visible)
+{
+	if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
+	{
+		if(_preferencesDialog->isCloudsShown(0))
+		{
+			std::string cloudName = uFormat("cloud%d", nodeId);
+			_ui->widget_cloudViewer->setCloudVisibility(cloudName, visible);
+		}
+
+		if(_preferencesDialog->isScansShown(0))
+		{
+			std::string scanName = uFormat("scan%d", nodeId);
+			_ui->widget_cloudViewer->setCloudVisibility(scanName, visible);
+		}
+	}
+	_ui->widget_cloudViewer->render();
+}
+
 void MainWindow::processRtabmapEventInit(int status, const QString & info)
 {
 	if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
@@ -1220,7 +1251,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		UINFO(" depthConstants = %d", event.getDepthConstants().size());
 		UINFO(" localTransforms = %d", event.getLocalTransforms().size());
 		UINFO(" poses = %d", event.getPoses().size());
-		UINFO(" mapCorrection = %s", event.getMapCorrection().prettyPrint().c_str());
 
 		_initProgressDialog->appendText("Inserting data in the cache...");
 
@@ -1269,7 +1299,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		_initProgressDialog->appendText(tr("Inserted %1 local transforms.").arg(_localTransformsMap.size()));
 		_initProgressDialog->incrementStep();
 
-		_odometryCorrection = event.getMapCorrection();
+		_odometryCorrection.setIdentity();
 
 		_initProgressDialog->appendText("Inserting data in the cache... done.");
 
@@ -2155,6 +2185,66 @@ void MainWindow::generateLocalMap()
 	}
 }
 
+void MainWindow::generateTOROMap()
+{
+	if(_toroSavingFileName.isEmpty())
+	{
+		_toroSavingFileName = _preferencesDialog->getWorkingDirectory() + "/toro.graph";
+	}
+
+	QStringList items;
+	items.append("Current map optimized");
+	items.append("Current map not optimized");
+	items.append("Full map optimized");
+	items.append("Full map not optimized");
+	bool ok;
+	QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 0, false, &ok);
+	if(ok)
+	{
+		bool optimized=false, full=false;
+		if(item.compare("Current map optimized") == 0)
+		{
+			optimized = true;
+		}
+		else if(item.compare("Current map not optimized") == 0)
+		{
+
+		}
+		else if(item.compare("Full map optimized") == 0)
+		{
+			full=true;
+			optimized=true;
+		}
+		else if(item.compare("Full map not optimized") == 0)
+		{
+			full=true;
+		}
+		else
+		{
+			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
+		}
+
+		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _toroSavingFileName, tr("TORO file (*.graph)"));
+		if(!path.isEmpty())
+		{
+			_toroSavingFileName = path;
+			if(full)
+			{
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphFull, path.toStdString(), optimized?1:0));
+			}
+			else
+			{
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraph, path.toStdString(), optimized?1:0));
+			}
+
+			_ui->dockWidget_console->show();
+			_ui->widget_console->appendMsg(QString("TORO Graph saved (full=%1, optimized=%2)... %3")
+					.arg(full?"true":"false").arg(optimized?"true":"false").arg(_toroSavingFileName));
+		}
+
+	}
+}
+
 void MainWindow::deleteMemory()
 {
 	QMessageBox::StandardButton button;
@@ -2261,12 +2351,61 @@ void MainWindow::dumpThePrediction()
 
 void MainWindow::downloadAllClouds()
 {
-	UINFO("Download clouds...");
+	bool showAll = _state != kMonitoringPaused && _state != kMonitoring;
-	_initProgressDialog->setAutoClose(true, 1);
+
-	_initProgressDialog->resetProgress();
+	QStringList items;
-	_initProgressDialog->show();
+	items.append("Current map optimized");
-	_initProgressDialog->appendText("Downloading the map...");
+	if(showAll)
-	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap));
+	{
+		items.append("Current map not optimized");
+	}
+	items.append("Full map optimized");
+	if(showAll)
+	{
+		items.append("Full map not optimized");
+	}
+	bool ok;
+	QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, showAll?2:1, false, &ok);
+	if(ok)
+	{
+		bool optimized=false, full=false;
+		if(item.compare("Current map optimized") == 0)
+		{
+			optimized = true;
+		}
+		else if(item.compare("Current map not optimized") == 0)
+		{
+
+		}
+		else if(item.compare("Full map optimized") == 0)
+		{
+			full=true;
+			optimized=true;
+		}
+		else if(item.compare("Full map not optimized") == 0)
+		{
+			full=true;
+		}
+		else
+		{
+			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
+		}
+
+		UINFO("Download clouds...");
+		_initProgressDialog->setAutoClose(true, 1);
+		_initProgressDialog->resetProgress();
+		_initProgressDialog->show();
+		_initProgressDialog->appendText(tr("Downloading the map (full=%1 ,optimized=%2)...")
+				.arg(full?"true":"false").arg(optimized?"true":"false"));
+		if(full)
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapFull, optimized?1:0));
+		}
+		else
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, optimized?1:0));
+		}
+	}
 }
 
 void MainWindow::clearTheCache()
@@ -2470,6 +2609,7 @@ void MainWindow::savePointClouds()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
 
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
 		if(button == QMessageBox::No)
@@ -2497,6 +2637,7 @@ void MainWindow::saveMeshes()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
 
 		std::map<int, pcl::PolygonMesh::Ptr> meshes;
 		if(button == QMessageBox::No)
@@ -2527,6 +2668,7 @@ void MainWindow::viewPointClouds()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
 
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
 		if(button == QMessageBox::No)
@@ -2586,6 +2728,7 @@ void MainWindow::viewMeshes()
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
+		_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
 
 		std::map<int, pcl::PolygonMesh::Ptr> meshes;
 		if(button == QMessageBox::No)
@@ -3135,6 +3278,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(true);
 		_ui->actionGenerate_map->setEnabled(true);
 		_ui->actionGenerate_local_map->setEnabled(true);
+		_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
 		_ui->actionOpen_working_directory->setEnabled(true);
 		_ui->actionApply_settings_to_the_detector->setEnabled(true);
 		_ui->actionDownload_all_clouds->setEnabled(true);
@@ -3163,6 +3307,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(false);
 		_ui->actionGenerate_map->setEnabled(false);
 		_ui->actionGenerate_local_map->setEnabled(false);
+		_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
 		_ui->actionOpen_working_directory->setEnabled(true);
 		_ui->actionApply_settings_to_the_detector->setEnabled(false);
 		_ui->actionDownload_all_clouds->setEnabled(false);
@@ -3201,6 +3346,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionDelete_memory->setEnabled(false);
 			_ui->actionGenerate_map->setEnabled(false);
 			_ui->actionGenerate_local_map->setEnabled(false);
+			_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
 			_ui->actionDownload_all_clouds->setEnabled(false);
 			_state = kDetecting;
 			_elapsedTime->start();
@@ -3231,6 +3377,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionDelete_memory->setEnabled(true);
 			_ui->actionGenerate_map->setEnabled(true);
 			_ui->actionGenerate_local_map->setEnabled(true);
+			_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
 			_ui->actionDownload_all_clouds->setEnabled(true);
 			_state = kPaused;
 			_oneSecondTimer->stop();
@@ -3267,6 +3414,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(true);
 		_ui->actionGenerate_map->setVisible(false);
 		_ui->actionGenerate_local_map->setVisible(false);
+		_ui->actionGenerate_TORO_graph_graph->setVisible(false);
 		_ui->actionOpen_working_directory->setEnabled(false);
 		_ui->actionApply_settings_to_the_detector->setVisible(false);
 		_ui->actionDownload_all_clouds->setEnabled(true);
@@ -3294,6 +3442,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(true);
 		_ui->actionGenerate_map->setVisible(false);
 		_ui->actionGenerate_local_map->setVisible(false);
+		_ui->actionGenerate_TORO_graph_graph->setVisible(false);
 		_ui->actionOpen_working_directory->setEnabled(false);
 		_ui->actionApply_settings_to_the_detector->setVisible(false);
 		_ui->actionDownload_all_clouds->setEnabled(true);

guilib/src/MapVisibilityWidget.cpp

@@ -0,0 +1,86 @@
+/*
+ * MapVisibilityWidget.cpp
+ *
+ *  Created on: 2014-01-20
+ *      Author: mathieu
+ */
+
+#include "MapVisibilityWidget.h"
+
+#include <QtGui/QCheckBox>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QScrollArea>
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+MapVisibilityWidget::MapVisibilityWidget(QWidget * parent) : QWidget(parent) {
+
+	QVBoxLayout * verticalLayout1 = new QVBoxLayout(this);
+	QScrollArea * scrollArea = new QScrollArea(this);
+	scrollArea->setWidgetResizable(true);
+	QWidget * scrollAreaWidgetContent = new QWidget();
+	scrollAreaWidgetContent->setObjectName("area");
+	QVBoxLayout * layout2 = new QVBoxLayout(scrollAreaWidgetContent);
+	scrollAreaWidgetContent->setLayout(layout2);
+	scrollArea->setWidget(scrollAreaWidgetContent);
+	verticalLayout1->addWidget(scrollArea);
+}
+
+MapVisibilityWidget::~MapVisibilityWidget() {
+
+}
+
+void MapVisibilityWidget::showEvent(QShowEvent * event)
+{
+	updateCheckBoxes();
+}
+
+void MapVisibilityWidget::updateCheckBoxes()
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QVBoxLayout * layout = (QVBoxLayout *)area->layout();
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	while(checkboxes.size() && checkboxes.size() > _poses.size())
+	{
+		delete *checkboxes.begin();
+		checkboxes.erase(checkboxes.begin());
+	}
+	int i=0;
+	for(std::map<int, Transform>::iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
+	{
+		bool added = false;
+		if(i >= checkboxes.size())
+		{
+			checkboxes.push_back(new QCheckBox(area));
+			added = true;
+		}
+		checkboxes[i]->setText(QString("%1 (%2)").arg(iter->first).arg(iter->second.prettyPrint().c_str()));
+		checkboxes[i]->setChecked(true);
+		if(added)
+		{
+			connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(signalVisibility()));
+			layout->addWidget(checkboxes[i]);
+		}
+
+		++i;
+	}
+}
+
+void MapVisibilityWidget::setMap(const std::map<int, Transform> & poses)
+{
+	_poses = poses;
+	if(this->isVisible())
+	{
+		updateCheckBoxes();
+	}
+}
+
+void MapVisibilityWidget::signalVisibility()
+{
+	QCheckBox * check = qobject_cast<QCheckBox*>(sender());
+	emit visibilityChanged(check->text().split('(').first().toInt(), check->isChecked());
+}
+
+} /* namespace rtabmap */

guilib/src/MapVisibilityWidget.h

@@ -0,0 +1,42 @@
+/*
+ * MapVisibilityWidget.h
+ *
+ *  Created on: 2014-01-20
+ *      Author: mathieu
+ */
+
+#ifndef MAPVISIBILITYWIDGET_H_
+#define MAPVISIBILITYWIDGET_H_
+
+#include <QtGui/QWidget>
+#include <rtabmap/core/Transform.h>
+#include <map>
+
+namespace rtabmap {
+
+class MapVisibilityWidget : public QWidget {
+	Q_OBJECT;
+public:
+	MapVisibilityWidget(QWidget * parent = 0);
+	virtual ~MapVisibilityWidget();
+
+	void setMap(const std::map<int, Transform> & poses);
+
+protected:
+	virtual void showEvent(QShowEvent * event);
+
+private slots:
+	void signalVisibility();
+
+private:
+	void updateCheckBoxes();
+
+signals:
+	void visibilityChanged(int id, bool visible);
+
+private:
+	std::map<int, Transform> _poses;
+};
+
+} /* namespace rtabmap */
+#endif /* MAPVISIBILITYWIDGET_H_ */

guilib/src/PreferencesDialog.cpp

@@ -137,6 +137,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->buttonBox_local, SIGNAL(clicked(QAbstractButton *)), this, SLOT(resetApply(QAbstractButton *)));
 	connect(_ui->pushButton_loadConfig, SIGNAL(clicked()), this, SLOT(loadConfigFrom()));
 	connect(_ui->pushButton_saveConfig, SIGNAL(clicked()), this, SLOT(saveConfigTo()));
+	connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig()));
 	connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView()));
 	connect(_ui->pushButton_testSourceOdometry, SIGNAL(clicked()), this, SLOT(testSourceOdometry()));
 
@@ -374,19 +375,19 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str());
 
-	_ui->localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
+	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
-	_ui->localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
+	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalLoopDetectionRadius().c_str());
 	_ui->localDetection_maxNeighbors->setObjectName(Parameters::kRGBDLocalLoopDetectionNeighbors().c_str());
-
+	_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
-	_ui->loopClosure_icpEnabled->setObjectName(Parameters::kLccIcpEnabled().c_str());
-	_ui->loopClosure_icpType->setObjectName(Parameters::kLccIcpType().c_str());
 
 	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
 	_ui->loopClosure_bowIterations->setObjectName(Parameters::kLccBowIterations().c_str());
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccBowMaxDepth().c_str());
 
+	_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
+
 	_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
 	_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());
 	_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kLccIcp3VoxelSize().c_str());
@@ -399,6 +400,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
 	_ui->loopClosure_icp2MaxFitness->setObjectName(Parameters::kLccIcp2MaxFitness().c_str());
 	_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
+	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
 
 	//Odometry
 	_ui->odom_type->setObjectName(Parameters::kOdomType().c_str());
@@ -589,6 +591,7 @@ void PreferencesDialog::setupSignals()
 			QCheckBox * check = qobject_cast<QCheckBox *>(obj);
 			QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
 			QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
+			QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
 			if(spin)
 			{
 				connect(spin, SIGNAL(valueChanged(int)), this, SLOT(addParameter(int)));
@@ -613,6 +616,10 @@ void PreferencesDialog::setupSignals()
 			{
 				connect(lineEdit, SIGNAL(textChanged(const QString &)), this, SLOT(addParameter(const QString &)));
 			}
+			else if(groupBox)
+			{
+				connect(groupBox, SIGNAL(clicked(bool)), this, SLOT(addParameter(bool)));
+			}
 			else
 			{
 				ULOGGER_WARN("QObject called %s can't be cast to a supported widget", (*iter).first.c_str());
@@ -823,6 +830,13 @@ void PreferencesDialog::resetSettings(int panelNumber)
 	{
 		this->resetSettings(boxes.at(panelNumber));
 	}
+	else if(panelNumber == -1)
+	{
+		for(QList<QGroupBox*>::iterator iter = boxes.begin(); iter!=boxes.end(); ++iter)
+		{
+			this->resetSettings(*iter);
+		}
+	}
 	else
 	{
 		ULOGGER_WARN("panel number and the number of stacked widget doesn't match");
@@ -856,7 +870,7 @@ QString PreferencesDialog::getIniFilePath() const
 
 void PreferencesDialog::loadConfigFrom()
 {
-	QString path = QFileDialog::getOpenFileName(this, tr("Load configurations..."), this->getWorkingDirectory(), "*.ini");
+	QString path = QFileDialog::getOpenFileName(this, tr("Load settings..."), this->getWorkingDirectory(), "*.ini");
 	if(!path.isEmpty())
 	{
 		this->readSettings(path);
@@ -1054,12 +1068,28 @@ bool PreferencesDialog::readCoreSettings(const QString & filePath)
 	return true;
 }
 
-void PreferencesDialog::saveConfigTo()
+bool PreferencesDialog::saveConfigTo()
 {
-	QString path = QFileDialog::getSaveFileName(this, tr("Save configurations..."), this->getWorkingDirectory()+"/config.ini", "*.ini");
+	QString path = QFileDialog::getSaveFileName(this, tr("Save settings..."), this->getWorkingDirectory()+"/config.ini", "*.ini");
 	if(!path.isEmpty())
 	{
 		this->writeSettings(path);
+		return true;
+	}
+	return false;
+}
+
+void PreferencesDialog::resetConfig()
+{
+	int button = QMessageBox::warning(this,
+			tr("Reset settings..."),
+			tr("This will reset all settings. Restore all settings to default without saving them first?"),
+			   QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::Save,
+			   QMessageBox::Cancel);
+	if(button == QMessageBox::Yes ||
+	   (button == QMessageBox::Save && saveConfigTo()))
+	{
+		this->resetSettings(-1);
 	}
 }
 
@@ -1204,6 +1234,7 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
 			QCheckBox * check = qobject_cast<QCheckBox *>(obj);
 			QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
 			QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
+			QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
 			if(spin)
 			{
 				settings.setValue(obj->objectName(), spin->value());
@@ -1228,6 +1259,10 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
 			{
 				settings.setValue(obj->objectName(), lineEdit->text());
 			}
+			else if(groupBox)
+			{
+				settings.setValue(obj->objectName(), uBool2Str(groupBox->isChecked()).c_str());
+			}
 			else
 			{
 				ULOGGER_WARN("QObject called %s can't be cast to a supported widget", (*iter).first.c_str());
@@ -1599,6 +1634,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 		QCheckBox * check = qobject_cast<QCheckBox *>(obj);
 		QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
 		QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
+		QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
 		bool ok;
 		if(spin)
 		{
@@ -1636,6 +1672,10 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 		{
 			lineEdit->setText(value.c_str());
 		}
+		else if(groupBox)
+		{
+			groupBox->setChecked(uStr2Bool(value.c_str()));
+		}
 		else
 		{
 			ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
@@ -1755,17 +1795,15 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						_ui->stackedWidget_odom->setCurrentIndex(2);
 					}
 				}
-				else if(comboBox == _ui->loopClosure_icpType)
+				else if(comboBox == _ui->globalDetection_icpType)
 				{
-					if(value == 0) // 0 icp3
+					if(value == 1) // 1 icp3
 					{
 						this->addParameters(_ui->groupBox_loopClosure_icp3);
-						_ui->stackedWidget_loopClosureICP->setCurrentIndex(0);
 					}
-					else if(value == 1) // 1 icp2
+					else if(value == 2) // 2 icp2
 					{
 						this->addParameters(_ui->groupBox_loopClosure_icp2);
-						_ui->stackedWidget_loopClosureICP->setCurrentIndex(1);
 					}
 				}
 			}
@@ -1797,28 +1835,38 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 
 		const QCheckBox * checkbox = qobject_cast<const QCheckBox*>(object);
 		const QRadioButton * radio = qobject_cast<const QRadioButton*>(object);
-		if(checkbox || radio)
+		const QGroupBox * groupBox = qobject_cast<const QGroupBox*>(object);
+		if(checkbox || radio || groupBox)
 		{
 			// Add parameter
 			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), uBool2Str(value)));
 
+			// RGBD panel
 			if(value && checkbox == _ui->general_checkBox_activateRGBD)
 			{
 				// add all RGBD parameters!
-				this->addParameter(_ui->rgdb_linearUpdate, _ui->rgdb_linearUpdate->value());
+				this->addParameters(_ui->groupBox_slam_update);
-				this->addParameter(_ui->rgdb_angularUpdate, _ui->rgdb_linearUpdate->value());
+				this->addParameters(_ui->groupBox_odom_correction);
-				this->addParameter(_ui->odomScanHistory, _ui->odomScanHistory->value());
+				this->addParameters(_ui->groupBox_localDetection_time);
-				this->addParameters(_ui->groupBox_local_loop_closure);
+				this->addParameters(_ui->groupBox_localDetection_space);
-
+				this->addParameters(_ui->groupBox_globalConstraints);
-				this->addParameters(_ui->groupBox_lcc_bow);
+				this->addParameters(_ui->groupBox_visualTransform2);
-				if(_ui->loopClosure_icpEnabled->isChecked())
-				{
-					this->addParameter(_ui->loopClosure_icpType, _ui->loopClosure_icpType->currentIndex());
-				}
 			}
-			if(value && checkbox == _ui->loopClosure_icpEnabled)
+
+			if(groupBox)
 			{
-				this->addParameter(_ui->loopClosure_icpType, _ui->loopClosure_icpType->currentIndex());
+				// RGBD panel
+				if(value && groupBox == _ui->groupBox_localDetection_time)
+				{
+					this->addParameters(_ui->groupBox_globalConstraints);
+					this->addParameters(_ui->groupBox_visualTransform2);
+				}
+				if(value && groupBox == _ui->groupBox_localDetection_space)
+				{
+					this->addParameters(_ui->groupBox_loopClosure_icp2);
+				}
+
+				this->addParameters(groupBox);
 			}
 		}
 		else
@@ -1910,7 +1958,14 @@ void PreferencesDialog::addParameters(const QObjectList & children)
 		}
 		else if(groupBox)
 		{
-			this->addParameters(groupBox);
+			if(groupBox->isCheckable())
+			{
+				this->addParameter(groupBox, groupBox->isChecked());
+			}
+			else
+			{
+				this->addParameters(groupBox);
+			}
 		}
 		else if(stackedWidget)
 		{

guilib/src/ui/DatabaseViewer.ui

@@ -288,7 +288,7 @@
      <x>0</x>
      <y>0</y>
      <width>736</width>
-     <height>21</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -297,6 +297,8 @@
     </property>
     <addaction name="actionOpen_database"/>
     <addaction name="separator"/>
+    <addaction name="actionExport"/>
+    <addaction name="separator"/>
     <addaction name="actionQuit"/>
    </widget>
    <widget class="QMenu" name="menuEdit">
@@ -359,6 +361,11 @@
     <string>Generate 3D map (.pcd) ...</string>
    </property>
   </action>
+  <action name="actionExport">
+   <property name="text">
+    <string>Export...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/exportDialog.ui

@@ -0,0 +1,166 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ExportDialog</class>
+ <widget class="QDialog" name="ExportDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>371</width>
+    <height>236</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Export...</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <item>
+      <widget class="QSpinBox" name="spinBox_ignored">
+       <property name="suffix">
+        <string/>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label">
+       <property name="text">
+        <string>Frames ignored</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QCheckBox" name="checkBox_rgb">
+     <property name="text">
+      <string>RGB images</string>
+     </property>
+     <property name="checked">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QCheckBox" name="checkBox_depth">
+     <property name="text">
+      <string>Depth images</string>
+     </property>
+     <property name="checked">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QCheckBox" name="checkBox_depth2d">
+     <property name="text">
+      <string>Depth 2D (laser scans)</string>
+     </property>
+     <property name="checked">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QCheckBox" name="checkBox_odom">
+     <property name="text">
+      <string>Odometry</string>
+     </property>
+     <property name="checked">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLineEdit" name="lineEdit_path">
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QToolButton" name="toolButton_path">
+       <property name="text">
+        <string>...</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>accepted()</signal>
+   <receiver>ExportDialog</receiver>
+   <slot>accept()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>248</x>
+     <y>254</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>157</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>ExportDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>

guilib/src/ui/mainWindow.ui

@@ -102,7 +102,7 @@
      <x>0</x>
      <y>0</y>
      <width>809</width>
-     <height>21</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -128,6 +128,7 @@
     <addaction name="actionDump_the_prediction_matrix"/>
     <addaction name="actionGenerate_map"/>
     <addaction name="actionGenerate_local_map"/>
+    <addaction name="actionGenerate_TORO_graph_graph"/>
     <addaction name="separator"/>
     <addaction name="actionView_high_res_point_cloud"/>
     <addaction name="actionSave_point_cloud"/>
@@ -620,6 +621,27 @@
     </layout>
    </widget>
   </widget>
+  <widget class="QDockWidget" name="dockWidget_mapVisibility">
+   <property name="windowTitle">
+    <string>Map visibility</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>8</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_8">
+    <layout class="QVBoxLayout" name="verticalLayout_9">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="rtabmap::MapVisibilityWidget" name="widget_mapVisibility" native="true"/>
+     </item>
+    </layout>
+   </widget>
+  </widget>
   <action name="actionExit">
    <property name="text">
     <string>Exit</string>
@@ -909,6 +931,11 @@
     <string>Trigger a new map</string>
    </property>
   </action>
+  <action name="actionGenerate_TORO_graph_graph">
+   <property name="text">
+    <string>Generate TORO graph (*.graph)...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>
@@ -946,6 +973,12 @@
    <header>../include/rtabmap/gui/LoopClosureViewer.h</header>
    <container>1</container>
   </customwidget>
+  <customwidget>
+   <class>rtabmap::MapVisibilityWidget</class>
+   <extends>QWidget</extends>
+   <header>MapVisibilityWidget.h</header>
+   <container>1</container>
+  </customwidget>
  </customwidgets>
  <resources>
   <include location="../GuiLib.qrc"/>

tools/DatabaseViewer/main.cpp

@@ -29,13 +29,13 @@ int main(int argc, char * argv[])
 	QApplication * app = new QApplication(argc, argv);
 	DatabaseViewer * mainWindow = new DatabaseViewer();
 
+	mainWindow->showNormal();
+
 	if(argc == 2)
 	{
 		mainWindow->openDatabase(argv[1]);
 	}
 
-	mainWindow->showNormal();
-
 	// Now wait for application to finish
 	app->connect( app, SIGNAL( lastWindowClosed() ),
 				app, SLOT( quit() ) );