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

GUI: Added Multi-Session Localization view. CameraModel: set localTransform to optical rotation in default constructor (fixed matrix invertion errors with code ignoring setting local transform), save/read local transform in/from camera calibration yaml file (so that local transform is also exported when extracting rgb/depth images). Rtabmap: in localization mode, ignore landmarks farther than RGBD/LocalRadius if no global loop closures are already in odometry cache.

Browse Source
This commit is contained in:
matlabbe
2022-02-12 13:35:23 -05:00
parent f584f42ea4
commit 191e165a28
13 changed files with 612 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+36 -18
View File
@@ -2348,23 +2348,6 @@ bool Rtabmap::process(
ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
}
//============================================================
// Landmark
//============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksIndex(), iter->first) &&
_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
{
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
}
}
}
//============================================================
// Proximity detections
//============================================================
@@ -2832,6 +2815,41 @@ bool Rtabmap::process(
timeAddLoopClosureLink = timer.ticks();
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
//============================================================
// Landmark
//============================================================
std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
if(!signature->getLandmarks().empty())
{
bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksIndex(), iter->first) &&
_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
{
if(!_memory->isIncremental() && // In localization mode
!hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks
_localRadius>0.0 &&
iter->second.transform().getNormSquared() > _localRadius*_localRadius)
{
// Ignore landmark detections over local radius
UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
"and odom cache doesn't contain global loop closure(s).",
iter->first,
iter->second.transform().getNorm(),
Parameters::kRGBDLocalRadius().c_str(),
_localRadius);
}
else
{
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
}
}
}
}
//============================================================
// Add virtual links if a path is activated
//============================================================
@@ -3064,7 +3082,7 @@ bool Rtabmap::process(
}
bool hasGlobalLoopClosuresOrLandmarks = false;
if(rejectLocalization)
if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
{
// Let's try again without local loop closures
localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);