Updated how retrieved locations on the planned path are linked to local map.

Added new parameters: "RGBD/PlanWithNearNodesLinked" and "Mem/LocalSpaceLinksKeptInWM"
2026-09-02 01:20:25 +08:00 · 2015-02-17 17:30:47 -05:00
parent 4279625d03
commit 2f6426f029
9 changed files with 182 additions and 64 deletions
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -172,6 +172,7 @@ void optimizeTOROGraph(
 		{
 			if(uContains(depthGraph, iter->first))
 			{
+				UASSERT(uContains(rtabmapToToro, iter->first));
 				UASSERT_MSG(!iter->second.isNull(), uFormat("Poses should not be null! Id=%d", iter->first).c_str());
 				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
 			}
@@ -182,6 +183,7 @@ void optimizeTOROGraph(
 		{
 			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
 			{
+				UASSERT(uContains(rtabmapToToro, iter->first) && uContains(rtabmapToToro, iter->second.to()));
 				UASSERT(!iter->second.transform().isNull());
 				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
 			}
@@ -247,6 +249,7 @@ void optimizeTOROGraph(
 		bool ignoreCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
 {
+	UDEBUG("Optimizing graph...");
 	UASSERT(toroIterations>0);
 	optimizedPoses.clear();
 	if(edgeConstraints.size()>=1 && poses.size()>=2)
@@ -254,6 +257,7 @@ void optimizeTOROGraph(
 		// Apply TORO optimization
 		AISNavigation::TreeOptimizer3 pg;
 		pg.verboseLevel = 0;
+		UDEBUG("fill poses to TORO...");
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			float x,y,z, roll,pitch,yaw;
@@ -271,6 +275,7 @@ void optimizeTOROGraph(
 			}
 		}

+		UDEBUG("fill edges to TORO...");
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
 			int id1 = iter->first;
@@ -299,6 +304,7 @@ void optimizeTOROGraph(
 				return;
 			}
 		}
+		UDEBUG("buildMST...");
 		pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();

 		UDEBUG("Initial guess...");
@@ -356,6 +362,7 @@ void optimizeTOROGraph(
 	{
 		UWARN("This method should be called at least with 1 pose!");
 	}
+	UDEBUG("Optimizing graph...end!");
 }

 bool saveTOROGraph(
@@ -692,13 +699,13 @@ struct Order
    }
 };

-std::vector<int> computePath(
+std::list<std::pair<int, Transform> > computePath(
 			const std::map<int, rtabmap::Transform> & poses,
 			const std::multimap<int, int> & links,
 			int from,
 			int to)
 {
-	std::list<int> path;
+	std::list<std::pair<int, Transform> > path;

 	//A*
 	int startNode = from;
@@ -719,10 +726,10 @@ std::vector<int> computePath(
 		{
 			while(currentNode.id()!=startNode)
 			{
-				path.push_front(currentNode.id());
+				path.push_front(std::make_pair(currentNode.id(), currentNode.pose()));
 				currentNode = nodes.find(currentNode.fromId())->second;
 			}
-			path.push_front(startNode);
+			path.push_front(std::make_pair(startNode, poses.at(startNode)));
 			break;
 		}

@@ -752,7 +759,7 @@ std::vector<int> computePath(
 			}
 		}
 	}
-	return uListToVector(path);
+	return path;
 }

 int findNearestNode(
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -67,6 +67,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_generateIds(Parameters::defaultMemGenerateIds()),
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_imageDecimation(Parameters::defaultMemImageDecimation()),
+	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -385,6 +386,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
+	Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);

 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -580,6 +582,29 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	while(_stMem.size() && _maxStMemSize>0 && (int)_stMem.size() > _maxStMemSize)
 	{
 		UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
+		if(!_localSpaceLinksKeptInWM)
+		{
+			// remove local space links outside STM
+			Signature * s = this->_getSignature(*_stMem.begin());
+			UASSERT(s!=0);
+			std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
+			for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kLocalSpaceClosure)
+				{
+					Signature * sTo = this->_getSignature(iter->first);
+					if(sTo)
+					{
+						sTo->removeLink(s->id());
+					}
+					else
+					{
+						UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
+					}
+					s->removeLink(iter->first);
+				}
+			}
+		}
 		_workingMem.insert(_workingMem.end(), *_stMem.begin());
 		_stMem.erase(*_stMem.begin());
 		++_signaturesAdded;
@@ -1466,6 +1491,27 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 			s->removeLinks(); // remove all links
 			s->setWeight(0);
 		}
+		else
+		{
+			//make sure that virtual links are removed
+			const std::map<int, Link> & links = s->getLinks();
+			for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kVirtualClosure)
+				{
+					Signature * sTo = this->_getSignature(iter->first);
+					if(sTo)
+					{
+						sTo->removeLink(s->id());
+					}
+					else
+					{
+						UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
+					}
+				}
+			}
+			s->removeVirtualLinks();
+		}

 		this->disableWordsRef(s->id());
 		if(!saveToDatabase)
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -109,6 +109,7 @@ Rtabmap::Rtabmap() :
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
+	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -374,6 +375,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDMaxAnticipatedNodes(), _maxAnticipatedNodes);
+	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);


 	// RGB-D SLAM stuff
