i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -357,6 +357,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, LinearSpeedUpdate, float, 0.0, "Maximum linear speed (m/s) to update the map (0 means not limit).");
RTABMAP_PARAM(RGBD, AngularSpeedUpdate, float, 0.0, "Maximum angular speed (rad/s) to update the map (0 means not limit).");
RTABMAP_PARAM(RGBD, AggressiveLoopThr, float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -284,6 +284,7 @@ private:
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;
float _loopRatio;
float _aggressiveLoopThr;
int _virtualPlaceLikelihoodRatio;
float _maxLoopClosureDistance;
bool _verifyLoopClosureHypothesis;

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;