i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

60
corelib/src/Optimizer.cpp
View File

@@ -263,6 +263,66 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
}
std::map<int, Transform> Optimizer::optimizeIncremental(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
std::map<int, Transform> incGraph;
std::multimap<int, Link> incGraphLinks;
incGraph.insert(*poses.begin());
int i=0;
std::multimap<int, Link> constraintsCpy = constraints;
UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
bool hasLoopClosure = false;
for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
{
if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
{
incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
incGraphLinks.insert(*jter);
}
else
{
if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
{
// node not yet in graph, switch link direction
constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
}
else
{
UASSERT(uContains(incGraph, jter->second.to()));
incGraphLinks.insert(*jter);
hasLoopClosure = true;
}
}
}
if(hasLoopClosure)
{
incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
if(incGraph.empty())
{
UWARN("Failed incremental optimization...");
break;
}
}
UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
}
if(!incGraph.empty() && incGraph.size() == poses.size())
{
UASSERT(incGraphLinks.size() == constraints.size());
return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
}
UDEBUG("Failed incremental optimization");
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,

23
corelib/src/Rtabmap.cpp
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());