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

GTSAM: return empty optimized poses in case of exception or error. g2o: don't assert on marginal not computed, log error and reutn empty optimized poses.

Browse Source
This commit is contained in:
matlabbe
2022-11-22 12:26:02 -08:00
parent 05676d1577
commit 85bc1ee27f
2 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -950,14 +950,17 @@ std::map<int, Transform> OptimizerG2O::optimize(
UDEBUG("Initial optimization..."); UDEBUG("Initial optimization...");
optimizer.initializeOptimization(); optimizer.initializeOptimization();
UASSERT_MSG(optimizer.verifyInformationMatrices(true), if(!optimizer.verifyInformationMatrices(true))
"This error can be caused by (1) bad covariance matrix " {
UERROR("This error can be caused by (1) bad covariance matrix "
"set in odometry messages " "set in odometry messages "
"(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) " "(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
"or that (2) PCL and g2o hadn't " "or that (2) PCL and g2o hadn't "
"been built both with or without \"-march=native\" compilation " "been built both with or without \"-march=native\" compilation "
"flag (if one library is built with this flag and not the other, " "flag (if one library is built with this flag and not the other, "
"this is causing Eigen to not work properly, resulting in segmentation faults)."); "this is causing Eigen to not work properly, resulting in segmentation faults).");
return optimizedPoses;
}
UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
int it = 0; int it = 0;
@@ -1085,7 +1088,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0) if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
{ {
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!"); UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses; return optimizedPoses;
} }
@@ -1128,7 +1131,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(optimizer.activeRobustChi2() > 1000000000000.0) if(optimizer.activeRobustChi2() > 1000000000000.0)
{ {
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!"); UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses; return optimizedPoses;
} }

9
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -714,16 +714,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
} }
else else
{ {
UWARN("GTSAM: Could not compute marginal covariance!"); UERROR("GTSAM: Could not compute marginal covariance!");
optimizedPoses.clear(); // Failed optimization
} }
} }
catch(gtsam::IndeterminantLinearSystemException & e) catch(gtsam::IndeterminantLinearSystemException & e)
{ {
UWARN("GTSAM exception caught: %s", e.what()); UERROR("GTSAM exception caught: %s", e.what());
optimizedPoses.clear(); // Failed optimization
} }
catch(std::exception& e) catch(std::exception& e)
{ {
UWARN("GTSAM exception caught: %s", e.what()); UERROR("GTSAM exception caught: %s", e.what());
optimizedPoses.clear(); // Failed optimization
} }
delete optimizer; delete optimizer;