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

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

@@ -120,6 +120,8 @@ public:
void clearPath();
std::list<std::pair<int, Transform> > computePath(int targetNode);
const std::vector<int> & getPath() const {return _path;}
std::list<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;
int getPathGoalId() const;
std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;

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

@@ -140,6 +140,7 @@ public:
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
// getters
bool extended() const {return _extended;}
@@ -158,6 +159,7 @@ public:
const std::map<int, float> & posterior() const {return _posterior;}
const std::map<int, float> & likelihood() const {return _likelihood;}
const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
const std::vector<int> & localPath() const {return _localPath;}
const std::map<std::string, float> & data() const {return _data;}
@@ -184,6 +186,8 @@ private:
std::map<int, float> _likelihood;
std::map<int, float> _rawLikelihood;
std::vector<int> _localPath;
// Format for statistics (Plottable statistics must go in that map) :
// {"Group/Name/Unit", value}
// Example : {"Timing/Total time/ms", 500.0f}

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