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

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