Integration of the path planner into rtabmap's memory management to actually retrieve nodes on the planned path on the graph.

Added Rtabmap::clearPath(), Rtabmap::computePath(), Rtabmap::getPathGoalId() and Rtabmap::updateGoalIndex() methods.
Added virtual links when path retrieval is activated (virtual links are not saved to database and removed when the path goal is reached).
New parameters: RGBD/GoalReachedRadius and RGBD/MaxAnticipatedNodes
Refactored how loop closure hypotheses are selected (fixed ratio between consecutive hypotheses, not the last loop closure).
Updated getNodesInRadius() to use all filtered scans.
GUI: window can be saved/loaded maximized
GUI: saving Graph View parameters to config.ini

app/src/main.cpp

@@ -43,16 +43,23 @@ int main(int argc, char* argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
 
-	ULOGGER_INFO("Program started...");
-
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	MainWindow * mainWindow = new MainWindow();
 
+	UINFO("Program started...");
+
 	UEventsManager::addHandler(mainWindow);
 
 	/* Start thread's task */
+	if(mainWindow->isSavedMaximized())
+	{
+		mainWindow->showMaximized();
+	}
+	else
+	{
 		mainWindow->show();
+	}
 
 	RtabmapThread * rtabmap = new RtabmapThread(new Rtabmap());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
@@ -67,37 +74,14 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	ULOGGER_INFO("Killing threads...");
+	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	ULogger::setLevel(ULogger::kInfo);
+	UINFO("Closing RTAB-Map...");
-
+	delete rtabmap;
-	ULOGGER_INFO("Closing RTAB-Map core...");
-
-	//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,
-			mainWindow);
-	msg->setEnabled(false);
-	msg->setIconPixmap(QPixmap(":/images/RTAB-Map.ico"));
-	msg->setWindowIcon(QIcon(":/images/RTAB-Map.ico"));
-	msg->show();
-	UObjDeletionThread<RtabmapThread> delThread(rtabmap);
-	ObjDeletionHandler handler(delThread.id(), app, SLOT(quit()));
-	UEventsManager::addHandler(&handler);
-	delThread.startDeletion(1); // make sure that app-exec() is called before the deletion of the object
-	app->exec();
-
-	ULOGGER_INFO("Closing RTAB-Map gui...");
 	delete mainWindow;
-
 	delete app;
-
+	UINFO("All done!");
-	ULOGGER_INFO("All done!");
 
     return 0;
 }

corelib/include/rtabmap/core/Link.h

@@ -35,7 +35,7 @@ namespace rtabmap {
 class Link
 {
 public:
-	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
 	Link() :
 		from_(0),
 		to_(0),

corelib/include/rtabmap/core/Memory.h

@@ -82,6 +82,7 @@ public:
 	void joinTrashThread();
 	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
 	void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
+	void removeAllVirtualLinks();
 	std::map<int, int> getNeighborsId(int signatureId,
 			int margin,
 			int maxCheckedInDatabase = -1,

corelib/include/rtabmap/core/Parameters.h

@@ -175,7 +175,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
 	RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,   	 bool, true, "Add column header description to log files.");
-	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.")
+	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
@@ -286,6 +286,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
 	RTABMAP_PARAM(RGBD, ToroIgnoreVariance,   bool, false,    "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 1.0, "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, MaxAnticipatedNodes,  unsigned int, 10, "Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by \"Rtabmap/MaxRetrieved\").");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -71,10 +71,11 @@ public:
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
-	int getLoopClosureId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
-	int getRetrievedId() const;
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
 	int getLastLocationId() const;
-	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM() const; // working memory
 	std::set<int> getSTM() const; // short-term memory
 	int getWMSize() const; // working memory size
@@ -90,6 +91,7 @@ public:
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -115,8 +117,13 @@ public:
 			std::map<int, int> & mapIds,
 			bool optimized,
 			bool global);
+	void clearPath();
+	std::list<std::pair<int, Transform> > computePath(int targetNode);
+	const std::vector<int> & getPath() const {return _path;}
+	int getPathGoalId() const;
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
+	std::map<int, Transform> getWMPosesInRadius(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;
@@ -126,6 +133,7 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	void updateGoalIndex();
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -166,10 +174,11 @@ private:
 	int _reextractFeatureType;
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
+	float _goalReachedRadius; // meters
+	unsigned int _maxAnticipatedNodes;
 
-	int _lcHypothesisId;
+	std::pair<int, float> _loopClosureHypothesis;
-	float _lcHypothesisValue;
+	std::pair<int, float> _highestHypothesis;
-	int _retrievedId;
 	double _lastProcessTime;
 
 	// Abstract classes containing all loop closure
@@ -193,6 +202,12 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+
+	// Planning stuff
+	std::vector<int> _path;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/Signature.h

@@ -89,6 +89,7 @@ public:
 
 	void removeLinks();
 	void removeLink(int idTo);
+	void removeVirtualLinks();
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}

corelib/src/DBDriverSqlite3.cpp

@@ -340,7 +340,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	}
 	if(rc != SQLITE_OK)
 	{
-		UFATAL("DB error : %s", sqlite3_errmsg(_ppDb));
+		UFATAL("DB error : %s (path=\"%s\")", sqlite3_errmsg(_ppDb), url.c_str());
 		_ppDb = 0;
 		return false;
 	}
@@ -2027,13 +2027,21 @@ std::string DBDriverSqlite3::queryStepLink() const
 		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
+void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const
 {
 	if(!ppStmt)
 	{
 		UFATAL("");
 	}
 	UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
+
+	// Don't save virtual links
+	if(type==Link::kVirtualClosure)
+	{
+		UDEBUG("Virtual link ignored....");
+		return;
+	}
+
 	int rc = SQLITE_OK;
 	int index = 1;
 	rc = sqlite3_bind_int(ppStmt, index++, fromId);

corelib/src/DBDriverSqlite3.h

@@ -109,7 +109,7 @@ private:
 			float cx,
 			float cy,
 			const Transform & localTransform) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 

corelib/src/Memory.cpp

@@ -2134,7 +2134,10 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 			return true;
 		}
 
+		if(type != Link::kVirtualClosure)
+		{
 			_memoryChanged = true;
+		}
 		UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id());
 
 		oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance));
@@ -2184,6 +2187,15 @@ void Memory::updateNeighborLink(int fromId, int toId, const Transform & transfor
 	}
 }
 
