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

Added new parameters: "RGBD/PlanWithNearNodesLinked" and "Mem/LocalSpaceLinksKeptInWM"

corelib/include/rtabmap/core/Graph.h

@@ -103,7 +103,7 @@ std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
  * @param to final node
  * @return the path ids from id "from" to id "to" including initial and final nodes.
  */
-std::vector<int> RTABMAP_EXP computePath(
+std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 			const std::map<int, rtabmap::Transform> & poses,
 			const std::multimap<int, int> & links,
 			int from,

corelib/include/rtabmap/core/Memory.h

@@ -209,6 +209,7 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	int _imageDecimation;
+	bool _localSpaceLinksKeptInWM;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Parameters.h

@@ -194,6 +194,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
 	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1).");
+	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
 
 
 	// KeypointMemory (Keypoint-based)
@@ -288,8 +289,9 @@ 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, GoalReachedRadius,    float, 0.5, "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\").");
+	RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked,  bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -119,10 +119,10 @@ public:
 			bool optimized,
 			bool global);
 	void clearPath();
-	std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
+	bool computePath(int targetNode, bool global);
-	std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
+	bool computePath(const Transform & targetPose, bool global);
-	const std::vector<int> & getPath() const {return _path;}
+	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
-	std::list<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
 	int getPathCurrentGoalId() const;
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
@@ -182,6 +182,7 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	unsigned int _maxAnticipatedNodes;
+	bool _planWithNearNodesLinked;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -210,7 +211,7 @@ private:
 	Transform _mapTransform; // for localization mode
 
 	// Planning stuff
-	std::vector<int> _path;
+	std::vector<std::pair<int,Transform> > _path;
 	unsigned int _pathCurrentIndex;
 	unsigned int _pathGoalIndex;
 	Transform _pathTransformToGoal;

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(

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)

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
+			// immunize all nodes after current node
-			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+			for(unsigned int i=_pathCurrentIndex; i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes; ++i)
 			{
-				immunizedLocations.insert(_path[i]);
+				immunizedLocations.insert(_path[i].first);
-				UDEBUG("Path immunization: node %d", _path[i]);
+				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]);
+					UINFO("retrieval of node %d on path", _path[i].first);
-					retrievalPathIds.push_back(_path[i]);
+					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);
+	if(_planWithNearNodesLinked)
-	//links.insert(clusters.begin(), clusters.end());
+	{
+		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()));
+			UASSERT(uContains(nodes, _path.back().first));
-			_pathTransformToGoal = nodes.at(_path.back()).inverse() * targetPose;
+			_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);

guilib/src/PreferencesDialog.cpp

@@ -339,6 +339,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str());
 	_ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str());
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
+	_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
 	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
 
 
@@ -451,6 +452,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_maxAnticipatedNodes->setObjectName(Parameters::kRGBDMaxAnticipatedNodes().c_str());
+	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -2836,7 +2836,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <string/>
                         </property>
                         <property name="checked">
-                         <bool>true</bool>
+                         <bool>false</bool>
                         </property>
                        </widget>
                       </item>
@@ -2880,7 +2880,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
                          <string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
@@ -2890,7 +2890,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                        <widget class="QSpinBox" name="spinBox_imageDecimation">
                         <property name="minimum">
                          <number>1</number>
@@ -2900,6 +2900,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_retrieved_7">
+                        <property name="text">
+                         <string>If local space links are kept in WM.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -5312,6 +5332,23 @@ Warning when set to false: when some nodes are transferred, the first referentia
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="0">
+                        <widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_space3_4">
+                         <property name="text">
+                          <string>Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by &quot;Goal reached radius&quot; above.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>