Updated how locations are retrieved on a planned path or near based on space if no path is activated.

Added mirroring option to OpenNI2 camera Removed "RGBD/MaxAnticipatedNodes" and "RGBD/GoalMaxDistance" parameters Renamed "RGBD/LocalLoopDetectionRadius" to "RGBD/LocalRadius" Added "RGBD/MaxLocalRetrieved" parameter GUI: fixed OpenNI 2 selection from the MainWindow, added OpenNI2 under Kinect menu
2026-09-02 01:20:25 +08:00 · 2015-02-20 15:32:39 -05:00
parent 647c709595
commit 38a4e8e4ee
16 changed files with 519 additions and 265 deletions
--- a/corelib/include/rtabmap/core/CameraRGBD.h
+++ b/corelib/include/rtabmap/core/CameraRGBD.h
@@ -199,7 +199,8 @@ public:
 	static bool exposureGainAvailable();

 public:
-	CameraOpenNI2(float imageRate = 0,
+	CameraOpenNI2(const std::string & deviceId = "",
+					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity(),
 					float fx = 0.0f,
 					float fy = 0.0f,
@@ -213,6 +214,7 @@ public:
 	bool setAutoExposure(bool enabled);
 	bool setExposure(int value);
 	bool setGain(int value);
+	bool setMirroring(bool enabled);

 protected:
 	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -223,6 +225,7 @@ private:
 	openni::VideoStream * _depth;
 	float _depthFx;
 	float _depthFy;
+	std::string _deviceId;
 };


--- a/corelib/include/rtabmap/core/Graph.h
+++ b/corelib/include/rtabmap/core/Graph.h
@@ -77,6 +77,14 @@ bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
 		std::map<int, Transform> & poses,
 		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);

+/**
+ * Get only the the most recent or older poses in the defined radius.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
+ * @return A map containing only most recent or older poses in the the defined radius
+ */
 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,
@@ -87,7 +95,7 @@ std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
 * Get all neighbor nodes in a fixed radius around each pose.
 * @param poses The poses
 * @param radius Radius (m) of the search for near neighbors
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
 * @return A map between each pose id and its neighbors found in the radius
 */
 std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
@@ -115,12 +123,26 @@ int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);

+/**
+ * Get nodes near the query
+ * @param nodeId the query id
+ * @param nodes the nodes to search for
+ * @param maxNearestNeighbors Maximum nearest neighbor to get. 0 means all.
+ * @param radius radius to search for (m)
+ * @return the nodes with squared distance to query node.
+ */
 std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		int maxNearestNeighbors,
 		float radius);

+float RTABMAP_EXP computePathLength(
+		const std::vector<std::pair<int, Transform> > & path,
+		unsigned int fromIndex = 0,
+		unsigned int toIndex = 0);
+
+
 } /* namespace graph */

 } /* namespace rtabmap */
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -290,14 +290,13 @@ class RTABMAP_EXP Parameters
 	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, 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.");
-	RTABMAP_PARAM(RGBD, GoalMaxDistance,  float, 0, "Maximum distance (m) of the target goal from the graph (0 means infinity). If the goal is too far from the graph, the plan is aborted. Also when set, the next goal in the graph can't be farther than this distance from the current position.");
+	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.");

 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 50,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")

--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -93,7 +93,7 @@ public:
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
 	float getGoalReachedRadius() const {return _goalReachedRadius;}
-	float getGoalMaxDistance() const {return _goalMaxDistance;}
+	float getLocalRadius() const {return _localRadius;}

 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -128,7 +128,6 @@ public:
 	int getPathCurrentGoalId() const;
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}

-	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,
@@ -156,6 +155,7 @@ private:
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
+	unsigned int _maxLocalRetrieved;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
 	bool _statisticLoggedHeaders;
@@ -168,7 +168,7 @@ private:
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
-	float _localDetectRadius;
+	float _localRadius;
 	float _localDetectMaxNeighbors;
 	int _localDetectMaxDiffID;
 	int _toroIterations;