+void Memory::removeAllVirtualLinks()
+{
+	UDEBUG("");
+	for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		iter->second->removeVirtualLinks();
+	}
+}
+
 void Memory::dumpMemory(std::string directory) const
 {
 	UINFO("Dumping memory to directory \"%s\"", directory.c_str());

corelib/src/Rtabmap.cpp

@@ -107,9 +107,10 @@ Rtabmap::Rtabmap() :
 	_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
 	_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
-	_lcHypothesisId(0),
+	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
-	_lcHypothesisValue(0),
+	_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
-	_retrievedId(0),
+	_loopClosureHypothesis(0,0.0f),
+	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
@@ -118,7 +119,9 @@ Rtabmap::Rtabmap() :
 	_foutInt(0),
 	_wDir("."),
 	_mapCorrection(Transform::getIdentity()),
-	_mapTransform(Transform::getIdentity())
+	_mapTransform(Transform::getIdentity()),
+	_pathCurrentIndex(0),
+	_pathGoalIndex(0)
 {
 }
 
@@ -281,14 +284,15 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 
 void Rtabmap::close()
 {
-	_retrievedId = 0;
+	UINFO("");
-	_lcHypothesisValue = 0;
+	_highestHypothesis = std::make_pair(0,0.0f);
-	_lcHypothesisId = 0;
+	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
 	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapTransform.setIdentity();
+	this->clearPath();
 
 	flushStatisticLogs();
 	if(_foutFloat)
@@ -367,6 +371,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
 	Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
+	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
+	Parameters::parse(parameters, Parameters::kRGBDMaxAnticipatedNodes(), _maxAnticipatedNodes);
+
 
 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -437,16 +444,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 }
 
-int Rtabmap::getLoopClosureId() const
-{
-	return _lcHypothesisId;
-}
-
-int Rtabmap::getRetrievedId() const
-{
-	return _retrievedId;
-}
-
 int Rtabmap::getLastLocationId() const
 {
 	int id = 0;
@@ -652,9 +649,8 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 
 void Rtabmap::resetMemory()
 {
-	_retrievedId = 0;
+	_highestHypothesis = std::make_pair(0,0.0f);
-	_lcHypothesisValue = 0;
+	_loopClosureHypothesis = std::make_pair(0,0.0f);
-	_lcHypothesisId = 0;
 	_lastProcessTime = 0.0;
 	_optimizedPoses.clear();
 	_constraints.clear();
@@ -719,7 +715,6 @@ bool Rtabmap::process(const SensorData & data)
 	std::map<int, float> likelihood;
 	std::map<int, int> weights;
 	std::map<int, float> posterior;
-	std::pair<int, float> hypothesis(0, 0.0f);
 	std::list<std::pair<int, float> > reactivateHypotheses;
 
 	std::map<int, int> childCount;
@@ -730,7 +725,8 @@ bool Rtabmap::process(const SensorData & data)
 	const Signature * signature = 0;
 	const Signature * sLoop = 0;
 
-	_lcHypothesisId = 0;
+	_loopClosureHypothesis.first = 0; // Don't reset now the last loop closure value
+	_highestHypothesis = std::make_pair(0,0.0f);
 
 	std::set<int> immunizedLocations;
 
@@ -999,61 +995,55 @@ bool Rtabmap::process(const SensorData & data)
 			ULOGGER_INFO("creating hypotheses...");
 			if(posterior.size())
 			{
-				hypothesis.first = 0;
-				hypothesis.second = 0.0f;
 				for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
 				{
-					if(iter->first > 0 && iter->second > hypothesis.second)
+					if(iter->first > 0 && iter->second > _highestHypothesis.second)
 					{
-						hypothesis.first = iter->first;
+						_highestHypothesis = *iter;
-						hypothesis.second = iter->second;
 					}
 				}
 				// With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
-				hypothesis.second = 1-posterior.begin()->second;
+				_highestHypothesis.second = 1-posterior.begin()->second;
 			}
 			timeHypothesesCreation = timer.ticks();
-			ULOGGER_INFO("Hypothesis=%d, value=%f, timeHypothesesCreation=%fs", hypothesis.first, hypothesis.second, timeHypothesesCreation);
+			ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
 
-			if(hypothesis.first > 0)
+			if(_highestHypothesis.first > 0)
 			{
 				// Loop closure Threshold
 				// When _loopThr=0, accept loop closure if the hypothesis is over
 				// the virtual (new) place hypothesis.
-				if(hypothesis.second >= _loopThr)
+				if(_highestHypothesis.second >= _loopThr)
 				{
-					//============================================================
+					rejectedHypothesis = true;
-					// Hypothesis verification for loop closure with geometric
+					if(posterior.size() <= 2)
-					// information (like the epipolar geometry or using the local map
-					// associated with the signature)
-					//============================================================
-					if(_lcHypothesisValue && hypothesis.second >= _loopRatio*_lcHypothesisValue &&
-					   (!_epipolarGeometry || _epipolarGeometry->check(signature, _memory->getSignature(hypothesis.first))))
 					{
-						_lcHypothesisId = hypothesis.first;
+						// Ignore loop closure if there is only one loop closure hypothesis
+						UDEBUG("rejected hypothesis: single hypothesis");
 					}
-					else
+					else if(_epipolarGeometry && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
-					{
-						if(_lcHypothesisValue == 0.0f)
-						{
-							UDEBUG("rejected hypothesis: last closure hypothesis is null");
-						}
-						else if(hypothesis.second < _loopRatio*_lcHypothesisValue)
-						{
-							UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
-									hypothesis.second, _loopRatio, _lcHypothesisValue);
-						}
-						else
 					{
 						UWARN("rejected hypothesis: by epipolar geometry");
 					}
-						rejectedHypothesis = true;
+					else if(_loopRatio > 0.0f && _loopClosureHypothesis.second && _highestHypothesis.second < _loopRatio*_loopClosureHypothesis.second)
+					{
+						UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
+								_highestHypothesis.second, _loopRatio, _loopClosureHypothesis.second);
+					}
+					else if(_loopRatio > 0.0f && _loopClosureHypothesis.second == 0)
+					{
+						UDEBUG("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
+					}
+					else
+					{
+						_loopClosureHypothesis = _highestHypothesis;
+						rejectedHypothesis = false;
 					}
 
 					timeHypothesesValidation = timer.ticks();
 					ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
 				}
-				else if(hypothesis.second < _loopRatio*_lcHypothesisValue)
+				else if(_highestHypothesis.second < _loopRatio*_loopClosureHypothesis.second)
 				{
 					// Used for Precision-Recall computation.
 					// When analysing logs, it's convenient to know
@@ -1061,16 +1051,14 @@ bool Rtabmap::process(const SensorData & data)
 					rejectedHypothesis = true;
 				}
 
-				//============================================================
-				// Retrieval id update
-				//============================================================
-				_retrievedId = hypothesis.first;
-
 				//for statistic...
-				hypothesisRatio = _lcHypothesisValue>0?hypothesis.second/_lcHypothesisValue:0;
+				hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
-				_lcHypothesisValue = hypothesis.second;
 
 			}
+			if(_loopClosureHypothesis.first == 0)
+			{
+				_loopClosureHypothesis.second = 0.0f; // reset loop closure value
+			}
 		} // if(_memory->getWorkingMemSize())
 	}// !isBadSignature
 
@@ -1089,22 +1077,24 @@ bool Rtabmap::process(const SensorData & data)
 	}
 
 	//============================================================
-	// RETRIEVAL : Loop closure neighbors reactivation
+	// RETRIEVAL 1/3 : Loop closure neighbors reactivation
 	//============================================================
-	if(_retrievedId > 0 )
+	int retrievalId = _highestHypothesis.first;
+	std::list<int> reactivatedIds;
+	double timeGetNeighborsTimeDb = 0.0;
+	double timeGetNeighborsSpaceDb = 0.0;
+	if(retrievalId > 0 )
 	{
 		//Load neighbors
-		ULOGGER_INFO("Retrieving locations... around id=%d", _retrievedId);
+		ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
 		int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
 		UASSERT(neighborhoodSize >= 0);
 		int margin = neighborhoodSize;
+		ULOGGER_DEBUG("margin=%d maxRetieved=%d", margin, _maxRetrieved);
 
 		UTimer timeGetN;
 		unsigned int nbLoadedFromDb = 0;
 		std::set<int> reactivatedIdsSet;
-		std::list<int> reactivatedIds;
-		double timeGetNeighborsTimeDb = 0.0;
-		double timeGetNeighborsSpaceDb = 0.0;
 		std::map<int, int> neighbors;
 		bool firstPassDone = false;
 		int m = 0;
@@ -1113,12 +1103,13 @@ bool Rtabmap::process(const SensorData & data)
 		// priority in time
 		// Direct neighbors TIME
 		ULOGGER_DEBUG("In TIME");
-		neighbors = _memory->getNeighborsId(_retrievedId,
+		neighbors = _memory->getNeighborsId(retrievalId,
 				margin,
 				_maxRetrieved,
 				true,
 				true,
 				&timeGetNeighborsTimeDb);
+		ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
 		//Priority to locations near in time (direct neighbor) then by space (loop closure)
 		while(m < margin)
 		{
@@ -1163,12 +1154,13 @@ bool Rtabmap::process(const SensorData & data)
 
 		// neighbors SPACE, already added direct neighbors will be ignored
 		ULOGGER_DEBUG("In SPACE");
-		neighbors = _memory->getNeighborsId(_retrievedId,
+		neighbors = _memory->getNeighborsId(retrievalId,
 				margin,
 				_maxRetrieved,
 				true,
 				false,
 				&timeGetNeighborsSpaceDb);
+		ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
 		m = 0;
 		firstPassDone = false;
 		while(m < margin)
@@ -1191,6 +1183,7 @@ bool Rtabmap::process(const SensorData & data)
 						{
 							++nbDirectNeighborsInDb;
 						}
+						UDEBUG("nt=%d m=%d", iter->first, iter->second);
 					}
 					std::map<int, int>::iterator tmp = iter++;
 					neighbors.erase(tmp);
@@ -1219,14 +1212,54 @@ bool Rtabmap::process(const SensorData & data)
 				timeGetNeighborsTimeDb,
 				timeGetNeighborsSpaceDb);
 
+	}
+
+	//============================================================
+	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
+	//============================================================
+	std::list<int> retrievalPathIds;
+	if(_path.size() && _rgbdSlamMode)
+	{
+		updateGoalIndex();
+
+		if(_path.size())
+		{
+			// immunize all nodes between current node and goal node
+			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+			{
+				immunizedLocations.insert(_path[i]);
+				UDEBUG("Path immunization: node %d", _path[i]);
+			}
+			// retrieve nodes after current node up to _maxPathRetrievalSize
+			for(unsigned int i=_pathCurrentIndex;
+				i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes && retrievalPathIds.size() < _maxRetrieved;
+				++i)
+			{
+				if(_memory->getSignature(_path[i]) == 0)
+				{
+					UINFO("retrieval of node %d on path", _path[i]);
+					retrievalPathIds.push_back(_path[i]);
+				}
+			}
+
+			// insert them first to make sure they are loaded.
+			reactivatedIds.insert(reactivatedIds.begin(), retrievalPathIds.begin(), retrievalPathIds.end());
+		}
+	}
+
+	//============================================================
+	// RETRIEVAL 3/3 : Load signatures from the database
+	//============================================================
+	if(reactivatedIds.size())
+	{
 		// Not important if the loop closure hypothesis don't have all its neighbors loaded,
 		// only a loop closure link is added...
 		signaturesRetrieved = _memory->reactivateSignatures(
 				reactivatedIds,
-				_maxRetrieved,
+				_maxRetrieved+retrievalPathIds.size(), // add path retrieved
 				timeRetrievalDbAccess);
 
-		ULOGGER_INFO("retrieval of %d reactivatedIds=%fs (db time = %fs)", (int)signaturesRetrieved.size(), timeGetN.ticks(), timeRetrievalDbAccess);
+		ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
 
 		timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
 		UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
@@ -1242,7 +1275,7 @@ bool Rtabmap::process(const SensorData & data)
 	// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
 	//=============================================================
 	int loopClosureVisualInliers = 0; // for statistics
-	if(_lcHypothesisId>0)
+	if(_loopClosureHypothesis.first>0)
 	{
 		//Compute transform if metric data are present
 		Transform transform;
@@ -1280,7 +1313,7 @@ bool Rtabmap::process(const SensorData & data)
 				// Add signatures
 				SensorData dataFrom = data;
 				dataFrom.setId(signature->id());
-				Signature tmpTo = _memory->getSignatureData(_lcHypothesisId, true);
+				Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true);
 				SensorData dataTo = tmpTo.toSensorData();
 				UDEBUG("timeTo = %fs", timeT.ticks());
 
@@ -1304,16 +1337,16 @@ bool Rtabmap::process(const SensorData & data)
 					// Fallback to normal way (raw data not kept in database...)
 					UWARN("Loop closure: Some images not found in memory for re-extracting "
 						  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
-					transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
+					transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
 				}
 			}
 			else
 			{
-				transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
+				transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
 			}
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
+				Transform icpTransform  = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
 				float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
 				if(!icpTransform.isNull() &&
 					_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1321,7 +1354,7 @@ bool Rtabmap::process(const SensorData & data)
 				{
 					UWARN("Global loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
 							signature->id(),
-							_lcHypothesisId,
+							_loopClosureHypothesis.first,
 							squaredNorm,
 							_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
 					icpTransform.setNull();
@@ -1331,34 +1364,42 @@ bool Rtabmap::process(const SensorData & data)
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
-				UWARN("Cannot compute a loop closure transform between %d and %d: %s", _lcHypothesisId, signature->id(), rejectedMsg.c_str());
+				UWARN("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
 			}
 		}
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, Link::kGlobalClosure, variance);
+			rejectedHypothesis = !_memory->addLoopClosureLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
 		}
 
 		if(rejectedHypothesis)
 		{
-			_lcHypothesisId = 0;
+			_loopClosureHypothesis.first = 0;
 		}
 		else
 		{
-			const Signature * oldS = _memory->getSignature(_lcHypothesisId);
+			const Signature * oldS = _memory->getSignature(_loopClosureHypothesis.first);
 			UASSERT(oldS != 0);
 			// Old map -> new map, used for localization correction on loop closure
 			_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
 		}
 	}
+
+	// Add a virtual loop closure link to keep the path linked to local map
+	if(_path.size() && !signature->hasLink(_path[_pathCurrentIndex]))
+	{
+		Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex]);
+		_memory->addLoopClosureLink(_path[_pathCurrentIndex], signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
+	}
+
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
 
 	int localSpaceDetectionPosesCount = 0;
 	int localSpaceClosureId = 0;
 	int localSpaceNearestId = 0;
