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: update lastLocalizationNodeId even when only a landmark is detected

Browse Source
This commit is contained in:
matlabbe
2019-01-15 21:34:01 -05:00
parent 95f48b694e
commit 479fb6cbca
1 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/Rtabmap.cpp
View File

@@ -316,6 +316,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
Transform lastPose; Transform lastPose;
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose); _optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), lastPose.prettyPrint().c_str());
if(!_optimizedPoses.empty()) if(!_optimizedPoses.empty())
{ {
if(!_savedLocalizationIgnored) if(!_savedLocalizationIgnored)
@@ -2672,7 +2673,19 @@ bool Rtabmap::process(
} }
} }
} }
_lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:_lastLocalizationNodeId; int newLocId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId>0?lastProximitySpaceClosureId:0;
_lastLocalizationNodeId = newLocId!=0?newLocId:_lastLocalizationNodeId;
if(newLocId==0 && landmarkDetected!=0)
{
std::map<int, std::set<int> >::const_iterator iter = _memory->getLandmarksInvertedIndex().find(landmarkDetected);
if(iter!=_memory->getLandmarksInvertedIndex().end())
{
if(iter->second.size() && *iter->second.begin()!=signature->id())
{
_lastLocalizationNodeId = *iter->second.begin();
}
}
}
timeMapOptimization = timer.ticks(); timeMapOptimization = timer.ticks();
ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization); ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
@@ -2923,7 +2936,7 @@ bool Rtabmap::process(
//Remove optimized poses from signatures transferred //Remove optimized poses from signatures transferred
if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size())) if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
{ {
//refresh the local map because some transferred nodes may have broken the tree //refresh the local map because some transferred nodes may have broken the tree
int id = 0; int id = 0;
if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size())) if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size()))