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

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