@@ -182,9 +182,7 @@ private:
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
-	unsigned int _maxAnticipatedNodes;
 	bool _planWithNearNodesLinked;
-	float _goalMaxDistance;

 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
--- a/corelib/src/CameraRGBD.cpp
+++ b/corelib/src/CameraRGBD.cpp
@@ -346,7 +346,14 @@ bool CameraOpenNI2::exposureGainAvailable()
 #endif
 }

-CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
+CameraOpenNI2::CameraOpenNI2(
+		const std::string & deviceId,
+		float imageRate,
+		const rtabmap::Transform & localTransform,
+		float fx,
+		float fy,
+		float cx,
+		float cy) :
 	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 #ifdef WITH_OPENNI2
 	_device(new openni::Device()),
@@ -358,7 +365,8 @@ CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTr
 	_depth(0),
 #endif
 	_depthFx(0.0f),
-	_depthFy(0.0f)
+	_depthFy(0.0f),
+	_deviceId(deviceId)
 {
 }

@@ -438,14 +446,31 @@ bool CameraOpenNI2::setGain(int value)
 	return false;
 }

+bool CameraOpenNI2::setMirroring(bool enabled)
+{
+	if(_color->isValid() && _depth->isValid())
+	{
+		return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
+				_color->setMirroringEnabled(enabled) == openni::STATUS_OK;
+	}
+	return false;
+}
+
 bool CameraOpenNI2::init()
 {
 #ifdef WITH_OPENNI2
 	openni::OpenNI::initialize();

-	if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
+	if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
 	{
-		UERROR("CameraOpenNI2: Cannot open device.");
+		if(!_deviceId.empty())
+		{
+			UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str());
+		}
+		else
+		{
+			UERROR("CameraOpenNI2: Cannot open device.");
+		}
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -505,9 +505,13 @@ bool loadTOROGraph(const std::string & fileName,
 }


-std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
+std::map<int, Transform> radiusPosesFiltering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		bool keepLatest)
 {
-	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
+	if(poses.size() > 1 && radius > 0.0f)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(poses.size());
@@ -528,7 +532,6 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p

 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
-			// ignore scans
 			if(indicesChecked.find(i) == indicesChecked.end())
 			{
 				std::vector<int> kIndices;
@@ -542,11 +545,18 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
 				{
 					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
 					{
-						const Transform & checkT = transforms.at(kIndices[j]);
-						// same orientation?
-						Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-						if(a <= angle)
+						if(angle > 0.0f)
+						{
+							const Transform & checkT = transforms.at(kIndices[j]);
+							// same orientation?
+							Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+							double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+							if(a <= angle)
+							{
+								cloudIndices.insert(kIndices[j]);
+							}
+						}
+						else
 						{
 							cloudIndices.insert(kIndices[j]);
 						}
@@ -605,7 +615,7 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
 std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
 {
 	std::multimap<int, int> clusters;
-	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
+	if(poses.size() > 1 && radius > 0.0f)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(poses.size());
@@ -635,11 +645,18 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 			{
 				if((int)i != kIndices[j])
 				{
-					const Transform & checkT = transforms.at(kIndices[j]);
-					// same orientation?
-					Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-					if(a <= angle)
+					if(angle > 0.0f)
+					{
+						const Transform & checkT = transforms.at(kIndices[j]);
+						// same orientation?
+						Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+						if(a <= angle)
+						{
+							clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
+						}
+					}
+					else
 					{
 						clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
 					}
@@ -834,7 +851,7 @@ int findNearestNode(
 	return id;
 }

-// return <id, distance>, including query
+// return <id, sqrd distance>, including query
 std::map<int, float> getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
@@ -868,7 +885,7 @@ std::map<int, float> getNodesInRadius(
 		{
 			if(ind[i] >=0)
 			{
-				UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
+				UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
 				foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
 			}
 		}
@@ -877,6 +894,31 @@ std::map<int, float> getNodesInRadius(
 	return foundNodes;
 }

+float computePathLength(
+		const std::vector<std::pair<int, Transform> > & path,
+		unsigned int fromIndex,
+		unsigned int toIndex)
+{
+	float length = 0.0f;
+	if(path.size() > 1)
+	{
+		UASSERT(fromIndex  < path.size() && toIndex < path.size() && fromIndex <= toIndex);
+		if(fromIndex >= toIndex)
+		{
+			toIndex = path.size()-1;
+		}
+		float x=0, y=0, z=0;
+		for(unsigned int i=fromIndex; i<toIndex-1; ++i)
+		{
+			x += fabs(path[i].second.x() - path[i+1].second.x());
+			y += fabs(path[i].second.y() - path[i+1].second.y());
+			z += fabs(path[i].second.z() - path[i+1].second.z());
+		}
+		length = sqrt(x*x + y*y + z*z);
+	}
+	return length;
+}
+
 } /* namespace graph */

 } /* namespace rtabmap */
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -82,6 +82,7 @@ Rtabmap::Rtabmap() :
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
+	_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
 	_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
 	_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
 	_statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
@@ -94,7 +95,7 @@ Rtabmap::Rtabmap() :
 	_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
-	_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
+	_localRadius(Parameters::defaultRGBDLocalRadius()),
 	_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
 	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
 	_toroIterations(Parameters::defaultRGBDToroIterations()),
@@ -108,9 +109,7 @@ Rtabmap::Rtabmap() :
 	_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
-	_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
 	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
-	_goalMaxDistance(Parameters::defaultRGBDGoalMaxDistance()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -351,6 +350,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
+	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
@@ -362,7 +362,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
-	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
+	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
@@ -375,9 +375,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
-	Parameters::parse(parameters, Parameters::kRGBDMaxAnticipatedNodes(), _maxAnticipatedNodes);
 	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
-	Parameters::parse(parameters, Parameters::kRGBDGoalMaxDistance(), _goalMaxDistance);

 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1224,34 +1222,80 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
 	//============================================================
-	std::list<int> retrievalPathIds;
-	if(_path.size() && _rgbdSlamMode)
+	std::set<int> retrievalLocalIds;
+	if(_rgbdSlamMode && _maxLocalRetrieved > 0)
 	{
-		updateGoalIndex();
-
+		// Priority on locations on the planned path
 		if(_path.size())
 		{
-			// immunize all nodes after current node
-			for(unsigned int i=_pathCurrentIndex; i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes; ++i)
+			updateGoalIndex();
+
+			if(_path.size())
 			{
-				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].first) == 0)
+				float distanceSoFar = 0.0f;
+				// immunize all nodes after current node and
+				// retrieve nodes after current node in the maximum radius from the current node
+				for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 				{
-					UINFO("retrieval of node %d on path", _path[i].first);
-					retrievalPathIds.push_back(_path[i].first);
+					if(_localRadius > 0.0f && i != _pathCurrentIndex)
+					{
+						distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
+					}
+
+					if(distanceSoFar <= _localRadius)
+					{
+						if(_memory->getSignature(_path[i].first) != 0)
+						{
+							immunizedLocations.insert(_path[i].first);
+							UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
+						}
+						else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+						{
+							UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
+							retrievalLocalIds.insert(_path[i].first);
+							// retrieved locations are automatically immunized
+						}
+					}
+					else
+					{
+						UDEBUG("Stop on node %d (dist=%fm > %fm)",
+								_path[i].first, distanceSoFar, _localRadius);
+						break;
+					}
 				}
 			}
-
-			// insert them first to make sure they are loaded.
-			reactivatedIds.insert(reactivatedIds.begin(), retrievalPathIds.begin(), retrievalPathIds.end());
 		}
+		else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+		{
+			// retrieval based on the nodes near the current pose
+			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, 0, _localRadius);
+			// sort by distance
+			std::multimap<float, int> nearNodesByDist;
+			for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
+			{
+				nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
+			}
+			for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
+				iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+				++iter)
+			{
+				const Signature * s = _memory->getSignature(iter->second);
+				UASSERT(s != 0);
+				for(std::map<int, Link>::const_iterator jter=s->getLinks().begin();
+					jter!=s->getLinks().end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++jter)
+				{
+					if(_memory->getSignature(jter->first) == 0)
+					{
+						UINFO("retrieval of node %d on local map", jter->first);
+						retrievalLocalIds.insert(jter->first);
+					}
+				}
+			}
+		}
+
+		// insert them first to make sure they are loaded.
+		reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
 	}

 	//============================================================
@@ -1263,7 +1307,7 @@ bool Rtabmap::process(const SensorData & data)
 		// only a loop closure link is added...
 		signaturesRetrieved = _memory->reactivateSignatures(
 				reactivatedIds,
-				_maxRetrieved+retrievalPathIds.size(), // add path retrieved
+				_maxRetrieved+retrievalLocalIds.size(), // add path retrieved
 				timeRetrievalDbAccess);

 		ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
@@ -1417,7 +1461,7 @@ bool Rtabmap::process(const SensorData & data)
 			localSpacePoses = this->getWMPosesInRadius(
 			signature->id(),
 			_localDetectMaxNeighbors,
-			_localDetectRadius,
+			_localRadius,
 			_localDetectMaxDiffID,
 			localSpaceNearestId);

@@ -1519,31 +1563,11 @@ bool Rtabmap::process(const SensorData & data)
 			uContains(_optimizedPoses, _path[_pathCurrentIndex].first))
 		{
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
-			if(_localDetectRadius > 0.0f && virtualLoop.getNorm() < _localDetectRadius)
+			if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
 			{
 				_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
@@ -1603,24 +1627,32 @@ bool Rtabmap::process(const SensorData & data)
 				UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
 				statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
 			}
+			statistics_.setMapCorrection(_mapCorrection);
+			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());

+			// Set local graph
 			if(!_rgbdSlamMode)
 			{
+				// no optimization on appearance-only mode, create a local graph
+				std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
+				std::map<int, Transform> poses;
 				std::map<int, int> mapIds;
-				mapIds.insert(std::make_pair(signature->id(), _memory->getMapId(signature->id())));
-				if(_loopClosureHypothesis.first)
+				std::multimap<int, Link> constraints;
+				_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
-					mapIds.insert(std::make_pair(_loopClosureHypothesis.first, _memory->getMapId(_loopClosureHypothesis.first)));
+					mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
 				}
+				statistics_.setPoses(poses);
+				statistics_.setConstraints(constraints);
 				statistics_.setMapIds(mapIds);
-			}//else... see finalize statistics below
+			}
+			else // RGBD-SLAM mode
+			{
+				//see after transfer below
+			}

