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

Tango: fixed black clouds. Rtabmap: only add latest optimized pose to graph if localized.

Browse Source
This commit is contained in:
matlabbe
2016-12-05 13:27:29 -05:00
parent 3c17049084
commit e5524ddc1d
6 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Rtabmap.cpp
View File

@@ -2351,10 +2351,11 @@ bool Rtabmap::process(
}
Signature lastSignatureData(signature->id());
Transform lastSignatureOptimizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && signature->getLinks().size())
{
lastSignatureOptimizedPose = _optimizedPoses.at(signature->id());
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
}
if(_publishLastSignatureData)
{
@@ -2578,7 +2579,10 @@ bool Rtabmap::process(
groundTruth)));
}
localGraphSize = (int)poses.size();
poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureOptimizedPose)); // in case we are in localization
if(!lastSignatureLocalizedPose.isNull())
{
poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose)); // in case we are in localization
}
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
statistics_.setSignatures(signatures);