Using Dijkstra for global planning for a significative performance boost (no need to optimize the graph before computing the path)

corelib/include/rtabmap/core/DBDriver.h

@@ -101,6 +101,7 @@ public:
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
 	void getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
@@ -137,6 +138,7 @@ private:
 	virtual void getNodeDataQuery(int signatureId, SensorData & data) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;

corelib/include/rtabmap/core/Graph.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 
 namespace rtabmap {
+class Memory;
 
 namespace graph {
 
@@ -198,6 +199,22 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 			int to,
 			bool updateNewCosts = false);
 
+/**
+ * Perform Dijkstra path planning in the graph.
+ * @param fromId initial node
+ * @param toId final node
+ * @param memory The graph's memory
+ * @param lookInDatabase check links in database
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
+ */
+std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
+		int fromId,
+		int toId,
+		const Memory * memory,
+		bool lookInDatabase = true,
+		bool updateNewCosts = false);
+
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);

corelib/include/rtabmap/core/Memory.h

@@ -115,6 +115,9 @@ public:
 			bool lookInDatabase = false) const;
 	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
+	std::map<int, Link> getLinks(int signatureId,
+			bool lookInDatabase = false) const;
+	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
 	bool isRawDataKept() const {return _rawDataKept;}
 	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}

corelib/include/rtabmap/core/Parameters.h

@@ -291,7 +291,7 @@ class RTABMAP_EXP Parameters
 	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, 0.5, "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
-	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
+	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
 	RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -129,7 +129,7 @@ public:
 			std::map<int, Signature> * signatures = 0);
 	void clearPath();
 	bool computePath(int targetNode, bool global);
-	bool computePath(const Transform & targetPose, bool global);
+	bool computePath(const Transform & targetPose); // only in current optimized map
 	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;

corelib/src/DBDriver.cpp

@@ -556,6 +556,33 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks) const
+{
+	_dbSafeAccessMutex.lock();
+	this->getAllLinksQuery(links, ignoreNullLinks);
+	_dbSafeAccessMutex.unlock();
+
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
+		{
+			links.erase(iter->first);
+			for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
+				jter!=iter->second->getLinks().end();
+				++jter)
+			{
+				if(!ignoreNullLinks || jter->second.isValid())
+				{
+					links.insert(std::make_pair(iter->first, jter->second));
+				}
+			}
+		}
+	}
+	_trashesMutex.unlock();
+}
+
 void DBDriver::getLastNodeId(int & id) const
 {
 	// look in the trash

corelib/src/DBDriverSqlite3.cpp

@@ -1055,6 +1055,95 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 	}
 }
 
