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

rtabmap/dbreader: save odometry covariance in pose prior link when in localization mode and saving localization data.

Browse Source
This commit is contained in:
matlabbe
2022-02-06 20:17:46 -05:00
parent b44b212218
commit f584f42ea4
2 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+7
View File
@@ -3798,6 +3798,13 @@ bool Rtabmap::process(
_memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
}
// Localization mode and saving localization data: save odometry covariance in a prior link
// so that DBReader can republish the covariance of localization data
if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
{
_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
}
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
int signatureRemoved = _memory->cleanup();
if(signatureRemoved)