@@ -1225,21 +1227,21 @@ bool Rtabmap::process(const SensorData & data)

 		if(_path.size())
 		{
-			// immunize all nodes between current node and goal node
-			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+			// immunize all nodes after current node
+			for(unsigned int i=_pathCurrentIndex; i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes; ++i)
 			{
-				immunizedLocations.insert(_path[i]);
-				UDEBUG("Path immunization: node %d", _path[i]);
+				immunizedLocations.insert(_path[i].first);
+				UDEBUG("Path immunization: node %d", _path[i].first);
 			}
 			// 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)
+				if(_memory->getSignature(_path[i].first) == 0)
 				{
-					UINFO("retrieval of node %d on path", _path[i]);
-					retrievalPathIds.push_back(_path[i]);
+					UINFO("retrieval of node %d on path", _path[i].first);
+					retrievalPathIds.push_back(_path[i].first);
 				}
 			}

@@ -1387,15 +1389,6 @@ bool Rtabmap::process(const SensorData & data)
 		}
 	}

-	// Add a virtual loop closure link to keep the path linked to local map
-	if(_path.size() &&
-		signature->id() != _path[_pathCurrentIndex] &&
-		!signature->hasLink(_path[_pathCurrentIndex]))
-	{
-		Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex]);
-		_memory->addLink(_path[_pathCurrentIndex], signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
-	}
-
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);

@@ -1511,6 +1504,40 @@ bool Rtabmap::process(const SensorData & data)
 	timeMapOptimization = timer.ticks();
 	ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);

+	//============================================================
+	// Add virtual links if a path is activated
+	//============================================================
+	if(_path.size())
+	{
+		// Add a virtual loop closure link to keep the path linked to local map
+		if( signature->id() != _path[_pathCurrentIndex].first &&
+			!signature->hasLink(_path[_pathCurrentIndex].first) &&
+			uContains(_optimizedPoses, _path[_pathCurrentIndex].first))
+		{
+			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
+			_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
+		}
+
+		// Make sure the next signatures on the path are linked together
+		for(unsigned int i=_pathCurrentIndex;
+			i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes;
+			++i)
+		{
+			if(i>0)
+			{
+				const Signature * s = _memory->getSignature(_path[i].first);
+				if(s)
+				{
+					if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
+					{
+						Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
+						_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 99999);
+						UWARN("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
+					}
+				}
+			}
+		}
+	}
 	//============================================================
 	// Prepare statistics
 	//============================================================
@@ -2313,11 +2340,14 @@ bool Rtabmap::computePath(
 		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());
+	if(_planWithNearNodesLinked)
+	{
+		std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
+		links.insert(clusters.begin(), clusters.end());
+	}

 	UINFO("Computing path from location %d to %d", currentNode, targetNode);