-	if(_lcHypothesisId == 0 &&
+	if(_loopClosureHypothesis.first == 0 &&
 	   _localLoopClosureDetectionSpace &&
 	   !signature->getLaserScanCompressed().empty())
 	{
@@ -1371,24 +1412,26 @@ bool Rtabmap::process(const SensorData & data)
 			//============================================================
 			// Scan matching LOCAL LOOP CLOSURE SPACE
 			//============================================================
-			// get all nodes in radius of the current node
+			std::map<int, Transform> localSpacePoses;
-			std::map<int, Transform> poses;
 			localSpaceNearestId = 0;
-			poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
+			localSpacePoses = this->getWMPosesInRadius(
+			signature->id(),
+			_localDetectMaxNeighbors,
+			_localDetectRadius,
+			_localDetectMaxDiffID,
+			localSpaceNearestId);
 
 			// add current node to poses
-			UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
+			localSpacePoses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-			poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+			localSpaceDetectionPosesCount = (int)localSpacePoses.size()-1;
-
-			localSpaceDetectionPosesCount = (int)poses.size()-1;
 			//The nearest will be the reference for a loop closure transform
-			if(poses.size() &&
+			if(localSpacePoses.size() &&
 				localSpaceNearestId &&
 				signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
 			{
 				double variance = 1.0;
 				std::string rejectedMsg;
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg, 0, &variance);
+				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, localSpacePoses, &rejectedMsg, 0, &variance);
 				if(!t.isNull())
 				{
 					localSpaceClosureId = localSpaceNearestId;
@@ -1417,7 +1460,7 @@ bool Rtabmap::process(const SensorData & data)
 	// Optimize map graph
 	//============================================================
 	if(_rgbdSlamMode &&
-		(_lcHypothesisId>0 ||				// can be different map of the current one
+		(_loopClosureHypothesis.first>0 ||				// can be different map of the current one
 		 localLoopClosuresInTimeFound>0 || 	// only same map of the current one
 		 scanMatchingSuccess || 			// only same map of the current one
 		 localSpaceClosureId>0 || 			// can be different map of the current one
@@ -1437,10 +1480,10 @@ bool Rtabmap::process(const SensorData & data)
 				UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
 			}
 		}
-		else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
+		else if(_loopClosureHypothesis.first > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
 		{
 			UINFO("Update map correction: Localization mode");
-			int oldId = _lcHypothesisId>0?_lcHypothesisId:localSpaceClosureId?localSpaceClosureId:_retrievedId;
+			int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first;
 			UASSERT(oldId != 0);
 			if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
 			{
@@ -1475,7 +1518,7 @@ bool Rtabmap::process(const SensorData & data)
 	int lcHypothesisReactivated = 0;
 	float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
 	int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
-	sLoop = _memory->getSignature(_lcHypothesisId?_lcHypothesisId:localSpaceClosureId?localSpaceClosureId:hypothesis.first);
+	sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first);
 	if(sLoop)
 	{
 		lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
@@ -1488,25 +1531,25 @@ bool Rtabmap::process(const SensorData & data)
 	float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
 
 	// prepare statistics
-	if(_lcHypothesisId || _publishStats)
+	if(_loopClosureHypothesis.first || _publishStats)
 	{
 		ULOGGER_INFO("sending stats...");
 		statistics_.setRefImageId(signature->id());
-		if(_lcHypothesisId != Memory::kIdInvalid)
+		if(_loopClosureHypothesis.first != Memory::kIdInvalid)
 		{
-			statistics_.setLoopClosureId(_lcHypothesisId);
+			statistics_.setLoopClosureId(_loopClosureHypothesis.first);
-			ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
+			ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
 		}
 		if(_publishStats)
 		{
 			ULOGGER_INFO("send all stats...");
 			statistics_.setExtended(1);
 
-			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), hypothesis.first);
+			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
-			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), hypothesis.second);
+			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
 			statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
-			statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
+			statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
 
@@ -1528,7 +1571,7 @@ bool Rtabmap::process(const SensorData & data)
 				d = d <= d3?d:d3;
 				statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
 			}
-			if(_lcHypothesisId || localSpaceClosureId)
+			if(_loopClosureHypothesis.first || localSpaceClosureId)
 			{
 				UASSERT(uContains(sLoop->getLinks(), signature->id()));
 				UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
@@ -1539,9 +1582,9 @@ bool Rtabmap::process(const SensorData & data)
 			{
 				std::map<int, int> mapIds;
 				mapIds.insert(std::make_pair(signature->id(), _memory->getMapId(signature->id())));
-				if(_lcHypothesisId)
+				if(_loopClosureHypothesis.first)
 				{
-					mapIds.insert(std::make_pair(_lcHypothesisId, _memory->getMapId(_lcHypothesisId)));
+					mapIds.insert(std::make_pair(_loopClosureHypothesis.first, _memory->getMapId(_loopClosureHypothesis.first)));
 				}
 				statistics_.setMapIds(mapIds);
 			}//else... see finalize statistics below
@@ -1709,7 +1752,7 @@ bool Rtabmap::process(const SensorData & data)
 									timeHypothesesValidation,
 									timeRealTimeLimitReachedProcess,
 									timeStatsCreation,
-									_lcHypothesisValue,
+									_loopClosureHypothesis.second,
 									0.0f,
 									0.0f,
 									0.0f,
@@ -1722,8 +1765,8 @@ bool Rtabmap::process(const SensorData & data)
 									timeRetrievalDbAccess,
 									timeAddLoopClosureLink);
 		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
-									_lcHypothesisId,
+									_loopClosureHypothesis.first,
-									hypothesis.first,
+									_highestHypothesis.first,
 									(int)signaturesRemoved.size(),
 									0,
 									refWordsCount,
@@ -1735,7 +1778,7 @@ bool Rtabmap::process(const SensorData & data)
 									int(signaturesRetrieved.size()),
 									lcHypothesisReactivated,
 									refUniqueWordsCount,
-									_retrievedId,
+									retrievalId,
 									0.0f,
 									rehearsalMaxId,
 									rehearsalMaxId>0?1:0);
@@ -1777,7 +1820,7 @@ void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 		ULOGGER_WARN("maxTimeAllowed < 0, then setting it to 0 (inf).");
 		_maxTimeAllowed = 0;
 	}
-	else if(_maxTimeAllowed < 1)
+	else if(_maxTimeAllowed > 0.0f && _maxTimeAllowed < 1.0f)
 	{
 		ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
 	}
@@ -1819,10 +1862,10 @@ void Rtabmap::deleteLocation(int locationId)
 
 void Rtabmap::rejectLoopClosure(int oldId, int newId)
 {
-	UDEBUG("_lcHypothesisId=%d", _lcHypothesisId);
+	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
-	if(_lcHypothesisId)
+	if(_loopClosureHypothesis.first)
 	{
-		_lcHypothesisId = 0;
+		_loopClosureHypothesis.first = 0;
 		if(_memory)
 		{
 			_memory->rejectLoopClosure(oldId, newId);
@@ -1845,7 +1888,57 @@ void Rtabmap::dumpData() const
 }
 
 // fromId must be in _memory and in _optimizedPoses
-std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
+// return <id, distance>
+std::map<int, float> Rtabmap::getNodesInRadius(
+		int fromId,
+		int maxNearestNeighbors,
+		float radius) 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());
+	std::vector<int> ids(_optimizedPoses.size());
+	int oi = 0;
+	for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+	{
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		ids[oi++] = iter->first;
+	}
+
+	cloud->resize(oi);
+	ids.resize(oi);
+
+	UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
+	Transform fromT = _optimizedPoses.at(fromId);
+
+	std::map<int, float> nodes;
+	if(cloud->size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
+		for(unsigned int i=0; i<ind.size(); ++i)
+		{
+			if(ind[i] >=0)
+			{
+				UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
+				nodes.insert(std::make_pair(ids[ind[i]], dist[i]));
+			}
+		}
+	}
+	UDEBUG("nodes=%d", (int)nodes.size());
+	return nodes;
+}
+
+// fromId must be in _memory and in _optimizedPoses
+// Get poses in front of the robot
+std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		int fromId,
 		int maxNearestNeighbors,
 		float radius,
@@ -1864,7 +1957,10 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 	for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 	{
 		// Only locations in Working Memory with ID not too far from the last loop closure child id
-		bool diffIdOk = maxDiffID == 0 || abs(fromId - iter->first) <= maxDiffID || (abs(iter->first - _memory->getLastGlobalLoopClosureChildId()) <= maxDiffID && abs(fromId - _memory->getLastGlobalLoopClosureParentId()) <= maxDiffID);
+		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());
@@ -1883,9 +1979,13 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 	float minDistance = -1;
 	if(cloud->size())
 	{
-		//filter poses in front of the fromId
+		//if(cloud->size())
-		//pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
+		//{
+		//	pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
+		//	UWARN("Saved radiusPoses.pcd");
+		//}
 
+		//filter poses in front of the fromId
 		Transform t=Transform::getIdentity();
 		t.x() = radius*0.95f;
 		float x,y,z, roll,pitch,yaw;
@@ -1900,7 +2000,11 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 		pcl::IndicesPtr indices(new std::vector<int>());
 		cropbox.filter(*indices);
 
-		//pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
+		//if(indices->size())
+		//{
+		//	pcl::io::savePCDFile("radiusCrop.pcd", *cloud, *indices);
+		//	UWARN("Saved radiusCrop.pcd");
+		//}
 
 		if(indices->size())
 		{
@@ -1940,23 +2044,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 			//	pcl::io::savePCDFile("radiusNearestPt.pcd", c);
 			//}
 
-			if(nearestId > 0)
+			if(nearestId == 0 && poses.size())
-			{
-				// 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();
@@ -2231,6 +2319,188 @@ void Rtabmap::getGraph(
 	}
 }
 
+void Rtabmap::clearPath()
+{
+	_path.clear();
+	_pathCurrentIndex=0;
+	_pathGoalIndex = 0;
+	if(_memory)
+	{
+		_memory->removeAllVirtualLinks();
+	}
+}
+
+// return true if path is updated
+std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
+{
+	this->clearPath();
+	std::list<std::pair<int, Transform> > pathPoses;
+
+	if(!_rgbdSlamMode)
+	{
+		UWARN("A path can only be computed in RGBD-SLAM mode");
+		return pathPoses;
+	}
+
+	if(_memory->getWorkingMem().size() <= 1 || !_memory->getLastWorkingSignature()) // ignore virtual place
+	{
+		UWARN("Working memory is empty... cannot compute a path");
+		return pathPoses;
+	}
+	int currentNode = _memory->getLastWorkingSignature()->id();
+
+	UTimer timer;
+	std::map<int, Transform> globalGraph;
+	std::multimap<int, Link> constraints;
+	std::map<int, int> mapIds;
+	this->getGraph(globalGraph, constraints, mapIds, true, true);
+
+	if(!uContains(globalGraph, currentNode))
+	{
+		UWARN("Last signature %d not found in the global graph! Cannot compute a path", currentNode);
+		return pathPoses;
+	}
+	if(!uContains(globalGraph, targetNode))
+	{
+		UWARN("Goal %d not found in the global graph! Cannot compute a path", targetNode);
+		return pathPoses;
+	}
+
+	std::multimap<int, int> links;
+	for(std::multimap<int, rtabmap::Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
+	{
+		links.insert(std::make_pair(iter->first, iter->second.to()));
+		links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
+	}
+	// Add links between neighbor nodes in the goal radius.
+	//std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(globalGraph, _goalMetricError, CV_PI);
+	//links.insert(clusters.begin(), clusters.end());
+
+	UINFO("Time creating global graph = %fs", timer.ticks());
+
+	UINFO("Computing path from location %d to %d", currentNode, targetNode);
+	_path = rtabmap::computePath(globalGraph, links, currentNode, targetNode);
+	UINFO("Time computing path = %fs", timer.ticks());
+
+	if(_path.size()<2)
+	{
+		_path.clear();
+		UWARN("Cannot compute a path (or goal is already reached)!");
+	}
+	else
+	{
+		UINFO("Path generated! Size=%d", (int)_path.size());
+		if(ULogger::level() == ULogger::kInfo)
+		{
+			std::stringstream stream;
+			for(unsigned int i=0; i<_path.size(); ++i)
+			{
+				stream << _path[i];
+				if(i+1 < _path.size())
+				{
+					stream << " ";
+				}
+			}
+			UINFO("Path = [%s]", stream.str().c_str());
+		}
+	}
+
+	updateGoalIndex();
+
+	for(unsigned int i = 0; i<_path.size(); ++i)
+	{
+		pathPoses.push_back(std::make_pair(_path[i], globalGraph.at(_path[i])));
+	}
+
+	return pathPoses;
+}
+
+int Rtabmap::getPathGoalId() const
+{
+	if(_path.size())
+	{
+		UASSERT(_pathGoalIndex <= _path.size());
+		return _path[_pathGoalIndex];
+	}
+	return 0;
+}
+
+void Rtabmap::updateGoalIndex()
+{
+	UDEBUG("");
+	if(!_rgbdSlamMode)
+	{
+		UWARN("This method can on be used in RGBD-SLAM mode!");
+		return;
+	}
+
+	if(_path.size())
+	{
+		if(_memory->getLastWorkingSignature() == 0 ||
+		   !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
+		{
+			UERROR("Last node is null in memory or not in optimized poses");
+			return;
+		}
+
+		int goalId = _path.back();
+		if(uContains(_optimizedPoses, goalId))
+		{
+			//use local position to know if the goal is reached
+			float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId));
+			if(d < _goalReachedRadius)
+			{
+				UINFO("Goal %d reached!", goalId);
+				this->clearPath();
+			}
+		}
+
+		if(_path.size())
+		{
+			//Always check if the farthest node is accessible in local map
+			int goalIndex = 0;
+			for(int i=(int)_path.size()-1; i>=0; --i)
+			{
+				if(uContains(_optimizedPoses, _path[i]))
+				{
+					goalIndex = i;
+					break;
+				}
+			}
+			UASSERT(_pathGoalIndex <= _path.size() && goalIndex >= 0 && goalIndex <= (int)_path.size());
+			if((int)_pathGoalIndex != goalIndex)
+			{
+				UINFO("Updated current goal from %d to %d (%d/%d)",
+						(int)_path[_pathGoalIndex], _path[goalIndex], goalIndex+1, (int)_path.size());
+				_pathGoalIndex = goalIndex;
+			}
+
+			// update nearest pose in the path
+			unsigned int nearestNodeIndex = 0;
+			float distance = -1.0f;
+			const Transform & currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
+			UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
+			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+			{
+				std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i]);
+				if(iter != _optimizedPoses.end())
+				{
+					float d = currentPose.getDistanceSquared(iter->second);
+					if(distance == -1.0f || distance > d)
+					{
+						distance = d;
+						nearestNodeIndex = i;
+					}
+				}
+			}
+			if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
+			{
+				_pathCurrentIndex = nearestNodeIndex;
+			}
+		}
+	}
+}
+
 void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
 {
 	CSimpleIniA ini;

corelib/src/Signature.cpp

@@ -149,6 +149,21 @@ void Signature::removeLink(int idTo)
 	}
 }
 
+void Signature::removeVirtualLinks()
+{
+	for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
+	{
+		if(iter->second.type() == Link::kVirtualClosure)
+		{
+			_links.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+}
+
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;

examples/BOWMapping/main.cpp

@@ -139,7 +139,7 @@ int main(int argc, char * argv[])
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
 					rtabmap.getLoopClosureId(),
-					rtabmap.getLcHypValue(),
+					rtabmap.getLoopClosureValue(),
 					nextIndex,
 					rtabmap.getLoopClosureId());
 		}
@@ -150,8 +150,8 @@ int main(int argc, char * argv[])
 					rtabmap.getLastProcessTime(),
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
-					rtabmap.getRetrievedId(), // highest loop closure hypothesis
+					rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
-					rtabmap.getLcHypValue());
+					rtabmap.getLoopClosureValue());
 		}
 
 		++nextIndex;