+void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const
+{
+	links.clear();
+	if(_ppDb)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		{
+			query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
+		}
+		else if(uStrNumCmp(_version, "0.7.4") >= 0)
+		{
+			query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id";
+		}
+		else
+		{
+			query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, to_id";
+		}
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		int fromId = -1;
+		int toId = -1;
+		int type = Link::kUndef;
+		float rotVariance = 1.0f;
+		float transVariance = 1.0f;
+		const void * data = 0;
+		int dataSize = 0;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int index = 0;
+
+			fromId = sqlite3_column_int(ppStmt, index++);
+			toId = sqlite3_column_int(ppStmt, index++);
+			type = sqlite3_column_int(ppStmt, index++);
+
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+
+			Transform transform;
+			if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
+			{
+				memcpy(transform.data(), data, dataSize);
+			}
+			else if(dataSize)
+			{
+				UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId);
+			}
+
+			if(!ignoreNullLinks || !transform.isNull())
+			{
+				if(uStrNumCmp(_version, "0.8.4") >= 0)
+				{
+					rotVariance = sqlite3_column_double(ppStmt, index++);
+					transVariance = sqlite3_column_double(ppStmt, index++);
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+				}
+				else if(uStrNumCmp(_version, "0.7.4") >= 0)
+				{
+					rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+				}
+				else
+				{
+					// neighbor is 0, loop closures are 1 and 2 (child)
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
+				}
+			}
+
+			rc = sqlite3_step(ppStmt);
+		}
+
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+}
+
 void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id) const
 {
 	if(_ppDb)

corelib/src/DBDriverSqlite3.h

@@ -74,6 +74,7 @@ private:
 	virtual void getNodeDataQuery(int signatureId, SensorData & data) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;

corelib/src/Graph.cpp

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Memory.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
@@ -1258,6 +1260,127 @@ std::list<std::pair<int, Transform> > computePath(
 	return path;
 }
 
+
+// return path starting from "fromId" (Identity pose for the first node)
+std::list<std::pair<int, Transform> > computePath(
+		int fromId,
+		int toId,
+		const Memory * memory,
+		bool lookInDatabase,
+		bool updateNewCosts)
+{
+	UASSERT(memory!=0);
+	UASSERT(fromId>=0);
+	UASSERT(toId>=0);
+	std::list<std::pair<int, Transform> > path;
+
+	std::multimap<int, Link> allLinks;
+	if(lookInDatabase)
+	{
+		// Faster to load all links in one query
+		//UTimer t;
+		allLinks = memory->getAllLinks(lookInDatabase);
+		//UWARN("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
+	}
+
+	//dijkstra
+	int startNode = fromId;
+	int endNode = toId;
+	std::map<int, Node> nodes;
+	nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity())));
+	std::priority_queue<Pair, std::vector<Pair>, Order> pq;
+	std::multimap<float, int> pqmap;
+	if(updateNewCosts)
+	{
+		pqmap.insert(std::make_pair(0, startNode));
+	}
+	else
+	{
+		pq.push(Pair(startNode, 0));
+	}
+
+	while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
+	{
+		Node * currentNode;
+		if(updateNewCosts)
+		{
+			currentNode = &nodes.find(pqmap.begin()->second)->second;
+			pqmap.erase(pqmap.begin());
+		}
+		else
+		{
+			currentNode = &nodes.find(pq.top().first)->second;
+			pq.pop();
+		}
+
+		currentNode->setClosed(true);
+
+		if(currentNode->id() == endNode)
+		{
+			while(currentNode->id()!=startNode)
+			{
+				path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
+				currentNode = &nodes.find(currentNode->fromId())->second;
+			}
+			path.push_front(std::make_pair(startNode, currentNode->pose()));
+			break;
+		}
+
+		// lookup neighbors
+		std::map<int, Link> links;
+		if(allLinks.size() == 0)
+		{
+			links = memory->getLinks(currentNode->id(), lookInDatabase);
+		}
+		else
+		{
+			for(std::multimap<int, Link>::const_iterator iter = allLinks.lower_bound(currentNode->id());
+				iter!=allLinks.end() && iter->first == currentNode->id();
+				++iter)
+			{
+				links.insert(std::make_pair(iter->second.to(), iter->second));
+			}
+		}
+		for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+		{
+			std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
+			if(nodeIter == nodes.end())
+			{
+				Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform());
+				n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm());
+				nodes.insert(std::make_pair(iter->second.to(), n));
+				if(updateNewCosts)
+				{
+					pqmap.insert(std::make_pair(n.totalCost(), n.id()));
+				}
+				else
+				{
+					pq.push(Pair(n.id(), n.totalCost()));
+				}
+			}
+			else if(updateNewCosts && nodeIter->second.isOpened())
+			{
+				float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
+				if(nodeIter->second.costSoFar() > newCostSoFar)
+				{
+					// update the cost in the priority queue
+					for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
+					{
+						if(mapIter->second == nodeIter->first)
+						{
+							pqmap.erase(mapIter);
+							nodeIter->second.setCostSoFar(newCostSoFar);
+							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
+							break;
+						}
+					}
+				}
+			}
+		}
+	}
+	return path;
+}
+
 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose)

corelib/src/Memory.cpp

@@ -876,6 +876,54 @@ std::map<int, Link> Memory::getLoopClosureLinks(
 	return loopClosures;
 }
 
