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

Statistics: added LoopOdom_correction stats for landmark detections.

Browse Source
This commit is contained in:
matlabbe
2021-12-10 21:58:11 -05:00
parent 8cf12c6135
commit cf5e90238b
2 changed files with 43 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -325,7 +325,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o...");
UDEBUG("fill poses to g2o... (rootId=%d)", rootId);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -339,6 +339,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v2->setFixed(true);
}
vertex = v2;
@@ -361,6 +362,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v2->setFixed(true);
}
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
@@ -384,6 +390,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
v3->setEstimate(pose);
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v3->setFixed(true);
}
vertex = v3;
@@ -412,6 +419,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v3->setFixed(true);
}
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;