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

OptimizerG2O: removed fixed frame if a posePrior link is detected

Browse Source
This commit is contained in:
matlabbe
2017-05-24 15:03:27 -04:00
parent 6f9ac4b88b
commit 10c23e60b7
1 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/OptimizerG2O.cpp
View File

@@ -215,6 +215,16 @@ std::map<int, Transform> OptimizerG2O::optimize(
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
} }
// detect if there is a global pose prior set, if so remove rootId
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() == iter->second.to())
{
rootId = 0;
break;
}
}
UDEBUG("fill poses to g2o..."); UDEBUG("fill poses to g2o...");
std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)