Added parameter RGBD/GoalMaxDistance, fixed graph:computePath() warnings

corelib/include/rtabmap/core/Graph.h

@@ -101,13 +101,15 @@ std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
  * @param links The graph's links (from node id -> to node id)
  * @param from initial node
  * @param to final node
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
  * @return the path ids from id "from" to id "to" including initial and final nodes.
  */
 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 			const std::map<int, rtabmap::Transform> & poses,
 			const std::multimap<int, int> & links,
 			int from,
-			int to);
+			int to,
+			bool updateNewCosts = false);
 
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,

corelib/include/rtabmap/core/Parameters.h

@@ -292,6 +292,7 @@ class RTABMAP_EXP Parameters
 	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.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -93,6 +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 getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -183,6 +184,7 @@ private:
 	float _goalReachedRadius; // meters
 	unsigned int _maxAnticipatedNodes;
 	bool _planWithNearNodesLinked;
+	float _goalMaxDistance;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/src/Graph.cpp

@@ -673,7 +673,7 @@ public:
 	rtabmap::Transform pose() const {return pose_;}
 	float distFrom(const rtabmap::Transform & pose) const
 	{
-		return pose_.getDistance(pose);
+		return pose_.getDistanceSquared(pose); // use sqrt distance
 	}
 
 	void setClosed(bool closed) {closed_ = closed;}
@@ -703,7 +703,8 @@ std::list<std::pair<int, Transform> > computePath(
 			const std::map<int, rtabmap::Transform> & poses,
 			const std::multimap<int, int> & links,
 			int from,
-			int to)
+			int to,
+			bool updateNewCosts)
 {
 	std::list<std::pair<int, Transform> > path;
 
@@ -714,28 +715,46 @@ std::list<std::pair<int, Transform> > computePath(
 	std::map<int, Node> nodes;
 	nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
 	std::priority_queue<Pair, std::vector<Pair>, Order> pq;
-	pq.push(Pair(startNode, 0));
-
-	while(pq.size())
+	std::multimap<float, int> pqmap;
+	if(updateNewCosts)
 	{
-		Node & currentNode = nodes.find(pq.top().first)->second;
-		pq.pop();
-		currentNode.setClosed(true);
+		pqmap.insert(std::make_pair(0, startNode));
+	}
+	else
+	{
+		pq.push(Pair(startNode, 0));
+	}
 
-		if(currentNode.id() == endNode)
+	while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
+	{
+		Node * currentNode;
+		if(updateNewCosts)
 		{
-			while(currentNode.id()!=startNode)
+			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(currentNode->id(), currentNode->pose()));
+				currentNode = &nodes.find(currentNode->fromId())->second;
 			}
 			path.push_front(std::make_pair(startNode, poses.at(startNode)));
 			break;
 		}
 
 		// lookup neighbors
-		for(std::multimap<int, int>::const_iterator iter = links.find(currentNode.id());
-			iter!=links.end() && iter->first == currentNode.id();
+		for(std::multimap<int, int>::const_iterator iter = links.find(currentNode->id());
+			iter!=links.end() && iter->first == currentNode->id();
 			++iter)
 		{
 			std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
@@ -743,18 +762,35 @@ std::list<std::pair<int, Transform> > computePath(
 			{
 				std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
 				UASSERT(poseIter != poses.end());
-				Node n(iter->second, currentNode.id(), poseIter->second);
-				n.setCostSoFar(currentNode.costSoFar() + currentNode.distFrom(poseIter->second));
+				Node n(iter->second, currentNode->id(), poseIter->second);
+				n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
 				n.setDistToEnd(n.distFrom(endPose));
 				nodes.insert(std::make_pair(iter->second, n));
-				pq.push(Pair(n.id(), n.totalCost()));
+				if(updateNewCosts)
+				{
+					pqmap.insert(std::make_pair(n.totalCost(), n.id()));
+				}
+				else
+				{
+					pq.push(Pair(n.id(), n.totalCost()));
+				}
 			}
-			else if(nodeIter->second.isOpened())
+			else if(updateNewCosts && nodeIter->second.isOpened())
 			{
-				float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose());
+				float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
 				if(nodeIter->second.costSoFar() > newCostSoFar)
 				{
-					UWARN("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
+					// update the cost in the priority queue
+					for(std::map<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;
+						}
+					}
 				}
 			}
 		}

corelib/src/Rtabmap.cpp

@@ -110,6 +110,7 @@ Rtabmap::Rtabmap() :
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
 	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
+	_goalMaxDistance(Parameters::defaultRGBDGoalMaxDistance()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -376,7 +377,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	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())
@@ -1518,7 +1519,10 @@ bool Rtabmap::process(const SensorData & data)
 			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);
+			if(_localDetectRadius > 0.0f && virtualLoop.getNorm() < _localDetectRadius)
+			{
+				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
+			}
 		}
 
 		// Make sure the next signatures on the path are linked together
@@ -2047,9 +2051,9 @@ void Rtabmap::optimizeCurrentMap(
 	UDEBUG("Optimize map: around location %d", id);
 	if(_memory && id > 0)
 	{
-
+		UTimer timer;
 		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
-		UDEBUG("ids=%d", (int)ids.size());
+		UDEBUG("get ids=%d", (int)ids.size());
 		if(!_optimizeFromGraphEnd && ids.size() > 1)
 		{
 			UTimer timer;
@@ -2063,11 +2067,13 @@ void Rtabmap::optimizeCurrentMap(
 				   id,
 				   timer.ticks());
 		}
+		UINFO("get ids time %f s", timer.ticks());
 
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> edgeConstraints;
 		_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
 		UDEBUG("poses=%d, edgeConstraints=%d", (int)poses.size(), (int)edgeConstraints.size());
+		UINFO("get constraints time %f s", timer.ticks());
 
 		if(constraints)
 		{
@@ -2083,6 +2089,7 @@ void Rtabmap::optimizeCurrentMap(
 		{
 			rtabmap::graph::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
 		}
+		UINFO("optimize time %f s", timer.ticks());
 	}
 }
 
@@ -2350,7 +2357,9 @@ bool Rtabmap::computePath(
 	}
 
 	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)
 	{
@@ -2428,15 +2437,23 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
 	if(nearestId > 0)
 	{
-		if(computePath(nearestId, nodes, constraints))
+		if(_goalMaxDistance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _goalMaxDistance)
 		{
-			UASSERT(_path.size() > 0);
-			UASSERT(uContains(nodes, _path.back().first));
-			_pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
-
-			updateGoalIndex();
+			UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
+					_goalMaxDistance, targetPose.getDistance(nodes.at(nearestId)), nearestId);
+		}
+		else
+		{
+			if(computePath(nearestId, nodes, constraints))
+			{
+				UASSERT(_path.size() > 0);
+				UASSERT(uContains(nodes, _path.back().first));
+				_pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose;
+
+				updateGoalIndex();
+			}
+			UINFO("Time computing path = %fs", timer.ticks());
 		}
-		UINFO("Time computing path = %fs", timer.ticks());
 	}
 	else
 	{
@@ -2538,11 +2555,12 @@ void Rtabmap::updateGoalIndex()
 
 		if(_path.size())
 		{
-			//Always check if the farthest node is accessible in local map
-			int goalIndex = 0;
-			for(int i=(int)_path.size()-1; i>=0; --i)
+			//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)
 			{
-				if(uContains(_optimizedPoses, _path[i].first))
+				if(uContains(_optimizedPoses, _path[i].first) &&
+				   (_goalMaxDistance == 0.0f || _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first)) < _goalMaxDistance))
 				{
 					goalIndex = i;
 					break;