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

g2o: fixed iterative optimization (when epsilon>0) not converging as fast as like batch optimization (and not converging at all in some cases).

Browse Source
This commit is contained in:
matlabbe
2025-01-27 14:20:41 -08:00
parent 3ab9a69fbb
commit 6aa188d2af
1 changed files with 14 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -1037,6 +1037,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
int it = 0;
UTimer timer;
double lastError = 0.0;
if (!optimizer.solver()->init()) {
UERROR("g2o: Error while initializing solver");
return optimizedPoses;
}
if(intermediateGraphes || this->epsilon() > 0.0)
{
for(int i=0; i<iterations(); ++i)
@@ -1150,7 +1156,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
it += optimizer.optimize(1);
g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i);
++it;
// early stop condition
optimizer.computeActiveErrors();
@@ -1163,6 +1170,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
return optimizedPoses;
}
if(result == g2o::OptimizationAlgorithm::Fail)
{
UERROR("g2o: Solver failed, aborting optimization!");
return optimizedPoses;
}
double errorDelta = lastError - chi2;
if(i>0 && errorDelta < this->epsilon())
{