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

Added parameter Rtabmap/VirtualPlaceLikelihoodRatio to select how likelihood is normalized/adjusted (0=default old approach used in paper).

Browse Source
This commit is contained in:
matlabbe
2024-05-19 17:46:00 -07:00
parent c034a9631c
commit 995f65b2ec
5 changed files with 103 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -197,6 +197,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold."); RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold.");
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0, "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue."); RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0, "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
RTABMAP_PARAM(Rtabmap, LoopGPS, bool, true, uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str())); RTABMAP_PARAM(Rtabmap, LoopGPS, bool, true, uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
RTABMAP_PARAM(Rtabmap, VirtualPlaceLikelihoodRatio, int, 0, "Likelihood ratio for virtual place (for no loop closure hypothesis): 0=Mean / StdDev, 1=StdDev / (Max-Mean)");
// Memory // Memory
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity."); RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity.");

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

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

38
corelib/src/Rtabmap.cpp
View File

@@ -103,6 +103,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()), _loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()), _loopRatio(Parameters::defaultRtabmapLoopRatio()),
_virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()), _maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()), _verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()), _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
@@ -567,6 +568,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed); Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr); Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio); Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance); Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis); Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved); Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
@@ -5275,36 +5278,37 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
//Adjust likelihood with mean and standard deviation (see Angeli phd) //Adjust likelihood with mean and standard deviation (see Angeli phd)
float epsilon = 0.0001;
float max = 0.0f; float max = 0.0f;
int maxId = 0; int maxId = 0;
for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter) for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
{ {
float value = iter->second; float value = iter->second;
if(value > mean+stdDev && stdDev) iter->second = 1.0f;
if(value > mean+stdDev)
{ {
iter->second = (value-mean)/stdDev; if(_virtualPlaceLikelihoodRatio==0 && mean)
if(value > max)
{ {
max = value; iter->second = (value-(stdDev-epsilon))/mean;
maxId = iter->first; }
else if(_virtualPlaceLikelihoodRatio!=0 && stdDev)
{
iter->second = (value-mean)/stdDev;
} }
} }
else if(value == 1.0f && stdDev == 0)
if(value > max)
{ {
iter->second = 1.0f; max = value;
if(value > max) maxId = iter->first;
{
max = value;
maxId = iter->first;
}
}
else
{
iter->second = 1.0f;
} }
} }
if(max > mean) if(_virtualPlaceLikelihoodRatio==0 && stdDev > epsilon && max)
{
likelihood.begin()->second = mean/stdDev + 1.0f;
}
else if(_virtualPlaceLikelihoodRatio!=0 && max > mean)
{ {
likelihood.begin()->second = stdDev/(max-mean) + 1.0f; likelihood.begin()->second = stdDev/(max-mean) + 1.0f;
} }

1
guilib/src/PreferencesDialog.cpp
View File

@@ -1006,6 +1006,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// Create hypotheses // Create hypotheses
_ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str()); _ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str());
_ui->general_doubleSpinBox_loopRatio->setObjectName(Parameters::kRtabmapLoopRatio().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()); _ui->comboBox_globalDescriptorExtractor->setObjectName(Parameters::kMemGlobalDescriptorStrategy().c_str());
connect(_ui->comboBox_globalDescriptorExtractor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGlobalDescriptorVisibility())); connect(_ui->comboBox_globalDescriptorExtractor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGlobalDescriptorVisibility()));

128
guilib/src/ui/preferencesDialog.ui
View File

@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>5</number> <number>11</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -11789,6 +11789,56 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
</item> </item>
<item> <item>
<layout class="QGridLayout" name="gridLayout_22" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_22" columnstretch="0,1">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_rtabmap_loopGPS">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QComboBox" name="comboBox_globalDescriptorExtractor">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Disabled</string>
</property>
</item>
<item>
<property name="text">
<string>PyDescriptor</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_142">
<property name="text">
<string>Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_502">
<property name="text">
<string>Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QCheckBox" name="checkBox_bayes_fullPredictionUpdate"> <widget class="QCheckBox" name="checkBox_bayes_fullPredictionUpdate">
<property name="text"> <property name="text">
@@ -11796,6 +11846,16 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_kp_tfIdfLikelihoodUsed">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_vp"> <widget class="QDoubleSpinBox" name="general_doubleSpinBox_vp">
<property name="maximum"> <property name="maximum">
@@ -11822,19 +11882,6 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="0" column="1">
<widget class="QLabel" name="label_142">
<property name="text">
<string>Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).</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="1"> <item row="2" column="1">
<widget class="QLabel" name="label_82"> <widget class="QLabel" name="label_82">
<property name="text"> <property name="text">
@@ -11848,37 +11895,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0"> <item row="5" column="1">
<widget class="QCheckBox" name="checkBox_kp_tfIdfLikelihoodUsed">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_502">
<property name="text">
<string>Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_rtabmap_loopGPS">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_6031"> <widget class="QLabel" name="label_6031">
<property name="text"> <property name="text">
<string>Global descriptor extractor.</string> <string>Global descriptor extractor.</string>
@@ -11891,19 +11908,32 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="1">
<widget class="QLabel" name="label_755">
<property name="text">
<string>Likelihood ratio for VP.</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="0"> <item row="4" column="0">
<widget class="QComboBox" name="comboBox_globalDescriptorExtractor"> <widget class="QComboBox" name="comboBox_virtualPlaceLikelihoodRatio">
<property name="sizeAdjustPolicy"> <property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum> <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property> </property>
<item> <item>
<property name="text"> <property name="text">
<string>Disabled</string> <string>Mean / StdDev</string>
</property> </property>
</item> </item>
<item> <item>
<property name="text"> <property name="text">
<string>PyDescriptor</string> <string>StdDev / (Max-Mean)</string>
</property> </property>
</item> </item>
</widget> </widget>