guilib/include/rtabmap/gui/MainWindow.h

@@ -102,6 +102,7 @@ public:
 
 	QString getWorkingDirectory() const;
 	void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
+	bool isSavedMaximized() const {return _savedMaximized;}
 
 public slots:
 	void processStats(const rtabmap::Statistics & stat);
@@ -266,6 +267,7 @@ private:
 	bool _emptyNewDatabase;
 	bool _odomImageShow;
 	bool _odomImageDepthShow;
+	bool _savedMaximized;
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -100,7 +100,8 @@ public:
 	void saveWindowGeometry(const QWidget * window);
 	void loadWindowGeometry(QWidget * window);
 	void saveMainWindowState(const QMainWindow * mainWindow);
-	void loadMainWindowState(QMainWindow * mainWindow);
+	void loadMainWindowState(QMainWindow * mainWindow, bool & maximized);
+
 	void saveWidgetState(const QWidget * widget);
 	void loadWidgetState(QWidget * widget);
 

guilib/src/CMakeLists.txt

@@ -22,6 +22,7 @@ SET(headers_ui
 	./PostProcessingDialog.h
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
+	./GraphViewer.h
 )
 
 SET(uis

guilib/src/GraphViewer.cpp

@@ -101,13 +101,13 @@ class LinkItem: public QGraphicsLineItem
 {
 public:
 	// in meter
-	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, bool loopClosure) :
+	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, Link::Type type) :
 		QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()),
 		_from(from),
 		_to(to),
 		_poseA(poseA),
 		_poseB(poseB),
