Added Odom/VarianceFromInliersCount parameter (default false)

corelib/include/rtabmap/core/OdometryThread.h

@@ -41,7 +41,7 @@ class Odometry;
 class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
 public:
 	// take ownership of Odometry
-	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
+	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1, bool varianceFromInliersCount = false);
 	virtual ~OdometryThread();
 
 protected:
@@ -63,6 +63,7 @@ private:
 	std::list<SensorData> _dataBuffer;
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
+	bool _varianceFromInliersCount;
 	bool _resetOdometry;
 };
 

corelib/include/rtabmap/core/Parameters.h

@@ -207,7 +207,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");
-	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"");
+	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM_COND(Kp, NndrRatio, 	     float, RTABMAP_NONFREE, 0.8, 0.9,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 	RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
@@ -331,6 +331,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
 	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
+	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
 	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 5.0,     "PnP reprojection error.");
 	RTABMAP_PARAM(Odom, PnPFlags, 				int, 1,    	     "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");

corelib/src/OdometryThread.cpp

@@ -34,9 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
+OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize, bool varianceFromInliersCount) :
 	_odometry(odometry),
 	_dataBufferMaxSize(dataBufferMaxSize),
+	_varianceFromInliersCount(varianceFromInliersCount),
 	_resetOdometry(false)
 {
 	UASSERT(_odometry != 0);
@@ -98,7 +99,17 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
-		double variance = info.variance>0?info.variance:1;
+
+		double variance;
+		if(_varianceFromInliersCount)
+		{
+			variance = info.inliers > 0?1.0/double(info.inliers):1.0;
+		}
+		else
+		{
+			variance = info.variance>0?info.variance:1.0;
+		}
+
 		this->post(new OdometryEvent(data, pose, variance, variance, info));
 	}
 }

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -208,6 +208,7 @@ public:
 	double getSimThr() const;
 	int getOdomStrategy() const;
 	int getOdomBufferSize() const;
+	bool getOdomVarianceFromInliersCount() const;
 	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//

guilib/src/MainWindow.cpp

@@ -2931,7 +2931,7 @@ void MainWindow::startDetection()
 				{
 					odom = new OdometryBOW(parameters);
 				}
-				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
+				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize(), _preferencesDialog->getOdomVarianceFromInliersCount());
 
 				UEventsManager::addHandler(_odomThread);
 				UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
@@ -2968,7 +2968,7 @@ void MainWindow::startDetection()
 			{
 				odom = new OdometryBOW(parameters);
 			}
-			_odomThread = new OdometryThread(odom);
+			_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize(), _preferencesDialog->getOdomVarianceFromInliersCount());
 
 			UEventsManager::addHandler(_odomThread);
 			_odomThread->start();

guilib/src/PreferencesDialog.cpp

@@ -650,6 +650,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
+	_ui->odom_varianceFromInliersCount->setObjectName(Parameters::kOdomVarianceFromInliersCount().c_str());
 	_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
 	_ui->odom_estimationType->setObjectName(Parameters::kOdomEstimationType().c_str());
 	connect(_ui->odom_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odomEstimation, SLOT(setCurrentIndex(int)));
@@ -3744,6 +3745,10 @@ int PreferencesDialog::getOdomBufferSize() const
 {
 	return _ui->odom_dataBufferSize->value();
 }
+bool PreferencesDialog::getOdomVarianceFromInliersCount() const
+{
+	return _ui->odom_varianceFromInliersCount->isChecked();
+}
 
 QString PreferencesDialog::getCameraInfoDir() const
 {
@@ -3883,7 +3888,8 @@ void PreferencesDialog::testOdometry(int type)
 
 	OdometryThread odomThread(
 			odometry, // take ownership of odometry
-			_ui->odom_dataBufferSize->value());
+			_ui->odom_dataBufferSize->value(),
+			_ui->odom_varianceFromInliersCount->isChecked());
 	odomThread.registerToEventsManager();
 
 	OdometryViewer * odomViewer = new OdometryViewer(10,

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1094</y>
+            <y>0</y>
-            <width>760</width>
+            <width>755</width>
-            <height>1655</height>
+            <height>1715</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>23</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -8476,7 +8476,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -8522,7 +8522,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
@@ -8584,13 +8584,33 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="0">
+                      <item row="9" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test selected odometry</string>
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_261">
+                        <property name="text">
+                         <string>Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="odom_varianceFromInliersCount">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>