-
-			statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
-			statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
-			statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
-
-			// timing...
+			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
 			statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000);
@@ -1634,6 +1666,9 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
 			statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);

+			// retrieval
+			statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
+
 			// Surf specific parameters
 			statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);

@@ -1683,7 +1718,7 @@ bool Rtabmap::process(const SensorData & data)
 		_memory->deleteLocation(signature->id());
 	}

-	// Pass this point signature should not be used, since it could be transferred...
+	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;

 	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
@@ -1713,11 +1748,40 @@ bool Rtabmap::process(const SensorData & data)
 	_lastProcessTime = totalTime;

 	//Remove optimized poses from signatures transferred
-	for(std::list<int>::iterator iter = signaturesRemoved.begin(); iter!=signaturesRemoved.end(); ++iter)
+	if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
 	{
-		UDEBUG("removing optimized pose %d...", *iter);
-		_optimizedPoses.erase(*iter);
-		_constraints.erase(*iter);
+		//refresh the local map because some transferred nodes may have broken the tree
+		if(_memory->getLastWorkingSignature())
+		{
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
+			{
+				if(!uContains(ids, iter->first))
+				{
+					_optimizedPoses.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+			for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
+			{
+				if(!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to()))
+				{
+					_constraints.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+		}
+		else
+		{
+			_optimizedPoses.clear();
+			_constraints.clear();
+		}
 	}


@@ -1740,34 +1804,22 @@ bool Rtabmap::process(const SensorData & data)
 		statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
 		statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());

-		//Poses, place this after Transfer! (_optimizedPoses may change)
-		std::map<int, int> mapIds;
+		// place after transfer because the memory/local graph may have changed
+		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
+		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+
 		if(_rgbdSlamMode)
 		{
+			std::map<int, int> mapIds;
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
 				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
 			}
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
-			statistics_.setMapCorrection(_mapCorrection);
-			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
+			statistics_.setMapIds(mapIds);
 		}
-		else if(_memory->getLastWorkingSignature())
-		{
-			// no optimization on appearance-only mode
-			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
-			std::map<int, Transform> poses;
-			std::multimap<int, Link> constraints;
-			_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-			{
-				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
-			}
-			statistics_.setPoses(poses);
-			statistics_.setConstraints(constraints);
-		}
-		statistics_.setMapIds(mapIds);
+
 	}

 	// Log info...