+std::map<int, Link> Memory::getLinks(
+		int signatureId,
+		bool lookInDatabase) const
+{
+	std::map<int, Link> links;
+	Signature * s = uValue(_signatures, signatureId, (Signature*)0);
+	if(s)
+	{
+		links = s->getLinks();
+	}
+	else if(lookInDatabase && _dbDriver)
+	{
+		_dbDriver->loadLinks(signatureId, links, Link::kUndef);
+	}
+	else
+	{
+		UWARN("Cannot find signature %d in memory", signatureId);
+	}
+	return links;
+}
+
+std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
+{
+	std::multimap<int, Link> links;
+
+	if(lookInDatabase && _dbDriver)
+	{
+		_dbDriver->getAllLinks(links, ignoreNullLinks);
+	}
+
+	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		links.erase(iter->first);
+		for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
+			jter!=iter->second->getLinks().end();
+			++jter)
+		{
+			if(!ignoreNullLinks || jter->second.isValid())
+			{
+				links.insert(std::make_pair(iter->first, jter->second));
+			}
+		}
+	}
+
+	return links;
+}
+
+
 // return map<Id,Margin>, including signatureId
 // maxCheckedInDatabase = -1 means no limit to check in database (default)
 // maxCheckedInDatabase = 0 means don't check in database

corelib/src/Rtabmap.cpp

@@ -2965,14 +2965,25 @@ void Rtabmap::clearPath()
 	}
 }
 
-bool Rtabmap::computePath(
-		int targetNode,
-		std::map<int, Transform> nodes,
-		const std::multimap<int, rtabmap::Link> & constraints)
+// return true if path is updated
+bool Rtabmap::computePath(int targetNode, bool global)
 {
+	UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
+	this->clearPath();
+
+	if(!_rgbdSlamMode)
+	{
+		UWARN("A path can only be computed in RGBD-SLAM mode");
+		return false;
+	}
+
+	UTimer totalTimer;
+	UTimer timer;
+
+	// No need to optimize the graph
 	if(_memory)
 	{
-		int currentNode;
+		int currentNode = 0;
 		if(_memory->isIncremental())
 		{
 			if(!_memory->getLastWorkingSignature())
@@ -2991,123 +3002,63 @@ bool Rtabmap::computePath(
 			}
 			currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
 		}
-
-		if(!uContains(nodes, currentNode))
+		if(currentNode && targetNode)
 		{
-			UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
-			return false;
-		}
+			std::list<std::pair<int, Transform> > path = graph::computePath(
+					currentNode,
+					targetNode,
+					_memory,
+					global);
 
-		if(!uContains(nodes, targetNode))
-		{
-			UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
-			return false;
-		}
-
-		// transform nodes into current referential
-		if(_optimizedPoses.size())
-		{
-			if(uContains(nodes, currentNode) && uContains(_optimizedPoses, currentNode))
+			//transform in current referential
+			Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity());
+			_path.resize(path.size());
+			int oi = 0;
+			for(std::list<std::pair<int, Transform> >::iterator iter=path.begin(); iter!=path.end();++iter)
 			{
-				Transform t = _optimizedPoses.at(currentNode) * nodes.at(currentNode).inverse();
-				for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
-				{
-					iter->second = t * iter->second;
-				}
+				_path[oi].first = iter->first;
+				_path[oi++].second = t * iter->second;
 			}
 		}
-
-		std::multimap<int, int> links;
-		for(std::multimap<int, rtabmap::Link>::const_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.
-		if(_planVirtualLinks)
-		{
-			std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
-			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
-			{
-				if(graph::findLink(links, iter->first, iter->second) == links.end())
-				{
-					links.insert(*iter);
-					links.insert(std::make_pair(iter->second, iter->first)); // <->
-				}
-			}
-		}
-
-		UINFO("Computing path from location %d to %d", currentNode, targetNode);
-		UTimer timer;
-		_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode));
-		UINFO("A* time = %fs", timer.ticks());
-
-		if(_path.size() == 0)
-		{
-			_path.clear();
-			UWARN("Cannot compute a path!");
-		}
-		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].first;
-					if(i+1 < _path.size())
-					{
-						stream << " ";
-					}
-				}
-				UINFO("Path = [%s]", stream.str().c_str());
-			}
-			if(_goalsSavedInUserData)
-			{
-				// set goal to latest signature
-				std::string goalStr = uFormat("GOAL:%d", targetNode);
-				setUserData(0, uStr2Bytes(goalStr));
-			}
-		}
-
-		return _path.size()>0;
 	}