-		_loopClosure(loopClosure)
+		_type(type)
 	{
 		this->setAcceptHoverEvents(true);
 	}
@@ -126,7 +126,7 @@ public:
 		_poseB = poseB;
 	}
 
-	bool isLoopClosure() const {return _loopClosure;}
+	Link::Type linkType() const {return _type;}
 	int from() const {return _from;}
 	int to() const {return _to;}
 
@@ -146,7 +146,7 @@ private:
 	int _to;
 	Transform _poseA;
 	Transform _poseB;
-	bool _loopClosure;
+	Link::Type _type;
 };
 
 GraphViewer::GraphViewer(QWidget * parent) :
@@ -154,6 +154,9 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_nodeColor(Qt::blue),
 		_neighborColor(Qt::blue),
 		_loopClosureColor(Qt::red),
+		_loopClosureLocalColor(Qt::yellow),
+		_loopClosureUserColor(Qt::red),
+		_loopClosureVirtualColor(Qt::magenta),
 		_root(0),
 		_nodeRadius(0.01),
 		_linkWidth(0),
@@ -192,6 +195,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_gridMap = this->scene()->addPixmap(QPixmap());
 	_gridMap->setZValue(0);
 	_gridMap->setParentItem(_root);
+
+	this->restoreDefaults();
 }
 
 GraphViewer::~GraphViewer()
