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 Statisics: Loop/Optimization_max_error_from_id and Loop/Optimization_max_error_to_id to help with debugging rejected loop closures

Browse Source
This commit is contained in:
matlabbe
2026-04-16 17:39:58 -07:00
parent aa7546ec62
commit 79ff91d293
2 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Rtabmap.cpp
View File

@@ -3186,6 +3186,8 @@ bool Rtabmap::process(
float maxLinearErrorRatio = 0.0f;
float maxAngularError = 0.0f;
float maxAngularErrorRatio = 0.0f;
int maxLinearErrorFromId = 0;
int maxLinearErrorToId = 0;
double optimizationError = 0.0;
int optimizationIterations = 0;
Transform previousMapCorrection;
@@ -3354,6 +3356,8 @@ bool Rtabmap::process(
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxLinearError,
maxLinearLink->from(),
@@ -3504,6 +3508,8 @@ bool Rtabmap::process(
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxLinearError,
maxLinearLink->from(),
@@ -3854,6 +3860,8 @@ bool Rtabmap::process(
bool reject = false;
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{
@@ -4075,6 +4083,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxAngularError*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxAngularErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_from_id(), maxLinearErrorFromId);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_to_id(), maxLinearErrorToId);
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first);