-	_path = rtabmap::graph::computePath(nodes, links, currentNode, targetNode);
+	_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode));

 	if(_path.size() == 0)
 	{
@@ -2332,7 +2362,7 @@ bool Rtabmap::computePath(
 			std::stringstream stream;
 			for(unsigned int i=0; i<_path.size(); ++i)
 			{
-				stream << _path[i];
+				stream << _path[i].first;
 				if(i+1 < _path.size())
 				{
 					stream << " ";
@@ -2346,15 +2376,14 @@ bool Rtabmap::computePath(
 }

 // return true if path is updated
-std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode, bool global)
+bool Rtabmap::computePath(int targetNode, bool global)
 {
 	this->clearPath();
-	std::list<std::pair<int, Transform> > pathPoses;

 	if(!_rgbdSlamMode)
 	{
 		UWARN("A path can only be computed in RGBD-SLAM mode");
-		return pathPoses;
+		return false;
 	}

 	UTimer timer;
@@ -2367,18 +2396,13 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode, bool
 	if(computePath(targetNode, nodes, constraints))
 	{
 		updateGoalIndex();
-
-		for(unsigned int i = 0; i<_path.size(); ++i)
-		{
-			pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
-		}
 	}
 	UINFO("Time computing path = %fs", timer.ticks());

-	return pathPoses;
+	return _path.size()>0;
 }

-std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & targetPose, bool global)
+bool Rtabmap::computePath(const Transform & targetPose, bool global)
 {
 	this->clearPath();
 	std::list<std::pair<int, Transform> > pathPoses;
@@ -2386,7 +2410,7 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & tar
 	if(!_rgbdSlamMode)
 	{
 		UWARN("This method can only be used in RGBD-SLAM mode");
-		return pathPoses;
+		return false;
 	}

 	//Find the nearest node
@@ -2404,15 +2428,10 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & tar
 		if(computePath(nearestId, nodes, constraints))
 		{
 			UASSERT(_path.size() > 0);
-			UASSERT(uContains(nodes, _path.back()));
-			_pathTransformToGoal = nodes.at(_path.back()).inverse() * targetPose;
+			UASSERT(uContains(nodes, _path.back().first));
+			_pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;

 			updateGoalIndex();
-			for(unsigned int i = 0; i<_path.size(); ++i)
-			{
-				pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
-			}
-			pathPoses.back().second *= _pathTransformToGoal;
 		}
 		UINFO("Time computing path = %fs", timer.ticks());
 	}
@@ -2421,27 +2440,30 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & tar
 		UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
 	}

-	return pathPoses;
+	return _path.size()>0;
 }

-std::list<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
+std::vector<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
 {
-	std::list<std::pair<int, Transform> > poses;
+	std::vector<std::pair<int, Transform> > poses;
 	if(_path.size())
 	{
 		UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
+		poses.resize(_pathGoalIndex-_pathCurrentIndex+1);
+		int oi=0;
 		for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
 		{
-			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i]);
+			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
 			if(iter != _optimizedPoses.end())
 			{
-				poses.push_back(*iter);
+				poses[oi++] = *iter;
 			}
 			else
 			{
 				break;
 			}
 		}
+		poses.resize(oi);
 	}
 	return poses;
 }
@@ -2456,7 +2478,7 @@ std::vector<int> Rtabmap::getPathNextNodes() const
 		int oi = 0;
 		for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
 		{
-			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i]);
+			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i].first);
 			if(iter != _optimizedPoses.end())
 			{
 				ids[oi++] = iter->first;
@@ -2476,7 +2498,7 @@ int Rtabmap::getPathCurrentGoalId() const
 	if(_path.size())
 	{
 		UASSERT(_pathGoalIndex <= _path.size());
-		return _path[_pathGoalIndex];
+		return _path[_pathGoalIndex].first;
 	}
 	return 0;
 }
@@ -2499,7 +2521,7 @@ void Rtabmap::updateGoalIndex()
 			return;
 		}

-		int goalId = _path.back();
+		int goalId = _path.back().first;
 		if(uContains(_optimizedPoses, goalId))
 		{
 			//use local position to know if the goal is reached
@@ -2517,7 +2539,7 @@ void Rtabmap::updateGoalIndex()
 			int goalIndex = 0;
 			for(int i=(int)_path.size()-1; i>=0; --i)
 			{
-				if(uContains(_optimizedPoses, _path[i]))
+				if(uContains(_optimizedPoses, _path[i].first))
 				{
 					goalIndex = i;
 					break;
@@ -2527,7 +2549,7 @@ void Rtabmap::updateGoalIndex()
 			if((int)_pathGoalIndex != goalIndex)
 			{
 				UINFO("Updated current goal from %d to %d (%d/%d)",
-						(int)_path[_pathGoalIndex], _path[goalIndex], goalIndex+1, (int)_path.size());
+						(int)_path[_pathGoalIndex].first, _path[goalIndex].first, goalIndex+1, (int)_path.size());
 				_pathGoalIndex = goalIndex;
 			}

@@ -2538,7 +2560,7 @@ void Rtabmap::updateGoalIndex()
 			UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
 			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
 			{
-				std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i]);
+				std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i].first);
 				if(iter != _optimizedPoses.end())
 				{
 					float d = currentPose.getDistanceSquared(iter->second);