@@ -286,11 +291,30 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			if(!added)
 			{
 				//create a link item
-				LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, loopClosure);
+				LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, iter->second.type());
 				QPen p = item->pen();
 				p.setWidthF(_linkWidth);
 				item->setPen(p);
-				item->setColor(loopClosure?_loopClosureColor:_neighborColor);
+				if(iter->second.type() == Link::kNeighbor)
+				{
+					item->setColor(_neighborColor);
+				}
+				else if(iter->second.type() == Link::kVirtualClosure)
+				{
+					item->setColor(_loopClosureVirtualColor);
+				}
+				else if(iter->second.type() == Link::kUserClosure)
+				{
+					item->setColor(_loopClosureUserColor);
+				}
+				else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
+				{
+					item->setColor(_loopClosureLocalColor);
+				}
+				else
+				{
+					item->setColor(_loopClosureColor);
+				}
 				this->scene()->addItem(item);
 				item->setZValue(1);
 				item->setParentItem(_root);
@@ -438,6 +462,144 @@ void GraphViewer::clearAll()
 	clearGraph();
 }
 
+bool GraphViewer::isGridMapVisible() const
+{
+	return _gridMap->isVisible();
+}
+
+void GraphViewer::setWorkingDirectory(const QString & path)
+{
+	_workingDirectory = path;
+}
+void GraphViewer::setNodeRadius(float radius)
+{
+	_nodeRadius = radius;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
+		if(node)
+		{
+			node->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
+		}
+	}
+}
+void GraphViewer::setLinkWidth(float width)
+{
+	_linkWidth = width;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
+		if(line)
+		{
+			QPen pen = line->pen();
+			pen.setWidthF(_linkWidth);
+			line->setPen(pen);
+		}
+	}
+}
+void GraphViewer::setNodeColor(const QColor & color)
+{
+	_nodeColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
+		if(node)
+		{
+			node->setColor(_nodeColor);
+		}
+	}
+}
+void GraphViewer::setNeighborColor(const QColor & color)
+{
+	_neighborColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+		if(link && link->linkType() == Link::kNeighbor)
+		{
+			link->setColor(_neighborColor);
+		}
+	}
+}
+void GraphViewer::setGlobalLoopClosureColor(const QColor & color)
+{
+	_loopClosureColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+		if(link && (link->linkType() != Link::kNeighbor &&
+				link->linkType() != Link::kLocalSpaceClosure &&
+				link->linkType() != Link::kLocalTimeClosure &&
+				link->linkType() != Link::kUserClosure &&
+				link->linkType() != Link::kVirtualClosure))
+		{
+			link->setColor(_loopClosureColor);
+		}
+	}
+}
+void GraphViewer::setLocalLoopClosureColor(const QColor & color)
+{
+	_loopClosureLocalColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+		if(link && (link->linkType() == Link::kLocalSpaceClosure || link->linkType() == Link::kLocalSpaceClosure))
+		{
+			link->setColor(_loopClosureLocalColor);
+		}
+	}
+}
+void GraphViewer::setUserLoopClosureColor(const QColor & color)
+{
+	_loopClosureUserColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+		if(link && link->linkType() == Link::kUserClosure)
+		{
+			link->setColor(_loopClosureUserColor);
+		}
+	}
+}
+void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
+{
+	_loopClosureVirtualColor = color;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+		if(link && link->linkType() == Link::kVirtualClosure)
+		{
+			link->setColor(_loopClosureVirtualColor);
+		}
+	}
+}
+void GraphViewer::setGridMapVisible(bool visible)
+{
+	_gridMap->setVisible(visible);
+}
+
+void GraphViewer::restoreDefaults()
+{
+	setNodeRadius(0.01f);
+	setLinkWidth(0.0f);
+	setNodeColor(Qt::blue);
+	setNeighborColor(Qt::blue);
+	setGlobalLoopClosureColor(Qt::red);
+	setLocalLoopClosureColor(Qt::yellow);
+	setUserLoopClosureColor(Qt::red);
+	setVirtualLoopClosureColor(Qt::magenta);
+	setGridMapVisible(true);
+}
+
+
 void GraphViewer::wheelEvent ( QWheelEvent * event )
 {
 	if(event->delta() > 0)
@@ -450,15 +612,41 @@ void GraphViewer::wheelEvent ( QWheelEvent * event )
 	}
 }
 
+QIcon createIcon(const QColor & color)
+{
+	QPixmap pixmap(50, 50);
+	pixmap.fill(color);
+	return QIcon(pixmap);
+}
+
 void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QMenu menu;
 	QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
 	QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
 	menu.addSeparator();
-	QAction * aChangeNodeColor = menu.addAction(tr("Set node color..."));
+
-	QAction * aChangeNeighborColor = menu.addAction(tr("Set neighbor link color..."));
+	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
-	QAction * aChangeLoopColor = menu.addAction(tr("Set loop closure link color..."));
+	aChangeNodeColor->setIconVisibleInMenu(true);
+
+	// Links
+	QMenu * menuLink = menu.addMenu(tr("Set link color..."));
+	QAction * aChangeNeighborColor = menuLink->addAction(tr("Neighbor"));
+	QAction * aChangeGlobalLoopColor = menuLink->addAction(tr("Global loop closure"));
+	QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
+	QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
+	QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
+	aChangeNeighborColor->setIcon(createIcon(_neighborColor));
+	aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
+	aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
+	aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
+	aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
+	aChangeNeighborColor->setIconVisibleInMenu(true);
+	aChangeGlobalLoopColor->setIconVisibleInMenu(true);
+	aChangeLocalLoopColor->setIconVisibleInMenu(true);
+	aChangeUserLoopColor->setIconVisibleInMenu(true);
+	aChangeVirtualLoopColor->setIconVisibleInMenu(true);
+
 	menu.addSeparator();
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
 	QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
@@ -472,6 +660,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideGridMap = menu.addAction(tr("Show grid map"));
 	}
+	menu.addSeparator();
+	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
 	QAction * r = menu.exec(event->globalPos());
 	if(r == aScreenShotPNG || r == aScreenShotSVG)
@@ -535,57 +725,73 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 
 			QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
 		}
