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

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 getRetrievedId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
+	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;
-	float _lcHypothesisValue;
-	int _retrievedId;
+	std::pair<int, float> _loopClosureHypothesis;
+	std::pair<int, float> _highestHypothesis;
 	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;
 		}
 
-		_memoryChanged = 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),
-	_lcHypothesisValue(0),
-	_retrievedId(0),
+	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
+	_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
+	_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;
-	_lcHypothesisValue = 0;
-	_lcHypothesisId = 0;
+	UINFO("");
+	_highestHypothesis = std::make_pair(0,0.0f);
+	_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;
-	_lcHypothesisValue = 0;
-	_lcHypothesisId = 0;
+	_highestHypothesis = std::make_pair(0,0.0f);
+	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_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;
-						hypothesis.second = iter->second;
+						_highestHypothesis = *iter;
 					}
 				}
 				// 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)
 				{
-					//============================================================
-					// Hypothesis verification for loop closure with geometric
-					// 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))))
+					rejectedHypothesis = true;
+					if(posterior.size() <= 2)
 					{
-						_lcHypothesisId = hypothesis.first;
+						// Ignore loop closure if there is only one loop closure hypothesis
+						UDEBUG("rejected hypothesis: single hypothesis");
+					}
+					else if(_epipolarGeometry && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
+					{
+						UWARN("rejected hypothesis: by epipolar geometry");
+					}
+					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
 					{
-						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;
+						_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;
-				_lcHypothesisValue = hypothesis.second;
+				hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
 
 			}
+			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> poses;
+			std::map<int, Transform> localSpacePoses;
 			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());
-			poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-
-			localSpaceDetectionPosesCount = (int)poses.size()-1;
+			localSpacePoses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+			localSpaceDetectionPosesCount = (int)localSpacePoses.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);
-			ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
+			statistics_.setLoopClosureId(_loopClosureHypothesis.first);
+			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_value(), hypothesis.second);
+			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
+			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,
-									hypothesis.first,
+									_loopClosureHypothesis.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);
-	if(_lcHypothesisId)
+	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
+	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
-		//pcl::io::savePCDFile("radiusPoses.pcd", *cloud);
+		//if(cloud->size())
+		//{
+		//	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)
-			{
-				// 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())
+			if(nearestId == 0 && 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;