i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-18 16:57:16 -05:00
parent 9dfbe8d233
commit cead1e00d4
7 changed files with 124 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
corelib/src/Graph.cpp
View File

@@ -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;
}
}
}
}
}

48
corelib/src/Rtabmap.cpp
View File

@@ -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;