+		return; // without emitting configChanged
 	}
 	else if(r == aChangeNodeColor ||
 			r == aChangeNeighborColor ||
-			r == aChangeLoopColor)
+			r == aChangeGlobalLoopColor ||
+			r == aChangeLocalLoopColor ||
+			r == aChangeUserLoopColor ||
+			r == aChangeVirtualLoopColor)
 	{
 		QColor color;
 		if(r == aChangeNodeColor)
 		{
 			color = _nodeColor;
 		}
-		else if(r == aChangeNeighborColor)
+		else if(r == aChangeGlobalLoopColor)
-		{
-			color = _neighborColor;
-		}
-		else if(r == aChangeLoopColor)
 		{
 			color = _loopClosureColor;
 		}
+		else if(r == aChangeLocalLoopColor)
+		{
+			color = _loopClosureLocalColor;
+		}
+		else if(r == aChangeUserLoopColor)
+		{
+			color = _loopClosureUserColor;
+		}
+		else if(r == aChangeVirtualLoopColor)
+		{
+			color = _loopClosureVirtualColor;
+		}
+		else //if(r == aChangeNeighborColor)
+		{
+			color = _neighborColor;
+		}
 		color = QColorDialog::getColor(color, this);
+		if(color.isValid())
+		{
 
 			if(r == aChangeNodeColor)
 			{
-			_nodeColor = color;
+				this->setNodeColor(color);
 			}
-		else if(r == aChangeNeighborColor)
+			else if(r == aChangeGlobalLoopColor)
 			{
-			_neighborColor = color;
+				this->setGlobalLoopClosureColor(color);
 			}
-		else if(r == aChangeLoopColor)
+			else if(r == aChangeLocalLoopColor)
 			{
-			_loopClosureColor = color;
+				this->setLocalLoopClosureColor(color);
 			}
-
+			else if(r == aChangeUserLoopColor)
-		QList<QGraphicsItem*> items = this->scene()->items();
-		for(int i=0; i<items.size(); ++i)
 			{
-			LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
+				this->setUserLoopClosureColor(color);
-			NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
-			if(r == aChangeNodeColor && node)
-			{
-				node->setColor(color);
 			}
-			else if(r == aChangeNeighborColor && link && !link->isLoopClosure())
+			else if(r == aChangeVirtualLoopColor)
 			{
-				link->setColor(color);
+				this->setVirtualLoopClosureColor(color);
 			}
-			else if(r == aChangeLoopColor && link && link->isLoopClosure())
+			else //if(r == aChangeNeighborColor)
 			{
-				link->setColor(color);
+				this->setNeighborColor(color);
 			}
 		}
+		else
+		{
+			return; // without emitting configChanged
+		}
 	}
 	else if(r == aSetNodeSize)
 	{
@@ -593,16 +799,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 100, 2, &ok);
 		if(ok)
 		{
-			_nodeRadius = value;
+			setNodeRadius(value);
-			QList<QGraphicsItem*> items = this->scene()->items();
-			for(int i=0; i<items.size(); ++i)
-			{
-				NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
-				if(node)
-				{
-					node->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
-				}
-			}
 		}
 	}
 	else if(r == aSetLinkSize)
@@ -611,23 +808,20 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 100, 2, &ok);
 		if(ok)
 		{
-			_linkWidth = value;
+			setLinkWidth(value);
-			QList<QGraphicsItem*> items = this->scene()->items();
-			for(int i=0; i<items.size(); ++i)
-			{
-				QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
-				if(line)
-				{
-					QPen pen = line->pen();
-					pen.setWidthF(_linkWidth);
-					line->setPen(pen);
-				}
-			}
 		}
 	}
 	else if(r == aShowHideGridMap)
 	{
-		_gridMap->setVisible(!_gridMap->isVisible());
+		this->setGridMapVisible(!this->isGridMapVisible());
+	}
+	else if(r == aRestoreDefaults)
+	{
+		this->restoreDefaults();
+	}
+	if(r)
+	{
+		emit configChanged();
 	}
 }
 

guilib/src/GraphViewer.h

@@ -44,6 +44,9 @@ class NodeItem;
 class LinkItem;
 
 class GraphViewer : public QGraphicsView {
+
+	Q_OBJECT;
+
 public:
 	GraphViewer(QWidget * parent = 0);
 	virtual ~GraphViewer();
@@ -57,7 +60,35 @@ public:
 	void clearPosterior();
 	void clearAll();
 
-	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
+	//getters
+	const QString & getWorkingDirectory() const {return _workingDirectory;}
+	float getNodeRadius() const {return _nodeRadius;}
+	float getLinkWidth() const {return _linkWidth;}
+	const QColor & getNodeColor() const {return _nodeColor;}
+	const QColor & getNeighborColor() const {return _neighborColor;}
+	const QColor & getGlobalLoopClosureColor() const {return _loopClosureColor;}
+	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
+	const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
+	const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
+	bool isGridMapVisible() const;
+
+	// setters
+	void setWorkingDirectory(const QString & path);
+	void setNodeRadius(float radius);
+	void setLinkWidth(float width);
+	void setNodeColor(const QColor & color);
+	void setNeighborColor(const QColor & color);
+	void setGlobalLoopClosureColor(const QColor & color);
+	void setLocalLoopClosureColor(const QColor & color);
+	void setUserLoopClosureColor(const QColor & color);
+	void setVirtualLoopClosureColor(const QColor & color);
+	void setGridMapVisible(bool visible);
+
+signals:
+	void configChanged();
+
+public slots:
+	void restoreDefaults();
 
 protected:
 	virtual void wheelEvent ( QWheelEvent * event );
@@ -68,6 +99,9 @@ private:
 	QColor _nodeColor;
 	QColor _neighborColor;
 	QColor _loopClosureColor;
+	QColor _loopClosureLocalColor;
+	QColor _loopClosureUserColor;
+	QColor _loopClosureVirtualColor;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _neighborLinkItems;

guilib/src/MainWindow.cpp

@@ -188,7 +188,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_preferencesDialog->init();
 
 	// Restore window geometry
-	_preferencesDialog->loadMainWindowState(this);
+	_preferencesDialog->loadMainWindowState(this, _savedMaximized);
 	_preferencesDialog->loadWindowGeometry(_preferencesDialog);
 	_preferencesDialog->loadWindowGeometry(_exportDialog);
 	_preferencesDialog->loadWindowGeometry(_postProcessingDialog);
@@ -212,6 +212,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_preferencesDialog->loadWidgetState(_ui->imageView_source);
 	_preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
 	_preferencesDialog->loadWidgetState(_ui->imageView_odometry);
+	_preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
 
 	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
@@ -366,6 +367,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 	connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 	connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 	connect(_ui->widget_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 	connect(_exportDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 	connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
@@ -1369,7 +1371,7 @@ void MainWindow::updateMapCloud(
 			QColor color = Qt::gray;
 			if(iter->first >= 0)
 			{
-				color = (Qt::GlobalColor)(iter->first % 12 + 7 );
+				color = (Qt::GlobalColor)((iter->first+2) % 12 + 7 );
 			}
 			_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
@@ -2212,6 +2214,7 @@ void MainWindow::configGUIModified()
 //ACTIONS
 void MainWindow::saveConfigGUI()
 {
+	_savedMaximized = this->isMaximized();
 	_preferencesDialog->saveMainWindowState(this);
 	_preferencesDialog->saveWindowGeometry(_preferencesDialog);
 	_preferencesDialog->saveWindowGeometry(_aboutDialog);
@@ -2221,6 +2224,7 @@ void MainWindow::saveConfigGUI()
 	_preferencesDialog->saveWidgetState(_ui->imageView_odometry);
 	_preferencesDialog->saveWidgetState(_exportDialog);
 	_preferencesDialog->saveWidgetState(_postProcessingDialog);
+	_preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
 	this->setWindowModified(false);
 }
 

guilib/src/PreferencesDialog.cpp

@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/DataRecorder.h"
 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/gui/ImageView.h"
+#include "GraphViewer.h"
 #include "ExportCloudsDialog.h"
 #include "PostProcessingDialog.h"
 
@@ -448,6 +449,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
 	_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 
+	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
+	_ui->graphPlan_maxAnticipatedNodes->setObjectName(Parameters::kRGBDMaxAnticipatedNodes().c_str());
+
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalLoopDetectionRadius().c_str());
@@ -1629,9 +1633,7 @@ void PreferencesDialog::readSettingsEnd()
 
 void PreferencesDialog::saveWindowGeometry(const QWidget * window)
 {
-	if(!window->objectName().isNull())
+	if(!window->objectName().isNull() && !window->isMaximized())
-	{
-		if(!window->isMaximized())
 	{
 		QSettings settings(getIniFilePath(), QSettings::IniFormat);
 		settings.beginGroup("Gui");
@@ -1640,7 +1642,6 @@ void PreferencesDialog::saveWindowGeometry(const QWidget * window)
 		settings.endGroup(); // "windowName"
 		settings.endGroup(); // rtabmap
 	}
-	}
 }
 
 void PreferencesDialog::loadWindowGeometry(QWidget * window)
@@ -1671,12 +1672,13 @@ void PreferencesDialog::saveMainWindowState(const QMainWindow * mainWindow)
 		settings.beginGroup("Gui");
 		settings.beginGroup(mainWindow->objectName());
 		settings.setValue("state", mainWindow->saveState());
+		settings.setValue("maximized", mainWindow->isMaximized());
 		settings.endGroup(); // "MainWindow"
 		settings.endGroup(); // rtabmap
 	}
 }
 
-void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow)
+void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow,  bool & maximized)
 {
 	if(!mainWindow->objectName().isNull())
 	{
@@ -1691,6 +1693,7 @@ void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow)
 		{
 			mainWindow->restoreState(bytes);
 		}
+		maximized = settings.value("maximized", false).toBool();
 		settings.endGroup(); // "MainWindow"
 		settings.endGroup(); // rtabmap
 	}
