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

Replaced all optimizeIncremental() by optimizeMultiSession() to fix GTSAM errors when merging multiple maps together. DBReader: always publish saved stamp. DBViewer: added actions to set covariance of all neighbor or loop closure links.

Browse Source
This commit is contained in:
matlabbe
2018-10-15 17:07:18 -04:00
parent b2c012d8bc
commit f1546c1fca
15 changed files with 285 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+66 -2
View File
@@ -276,7 +276,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
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());
UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
incGraph.insert(*iter);
@@ -310,7 +310,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
if(incGraph.empty())
{
UWARN("Failed incremental optimization...");
UWARN("Failed incremental optimization... last pose added is %d", iter->first);
break;
}
}
@@ -328,6 +328,70 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimizeMultiSession(
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;
if(poses.empty())
{
return incGraph;
}
UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
std::set<int> nextPoses;
nextPoses.insert(rootId);
UASSERT(uContains(poses, rootId));
while(!nextPoses.empty())
{
std::set<int> currentPoses = nextPoses;
nextPoses.clear();
for(std::set<int>::iterator iter=currentPoses.begin(); iter!=currentPoses.end(); ++iter)
{
int fromId = *iter;
if(incGraph.empty())
{
std::map<int, Transform>::const_iterator cter = poses.find(fromId);
UASSERT(cter != poses.end());
incGraph.insert(*cter);
}
std::list<Link> links = graph::findLinks(constraints, fromId);
for(std::list<Link>::iterator jter = links.begin(); jter!=links.end(); ++jter)
{
if(!uContains(incGraph, jter->to()))
{
incGraph.insert(std::make_pair(jter->to(), incGraph.at(jter->from()) * jter->transform()));
nextPoses.insert(jter->to());
}
}
}
}
UASSERT(!incGraph.empty());
if(intermediateGraphes)
{
intermediateGraphes->push_back(incGraph);
}
if(incGraph.size() != poses.size())
{
UWARN("Failed multi-session optimization, output poses (%d) != input poses (%d)", incGraph.size(), poses.size());
}
else
{
UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
incGraph.at(rootId) = poses.at(rootId);
return this->optimize(rootId, incGraph, constraints, 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,