Skip to content

Commit

Permalink
Added new parameter: RGBD/AggressiveLoopThr
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Jun 19, 2024
1 parent 7c601bb commit 4c18226
Show file tree
Hide file tree
Showing 6 changed files with 72 additions and 35 deletions.
1 change: 1 addition & 0 deletions app/android/jni/RTABMapApp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), 0.0f));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/Rtabmap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,7 @@ class RTABMAP_CORE_EXPORT Rtabmap
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;
float _loopRatio;
float _aggressiveLoopThr;
int _virtualPlaceLikelihoodRatio;
float _maxLoopClosureDistance;
bool _verifyLoopClosureHypothesis;
Expand Down
25 changes: 16 additions & 9 deletions corelib/src/Rtabmap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()),
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
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)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
_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;
Expand Down
1 change: 1 addition & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1010,6 +1010,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

// Create hypotheses
_ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str());
_ui->general_doubleSpinBox_agressiveThr->setObjectName(Parameters::kRGBDAggressiveLoopThr().c_str());
_ui->general_doubleSpinBox_loopRatio->setObjectName(Parameters::kRtabmapLoopRatio().c_str());
_ui->comboBox_virtualPlaceLikelihoodRatio->setObjectName(Parameters::kRtabmapVirtualPlaceLikelihoodRatio().c_str());
_ui->comboBox_globalDescriptorExtractor->setObjectName(Parameters::kMemGlobalDescriptorStrategy().c_str());
Expand Down
78 changes: 52 additions & 26 deletions guilib/src/ui/preferencesDialog.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-640</y>
<y>0</y>
<width>713</width>
<height>4653</height>
</rect>
Expand Down Expand Up @@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>7</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
Expand Down Expand Up @@ -9538,6 +9538,45 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>Thresholds</string>
</property>
<layout class="QGridLayout" name="gridLayout_44" columnstretch="0,1">
<item row="1" column="1">
<widget class="QLabel" name="label_maxWmSize">
<property name="text">
<string>Maximum signatures allowed in Working Memory (0 means inf).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_96">
<property name="text">
<string>T_ratio : The loop closure hypothesis must be over T_ratio x lastHypothesisValue.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_hardThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.950000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_timeThr">
<property name="suffix">
Expand Down Expand Up @@ -9580,10 +9619,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_maxWmSize">
<item row="2" column="1">
<widget class="QLabel" name="label_93">
<property name="text">
<string>Maximum signatures allowed in Working Memory (0 means inf).</string>
<string>T_loop : Loop closure threshold. Setting to 0 means that the new place hypothesis is used as threshold.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
Expand All @@ -9593,23 +9632,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_hardThr">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_loopRatio">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.950000000000000</double>
<double>0.700000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_93">
<item row="3" column="1">
<widget class="QLabel" name="label_332">
<property name="text">
<string>T_loop : Loop closure threshold. Setting to 0 means that the new place hypothesis is used as threshold.</string>
<string>Loop closure threshold used (overriding T_loop) 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 (see &quot;Max odometry cache size&quot; under RGB-D SLAM panel). In all cases, the goal is to aggressively loop on a previous map in the database that is not linked to current map. Only used when RGB-D SLAM mode is enabled. Set 1 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
Expand All @@ -9620,28 +9659,15 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_loopRatio">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_agressiveThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.700000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_96">
<property name="text">
<string>T_ratio : The loop closure hypothesis must be over T_ratio x lastHypothesisValue.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<double>0.050000000000000</double>
</property>
</widget>
</item>
Expand Down

0 comments on commit 4c18226

Please sign in to comment.