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

Optimizer: added optimizeIncremental() method to solve optimization error problems when re-opening the database (when graph should be re-optimized from raw odometry poses, not latest optimized poses, #172)

Browse Source
This commit is contained in:
matlabbe
2018-02-13 18:18:42 -05:00
parent 02a64a7fa3
commit bfb3a58c01
5 changed files with 139 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+23
View File
@@ -3254,6 +3254,29 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
else
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
{
UERROR("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
if(optimizedPoses.empty())
{
if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
{
UERROR("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
}
else
{
UERROR("Incremental optimization also failed.");
}
}
else
{
UERROR("Incremental optimization succeeded!");
}
}
}
UINFO("Optimization time %f s", timer.ticks());