@@ -1708,6 +1711,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
 		const ImageView * imageView = qobject_cast<const ImageView*>(widget);
 		const ExportCloudsDialog * exportCloudsDialog = qobject_cast<const ExportCloudsDialog*>(widget);
 		const PostProcessingDialog * postProcessingDialog = qobject_cast<const PostProcessingDialog *>(widget);
+		const GraphViewer * graphViewer = qobject_cast<const GraphViewer *>(widget);
 
 		if(cloudViewer)
 		{
@@ -1781,6 +1785,18 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
 			settings.setValue("refine_neigbors", postProcessingDialog->isRefineNeighborLinks());
 			settings.setValue("refine_lc", postProcessingDialog->isRefineLoopClosureLinks());
 		}
+		else if(graphViewer)
+		{
+			settings.setValue("node_radius", graphViewer->getNodeRadius());
+			settings.setValue("link_width", graphViewer->getLinkWidth());
+			settings.setValue("node_color", graphViewer->getNodeColor());
+			settings.setValue("neighbor_color", graphViewer->getNeighborColor());
+			settings.setValue("global_color", graphViewer->getGlobalLoopClosureColor());
+			settings.setValue("local_color", graphViewer->getLocalLoopClosureColor());
+			settings.setValue("user_color", graphViewer->getUserLoopClosureColor());
+			settings.setValue("virtual_color", graphViewer->getVirtualLoopClosureColor());
+			settings.setValue("grid_visible", graphViewer->isGridMapVisible());
+		}
 		else
 		{
 			UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
@@ -1804,6 +1820,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
 		ImageView * imageView = qobject_cast<ImageView*>(widget);
 		ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
 		PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
+		GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
 
 		if(cloudViewer)
 		{
@@ -1865,6 +1882,18 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
 			postProcessingDialog->setRefineNeighborLinks(settings.value("refine_neigbors", postProcessingDialog->isRefineNeighborLinks()).toBool());
 			postProcessingDialog->setRefineLoopClosureLinks(settings.value("refine_lc", postProcessingDialog->isRefineLoopClosureLinks()).toBool());
 		}
+		else if(graphViewer)
+		{
+			graphViewer->setNodeRadius(settings.value("node_radius", graphViewer->getNodeRadius()).toDouble());
+			graphViewer->setLinkWidth(settings.value("link_width", graphViewer->getLinkWidth()).toDouble());
+			graphViewer->setNodeColor(settings.value("node_color", graphViewer->getNodeColor()).value<QColor>());
+			graphViewer->setNeighborColor(settings.value("neighbor_color", graphViewer->getNeighborColor()).value<QColor>());
+			graphViewer->setGlobalLoopClosureColor(settings.value("global_color", graphViewer->getGlobalLoopClosureColor()).value<QColor>());
+			graphViewer->setLocalLoopClosureColor(settings.value("local_color", graphViewer->getLocalLoopClosureColor()).value<QColor>());
+			graphViewer->setUserLoopClosureColor(settings.value("user_color", graphViewer->getUserLoopClosureColor()).value<QColor>());
+			graphViewer->setVirtualLoopClosureColor(settings.value("virtual_color", graphViewer->getVirtualLoopClosureColor()).value<QColor>());
+			graphViewer->setGridMapVisible(settings.value("grid_visible", graphViewer->isGridMapVisible()).toBool());
+		}
 		else
 		{
 			UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-693</y>
             <width>744</width>
-            <height>1056</height>
+            <height>1378</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4900,7 +4900,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="title">
                        <string>Map update</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_20">
+                      <layout class="QGridLayout" name="gridLayout_47" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
                          <property name="suffix">
@@ -4950,16 +4950,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_163">
-                         <property name="text">
-                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
                          <property name="suffix">
@@ -4979,10 +4969,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_scanMatching">
+                        <widget class="QLabel" name="label_163">
                          <property name="text">
-                          <string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
+                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -4996,6 +4986,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_scanMatching">
+                         <property name="text">
+                          <string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5054,10 +5054,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </layout>
                        </item>
                        <item>
-                        <layout class="QFormLayout" name="formLayout_5">
+                        <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
-                         <property name="fieldGrowthPolicy">
-                          <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                         </property>
                          <item row="0" column="0">
                           <widget class="QComboBox" name="globalDetection_icpType">
                            <property name="sizeAdjustPolicy">
@@ -5126,7 +5123,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="title">
                        <string>Graph optimization</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_19">
+                      <layout class="QGridLayout" name="gridLayout_49" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="globalDetection_toroIterations">
                          <property name="minimum">
@@ -5147,13 +5144,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="1" column="0">
-                        <widget class="QLabel" name="label_151">
+                        <widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
                          <property name="text">
-                          <string>Optimize graph from the newest node.
+                          <string/>
--If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
+                         </property>
--If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
+                        </widget>
-Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_141">
+                         <property name="text">
+                          <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5167,17 +5168,13 @@ Warning when set to false: when some nodes are transferred, the first referentia
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
+                       <item row="2" column="1">
-                        <widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
+                        <widget class="QLabel" name="label_151">
                          <property name="text">
-                          <string/>
+                          <string>Optimize graph from the newest node.
-                         </property>
+-If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
-                        </widget>
+-If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
-                       </item>
+Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_141">
-                         <property name="text">
-                          <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5229,7 +5226,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
                         </widget>
                        </item>
                        <item>
-                        <layout class="QFormLayout" name="formLayout_13">
+                        <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
                          <item row="0" column="0">
                           <widget class="QDoubleSpinBox" name="localDetection_radius">
                            <property name="suffix">
@@ -5267,16 +5264,6 @@ Warning when set to false: when some nodes are transferred, the first referentia
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_space3_2">
-                           <property name="text">
-                            <string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="0">
                           <widget class="QSpinBox" name="localDetection_maxDiffID">
                            <property name="maximum">
@@ -5287,11 +5274,70 @@ Warning when set to false: when some nodes are transferred, the first referentia
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_space3_2">
+                           <property name="text">
+                            <string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
                      </widget>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_12">
+                      <property name="title">
+                       <string>Graph planning</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_51" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QDoubleSpinBox" name="graphPlan_goalReachedRadius">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_space2_2">
+                         <property name="text">
+                          <string>Goal reached radius.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="graphPlan_maxAnticipatedNodes">
+                         <property name="maximum">
+                          <number>999999999</number>
+                         </property>
+                         <property name="value">
+                          <number>10</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_space3_3">
+                         <property name="text">
+                          <string>Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by &quot;Maximum locations retrieved&quot; from the Memory panel).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>

tools/ConsoleApp/main.cpp

@@ -502,12 +502,12 @@ int main(int argc, char * argv[])
 			if(rtabmap.getLoopClosureId())
 			{
 				printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
-						count, rtabmap.getLoopClosureId(), rtabmap.getLcHypValue(), rtabmapTime, iterationTime);
+						count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
 			}
-			else if(rtabmap.getRetrievedId())
+			else if(rtabmap.getHighestHypothesisId())
 			{
 				printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
-						count, rtabmap.getRetrievedId(), rtabmap.getLcHypValue(), rtabmapTime, iterationTime);
+						count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
 			}
 			else
 			{