Added confidence threshold parameter

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@310 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Parameters.h

@@ -133,6 +133,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,       unsigned int, 2); // Maximum locations retrieved at the same time from LTM
 	RTABMAP_PARAM(Rtabmap, ActionsByTime,       bool, false); // Select next actions using directly the more recent neighbor of the current node, otherwise, highest hypothesis is used
 	RTABMAP_PARAM(Rtabmap, ActionsSentRejectHyp,  bool, false); // Actions sent also on rejected hypotheses (on decreasing hypotheses)
+	RTABMAP_PARAM(Rtabmap, ConfidenceThr, 		float, 0.0); // Actions are not sent when the loop closure hypothesis is under the confidence threshold
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.10); // Loop closing threshold

corelib/include/rtabmap/core/Rtabmap.h

@@ -135,6 +135,7 @@ private:
 	unsigned int _maxRetrieved;
 	bool _actionsByTime;
 	bool _actionsSentRejectHyp;
+	float _confidenceThr;
 
 	int _lcHypothesisId;
 	int _reactivateId;

corelib/src/Rtabmap.cpp

@@ -65,6 +65,7 @@ Rtabmap::Rtabmap() :
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
 	_actionsByTime(Parameters::defaultRtabmapActionsByTime()),
 	_actionsSentRejectHyp(Parameters::defaultRtabmapActionsSentRejectHyp()),
+	_confidenceThr(Parameters::defaultRtabmapConfidenceThr()),
 	_lcHypothesisId(0),
 	_reactivateId(0),
 	_highestHypothesisValue(0),
@@ -283,6 +284,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	{
 		_actionsSentRejectHyp = uStr2Bool(iter->second.c_str());
 	}
+	if((iter=parameters.find(Parameters::kRtabmapConfidenceThr())) != parameters.end())
+	{
+		_confidenceThr = std::atof(iter->second.c_str());
+	}
 
 	// By default, we create our strategies if they are not already created.
 	// If they already exists, we check the parameters if a change is requested
@@ -946,7 +951,7 @@ void Rtabmap::process()
 	}
 
 	// only send actions if rejectLoopReason!=3 (decreasing hypotheses)
-	if(sLoop && (_actionsSentRejectHyp || !rejectedHypothesis))
+	if(sLoop && (_actionsSentRejectHyp || !rejectedHypothesis) && (_highestHypothesisValue > _confidenceThr))
 	{
 		// select the actions of the neighbor with the highest
 		// weight (select the more recent if some have the same weight)

guilib/src/PreferencesDialog.cpp

@@ -113,6 +113,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str());
 	_ui->general_checkBox_actionsByTime->setObjectName(Parameters::kRtabmapActionsByTime().c_str());
 	_ui->general_checkBox_actionsSentRejectHyp->setObjectName(Parameters::kRtabmapActionsSentRejectHyp().c_str());
+	_ui->general_doubleSpinBox_confidenceThr->setObjectName(Parameters::kRtabmapConfidenceThr().c_str());
 	_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
 	connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
 

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
           <enum>QFrame::Raised</enum>
          </property>
          <property name="currentIndex">
-          <number>0</number>
+          <number>2</number>
          </property>
          <widget class="QWidget" name="page_22">
           <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -806,7 +806,7 @@ when using the file type, logs are saved in Logrtabmap.txt (located in the worki
              <property name="title">
               <string>0RTAB-Map settings</string>
              </property>
-             <layout class="QGridLayout" name="gridLayout_4" columnstretch="1,0,10">
+             <layout class="QGridLayout" name="gridLayout_4" columnstretch="1,0,0">
               <item row="0" column="0">
                <widget class="QCheckBox" name="general_checkBox_publishStats">
                 <property name="text">
@@ -930,21 +930,21 @@ when using the file type, logs are saved in Logrtabmap.txt (located in the worki
                 </property>
                </widget>
               </item>
-              <item row="9" column="0">
+              <item row="10" column="0">
                <widget class="QLineEdit" name="lineEdit_workingDirectory">
                 <property name="readOnly">
                  <bool>true</bool>
                 </property>
                </widget>
               </item>
-              <item row="9" column="2">
+              <item row="10" column="2">
                <widget class="QLabel" name="label_87">
                 <property name="text">
                  <string>Working directory (where the database is saved/loaded)</string>
                 </property>
                </widget>
               </item>
-              <item row="9" column="1">
+              <item row="10" column="1">
                <widget class="QToolButton" name="toolButton_workingDirectory">
                 <property name="text">
                  <string>...</string>
@@ -1014,6 +1014,29 @@ when using the file type, logs are saved in Logrtabmap.txt (located in the worki
                 </property>
                </widget>
               </item>
+              <item row="9" column="2">
+               <widget class="QLabel" name="label_52">
+                <property name="text">
+                 <string>Actions are not sent when the loop closure hypothesis is under the confidence threshold</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="9" column="0">
+               <widget class="QDoubleSpinBox" name="general_doubleSpinBox_confidenceThr">
+                <property name="maximum">
+                 <double>1.000000000000000</double>
+                </property>
+                <property name="singleStep">
+                 <double>0.010000000000000</double>
+                </property>
+                <property name="value">
+                 <double>0.000000000000000</double>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>
@@ -1110,7 +1133,7 @@ We can edit how will like the prediction used in the Bayes filter when there is
                     <rect>
                      <x>0</x>
                      <y>0</y>
-                     <width>98</width>
+                     <width>28</width>
                      <height>71</height>
                     </rect>
                    </property>