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

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

Browse Source 
Added new parameters: "RGBD/PlanWithNearNodesLinked" and "Mem/LocalSpaceLinksKeptInWM"
This commit is contained in:
Mathieu Labbe
2015-02-17 17:30:47 -05:00
parent 4279625d03
commit 2f6426f029
9 changed files with 182 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Graph.h
View File

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

1
corelib/include/rtabmap/core/Memory.h
View File

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

4
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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.");

11
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -119,10 +119,10 @@ public:
bool optimized,
bool global);
void clearPath();
std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
const std::vector<int> & getPath() const {return _path;}
std::list<std::pair<int, Transform> > getPathNextPoses() const;
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, bool global);
const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
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;

17
corelib/src/Graph.cpp
View File

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

46
corelib/src/Memory.cpp
View File

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

120
corelib/src/Rtabmap.cpp
View File

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