-	return false;
-}
+	UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
 
-// return true if path is updated
-bool Rtabmap::computePath(int targetNode, bool global)
-{
-	UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
-	this->clearPath();
-
-	if(!_rgbdSlamMode)
+	if(_path.size() == 0)
 	{
-		UWARN("A path can only be computed in RGBD-SLAM mode");
-		return false;
+		_path.clear();
+		UWARN("Cannot compute a path!");
 	}
-
-	UTimer totalTimer;
-	UTimer timer;
-	std::map<int, Transform> nodes;
-	std::multimap<int, Link> constraints;
-	this->getGraph(nodes, constraints, true, global);
-	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
-
-	if(computePath(targetNode, nodes, constraints))
+	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].first;
+				if(i+1 < _path.size())
+				{
+					stream << " ";
+				}
+			}
+			UINFO("Path = [%s]", stream.str().c_str());
+		}
+		if(_goalsSavedInUserData)
+		{
+			// set goal to latest signature
+			std::string goalStr = uFormat("GOAL:%d", targetNode);
+			setUserData(0, uStr2Bytes(goalStr));
+		}
 		updateGoalIndex();
 	}
-	UINFO("Time computing path (A*) = %fs", timer.ticks());
-	UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
 
 	return _path.size()>0;
 }
 
-bool Rtabmap::computePath(const Transform & targetPose, bool global)
+bool Rtabmap::computePath(const Transform & targetPose)
 {
-	UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
+	UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
 
 	this->clearPath();
 	std::list<std::pair<int, Transform> > pathPoses;
@@ -3120,14 +3071,19 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 
 	//Find the nearest node
 	UTimer timer;
-	std::map<int, Transform> nodes;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, true, global);
-	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
+	std::map<int, Transform> nodes = _optimizedPoses;
+	std::multimap<int, int> links;
+	for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+	{
+		const Signature * s = _memory->getSignature(iter->first);
+		UASSERT(s);
+		for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
+		{
+			links.insert(std::make_pair(jter->second.from(), jter->second.to()));
+			links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <->
+		}
+	}
+	UINFO("Time getting links = %fs", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
 	UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
@@ -3140,15 +3096,72 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 		}
 		else
 		{
-			if(computePath(nearestId, nodes, constraints))
+			int currentNode = 0;
+			if(_memory->isIncremental())
 			{
-				UASSERT(_path.size() > 0);
+				if(!_memory->getLastWorkingSignature())
+				{
+					UWARN("Working memory is empty... cannot compute a path");
+					return false;
+				}
+				currentNode = _memory->getLastWorkingSignature()->id();
+			}
+			else
+			{
+				if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
+				{
+					UWARN("Last localization pose is null... cannot compute a path");
+					return false;
+				}
+				currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
+			}
+
+			// Add links between neighbor nodes in the goal radius.
+			if(_planVirtualLinks)
+			{
+				std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
+				for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
+				{
+					if(graph::findLink(links, iter->first, iter->second) == links.end())
+					{
+						links.insert(*iter);
+						links.insert(std::make_pair(iter->second, iter->first)); // <->
+					}
+				}
+			}
+
+			UINFO("Computing path from location %d to %d", currentNode, nearestId);
+			UTimer timer;
+			_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));
+			UINFO("A* time = %fs", timer.ticks());
+
+			if(_path.size() == 0)
+			{
+				_path.clear();
+				UWARN("Cannot compute a path!");
+			}
+			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].first;
+						if(i+1 < _path.size())
+						{
+							stream << " ";
+						}
+					}
+					UINFO("Path = [%s]", stream.str().c_str());
+				}
+
 				UASSERT(uContains(nodes, _path.back().first));
 				_pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
 
 				updateGoalIndex();
 			}
-			UINFO("Time computing path = %fs", timer.ticks());
 		}
 	}
 	else