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

Localization: Fixed local map cleared when gravity links are added and no localization happened. Fixed "Word not found" error on initialization when Mem/InitWMWithAllNodes is true.

Browse Source
This commit is contained in:
matlabbe
2019-09-19 15:23:18 -04:00
parent 955660ba2a
commit c437375f08
2 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+5 -3
View File
@@ -2556,7 +2556,8 @@ bool Rtabmap::process(
double optimizationError = 0.0;
int optimizationIterations = 0;
cv::Mat localizationCovariance;
if(_rgbdSlamMode &&
if(_rgbdSlamMode
&&
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
@@ -2564,8 +2565,9 @@ bool Rtabmap::process(
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one
signaturesRetrieved.size()) // can be different map of the current one
&&
(_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())) // In localization mode, the new node should be linked
{
UASSERT(uContains(_optimizedPoses, signature->id()));