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

Added ground truth localization error stats

Browse Source
This commit is contained in:
matlabbe
2019-06-26 16:21:03 -04:00
parent 9d8fd364e3
commit 49977b5475
2 changed files with 18 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -171,6 +171,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Gt, Rotational_std, deg);
RTABMAP_STATS(Gt, Rotational_min, deg);
RTABMAP_STATS(Gt, Rotational_max, deg);
RTABMAP_STATS(Gt, Localization_linear_error, m);
RTABMAP_STATS(Gt, Localization_angular_error, deg);
public:
static const std::map<std::string, float> & defaultData();

19
corelib/src/Rtabmap.cpp
View File

@@ -3077,10 +3077,23 @@ bool Rtabmap::process(
statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().find(signature->id())->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().find(signature->id())->second.transform());
// Loop closure transform
UASSERT(sLoop);
std::multimap<int, Link>::const_iterator loopIter = sLoop->getLinks().find(signature->id());
UASSERT(loopIter!=sLoop->getLinks().end());
UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(loopIter->second.transform());
// if ground truth exists, compute localization error
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
Transform error = loopIter->second.transform().inverse() * transformGT;
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
}
// Map correction (/map -> /odom)
statistics_.addStatistic(Statistics::kLoopMap_correction_norm(), _mapCorrection.getNorm());
float roll,pitch,yaw;
_mapCorrection.getEulerAngles(roll, pitch, yaw);