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

rtabmap: fixed graph re-optimized with only virtual links in localization mode (which can make gtsam crash because of under constrained covariance)

Browse Source
This commit is contained in:
matlabbe
2021-12-12 13:07:02 -05:00
parent cf5e90238b
commit 0e62050824
1 changed files with 14 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/Rtabmap.cpp
View File

@@ -2815,6 +2815,19 @@ bool Rtabmap::process(
cv::Mat localizationCovariance; cv::Mat localizationCovariance;
Transform previousMapCorrection; Transform previousMapCorrection;
bool rejectedLandmark = false; bool rejectedLandmark = false;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
UDEBUG("Last prox: %d", lastProximitySpaceClosureId);
UDEBUG("Reduced ids: %d", (int)statistics_.reducedIds().size());
UDEBUG("Has prior: %d (prior ignored=%d)", signature->hasLink(signature->id(), Link::kPosePrior)?1:0, _graphOptimizer->priorsIgnored()?1:0);
UDEBUG("Has gravity: %d (sigma=%f, odomGravity=%d, refined=%d)", signature->hasLink(signature->id(), Link::kGravity)?1:0, _graphOptimizer->gravitySigma(), _memory->isOdomGravityUsed()?1:0, neighborLinkRefined?1:0);
UDEBUG("Has virtual link: %d", (int)graph::filterLinks(signature->getLinks(), Link::kVirtualClosure, true).size());
UDEBUG("Prox Time: %d", proximityDetectionsInTimeFound);
UDEBUG("Landmarks: %d", (int)landmarksDetected.size());
UDEBUG("Retrieved: %d", (int)signaturesRetrieved.size());
UDEBUG("Not self ref links: %d", (int)graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size());
if(_rgbdSlamMode if(_rgbdSlamMode
&& &&
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
@@ -2828,7 +2841,7 @@ bool Rtabmap::process(
&& &&
(_memory->isIncremental() || (_memory->isIncremental() ||
// In localization mode, the new node should be linked to another node or a landmark already in the working memory // In localization mode, the new node should be linked to another node or a landmark already in the working memory
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() || graph::filterLinks(graph::filterLinks(signature->getLinks(), Link::kVirtualClosure), Link::kSelfRefLink).size() ||
!landmarksDetected.empty())) !landmarksDetected.empty()))
{ {
UASSERT(uContains(_optimizedPoses, signature->id())); UASSERT(uContains(_optimizedPoses, signature->id()));