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

Fixed #1227

Browse Source
This commit is contained in:
matlabbe
2024-03-02 16:17:10 -08:00
parent 33b875ecbf
commit 8019a2dc49
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Rtabmap.cpp
View File

@@ -3027,7 +3027,7 @@ bool Rtabmap::process(
// Landmark // Landmark
//============================================================ //============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark> std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty()) if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored())
{ {
bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0; bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0); UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
@@ -3527,6 +3527,7 @@ bool Rtabmap::process(
} }
// update localization links // update localization links
UASSERT(uContains(optPoses, signature->id()));
Transform newOptPoseInv = optPoses.at(signature->id()).inverse(); Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
{ {
@@ -3539,6 +3540,7 @@ bool Rtabmap::process(
else else
{ {
// Adjust with optimized poses, this will smooth the localization // Adjust with optimized poses, this will smooth the localization
UASSERT(uContains(optPoses, iter->first));
Transform newT = newOptPoseInv * optPoses.at(iter->first); Transform newT = newOptPoseInv * optPoses.at(iter->first);
UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());