i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added new parameter: RGBD/AggressiveLoopThr

Browse Source
This commit is contained in:
matlabbe
2024-06-19 10:28:17 -07:00
parent 7c601bb6e8
commit 4c1822683a
6 changed files with 78 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/src/Rtabmap.cpp
View File

@@ -103,6 +103,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_aggressiveLoopThr(Parameters::defaultRGBDAggressiveLoopThr()),
_virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
@@ -568,6 +569,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRGBDAggressiveLoopThr(), _aggressiveLoopThr);
Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
@@ -2104,21 +2106,26 @@ bool Rtabmap::process(
if(_highestHypothesis.first > 0)
{
float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
bool hasLoopClosureConstraints = false;
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end() && !hasLoopClosureConstraints; ++iter)
{
hasLoopClosureConstraints =
iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
iter->second.type() == Link::kLandmark;
}
if( (( _memory->isIncremental() && !uContains(_optimizedPoses, _highestHypothesis.first) == 0) || // not linked to previous map of that hypothesis
(!_memory->isIncremental() && !hasLoopClosureConstraints)) && // not yet localized to any previous sessions
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
_rgbdSlamMode &&
loopThr > _aggressiveLoopThr)
{
// If the map is very small (under STM size) and we need to find
// a loop closure before continuing the map or localizing,
// use the best hypothesis directly.
loopThr = 0.0f;
UDEBUG("Using %s=%f", Parameters::kRGBDAggressiveLoopThr().c_str(), _aggressiveLoopThr);
loopThr = _aggressiveLoopThr;
}
// Loop closure Threshold
// When _loopThr=0, accept loop closure if the hypothesis is over
// the virtual (new) place hypothesis.
if(_highestHypothesis.second >= loopThr)
{
rejectedGlobalLoopClosure = true;