i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master to 0.11.0

Browse Source
This commit is contained in:
matlabbe
2015-12-02 11:55:46 -05:00
parent 676cd8cbd9 60f34fe994
commit 818eda005f
5 changed files with 61 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OptimizerG2O.cpp
+15
View File
@@ -302,6 +302,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
UDEBUG("Initial optimization...");
optimizer.initializeOptimization();
UASSERT(optimizer.verifyInformationMatrices());
UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
int it = 0;
UTimer timer;
@@ -361,6 +363,13 @@ std::map<int, Transform> OptimizerG2O::optimize(
optimizer.computeActiveErrors();
double chi2 = optimizer.activeRobustChi2();
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
double errorDelta = lastError - chi2;
if(i>0 && errorDelta < this->epsilon())
{
@@ -398,6 +407,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)