@@ -2437,10 +2489,10 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
 	if(nearestId > 0)
 	{
-		if(_goalMaxDistance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _goalMaxDistance)
+		if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
 		{
 			UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
-					_goalMaxDistance, targetPose.getDistance(nodes.at(nearestId)), nearestId);
+					_localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
 		}
 		else
 		{
@@ -2525,7 +2577,6 @@ int Rtabmap::getPathCurrentGoalId() const

 void Rtabmap::updateGoalIndex()
 {
-	UDEBUG("");
 	if(!_rgbdSlamMode)
 	{
 		UWARN("This method can on be used in RGBD-SLAM mode!");
@@ -2534,6 +2585,39 @@ void Rtabmap::updateGoalIndex()

 	if(_path.size())
 	{
+		// Make sure the next signatures on the path are linked together
+		float distanceSoFar = 0.0f;
+		for(unsigned int i=_pathCurrentIndex;
+			i<_path.size();
+			++i)
+		{
+			if(i>0)
+			{
+				if(_localRadius > 0.0f)
+				{
+					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
+				}
+				if(distanceSoFar <= _localRadius)
+				{
+					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);
+							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
+						}
+					}
+				}
+				else
+				{
+					break;
+				}
+			}
+		}
+
+		UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
 		if(_memory->getLastWorkingSignature() == 0 ||
 		   !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
 		{
@@ -2556,13 +2640,35 @@ void Rtabmap::updateGoalIndex()
 		if(_path.size())
 		{
 			//Always check if the farthest node is accessible in local map (max to local space radius if set)
-			int goalIndex = _pathGoalIndex;
-			for(int i=(int)_path.size()-1; i>=goalIndex; --i)
+			int goalIndex = _pathCurrentIndex;
+			float distanceSoFar = 0.0f;
+			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
-				if(uContains(_optimizedPoses, _path[i].first) &&
-				   (_goalMaxDistance == 0.0f || _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first)) < _goalMaxDistance))
+				if(uContains(_optimizedPoses, _path[i].first))
+				{
+					if(_localRadius > 0.0f)
+					{
+						if(i == _pathCurrentIndex)
+						{
+							distanceSoFar += _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
+						}
+						else
+						{
+							distanceSoFar += _optimizedPoses.at(_path[i-1].first).getDistance(_optimizedPoses.at(_path[i].first));
+						}
+					}
+
+					if(distanceSoFar <= _localRadius)
+					{
+						goalIndex = i;
+					}
+					else
+					{
+						break;
+					}
+				}
+				else
 				{
-					goalIndex = i;
 					break;
 				}
 			}
@@ -2592,6 +2698,14 @@ void Rtabmap::updateGoalIndex()
 					}
 				}
 			}
+			if(distance < 0)
+			{
+				UERROR("The nearest pose on the path not found!");
+			}
+			else
+			{
+				UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first);
+			}
 			if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex)
 			{
 				_pathCurrentIndex = nearestNodeIndex;