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

GraphOptimizerGTSAM: Fixed not returned poses when there is only one

Browse Source
This commit is contained in:
matlabbe
2015-09-15 16:36:28 -04:00
parent ce025d1375
commit 16d998f3b0
2 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Graph.cpp
View File

@@ -202,7 +202,7 @@ std::map<int, Transform> Optimizer::optimize(
const std::multimap<int, Link> & constraints, const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes) std::list<std::map<int, Transform> > * intermediateGraphes)
{ {
UERROR("Optimizer %d doesn't implement optimize() method. See optimizeBA().", (int)this->type()); UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
return std::map<int, Transform>(); return std::map<int, Transform>();
} }
@@ -212,7 +212,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
const std::multimap<int, Link> & links, const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures) const std::map<int, Signature> & signatures)
{ {
UERROR("Optimizer %d doesn't implement optimizeBA() method. See optimize().", (int)this->type()); UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
return std::map<int, Transform>(); return std::map<int, Transform>();
} }
@@ -1348,6 +1348,14 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
} }
} }
} }
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!"); UDEBUG("Optimizing graph...end!");
#else #else
UERROR("Not built with GTSAM support!"); UERROR("Not built with GTSAM support!");

1
corelib/src/Rtabmap.cpp
View File

@@ -2013,6 +2013,7 @@ bool Rtabmap::process(
//optimize the path's poses locally //optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), false); path = optimizeGraph(nearestId, uKeysSet(path), false);
// transform local poses in optimized graph referential // transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse(); Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter) for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{ {