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

Rtabmap: Added getPathNextPoses() and getPathNextNodes(). Local path is sent over statistics.

Browse Source 
GUI: local path is shown in GraphViewer. Now handling stereo odometry data.
This commit is contained in:
Mathieu Labbe
2015-02-03 11:09:10 -05:00
parent ee0051a585
commit ee885321aa
8 changed files with 168 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/src/Rtabmap.cpp
View File

@@ -1633,6 +1633,12 @@ bool Rtabmap::process(const SensorData & data)
statistics_.setRawLikelihood(rawLikelihood);
}
}
// Path
if(_path.size())
{
statistics_.setLocalPath(this->getPathNextNodes());
}
}
timeStatsCreation = timer.ticks();
@@ -2415,6 +2421,53 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
return pathPoses;
}
std::list<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
{
std::list<std::pair<int, Transform> > poses;
if(_path.size())
{
UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
{
std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i]);
if(iter != _optimizedPoses.end())
{
poses.push_back(*iter);
}
else
{
break;
}
}
}
return poses;
}
std::vector<int> Rtabmap::getPathNextNodes() const
{
std::vector<int> ids;
if(_path.size())
{
UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
ids.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]);
if(iter != _optimizedPoses.end())
{
ids[oi++] = iter->first;
}
else
{
break;
}
}
ids.resize(oi);
}
return ids;
}
int Rtabmap::getPathGoalId() const
{
if(_path.size())