i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 04:49:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

OptimizerGTSAM: keep iterating even if error is very large (aarch64 issue), but only when Optimizer/Epsilon is 0.

Browse Source
This commit is contained in:
matlabbe
2025-01-17 10:31:06 -08:00
parent f8b1d50656
commit 853f603898
1 changed files with 19 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerGTSAM.cpp
+18 -6
View File
@@ -924,11 +924,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
// early stop condition // early stop condition
UDEBUG("iteration %d error =%f", i+1, error); UDEBUG("iteration %d error =%f", i+1, error);
double errorDelta = lastError - error; double errorDelta = lastError - error;
if(fabs(error) > 1000000000000.0) if(this->epsilon() > 0.0 && fabs(error) > 1000000000000.0)
{ {
UERROR("Error computed (%e) is very huge! There could be an issue with too small " UERROR("Error computed (%e) is very huge! There could be an issue with too small "
"variance set in some factors (set log level to debug to print the values). Aborting!", error); "variance set in some factors (set log level to debug to print the values). Aborting! "
if(ULogger::level() >= ULogger::kDebug) "Set %s to 0 to ignore that check and keep iterating up to %s (%d).",
error,
Parameters::kOptimizerEpsilon().c_str(),
Parameters::kOptimizerIterations().c_str(),
this->iterations());
if(ULogger::level() == ULogger::kDebug)
{ {
if(optimizer) if(optimizer)
graph.printErrors(optimizer->values()); graph.printErrors(optimizer->values());
@@ -951,10 +956,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
break; break;
} }
} }
else if(i==0 && error < this->epsilon()) else if(i==0)
{ {
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon()); if(error < 0)
break; {
UDEBUG("Negative error?! Ignore and continue optimizing... (%f)", error);
}
else if(error < this->epsilon())
{
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
break;
}
} }
} }
lastError = error; lastError = error;