Added odometry honolomic parameter

corelib/include/rtabmap/core/Odometry.h

@@ -77,6 +77,7 @@ private:
 	float _maxDepth;
 	int _resetCountdown;
 	bool _force2D;
+	bool _holonomic;
 	bool _particleFiltering;
 	int _particleSize;
 	float _particleNoiseT;

corelib/include/rtabmap/core/Parameters.h

@@ -322,6 +322,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	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, PnPEstimation, 		    bool, false,     "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");

corelib/src/Odometry.cpp

@@ -43,6 +43,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultOdomForce2D()),
+		_holonomic(Parameters::defaultOdomHolonomic()),
 		_particleFiltering(Parameters::defaultOdomParticleFiltering()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
 		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -66,6 +67,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
+	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation);
 	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
@@ -191,7 +193,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		if(_force2D || filters_.size())
+		if(_force2D || !_holonomic || filters_.size())
 		{
 			float x,y,z, roll,pitch,yaw;
 			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
@@ -211,8 +213,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 				else
 				{
 					x = filters_[0]->filter(x);
-					y = filters_[1]->filter(y);
 					yaw = filters_[5]->filter(yaw);
+					if(_holonomic)
+					{
+						y = filters_[1]->filter(y);
+					}
+					else
+					{
+						y = x * tan(yaw);
+					}
 
 					if(!_force2D)
 					{
@@ -227,13 +236,17 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 					info->timeParticleFiltering = time.ticks();
 				}
 			}
+			else if(!_holonomic)
+			{
+				y = x * tan(yaw);
+			}
 			UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
 					uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
 					uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
 							x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
 			t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
 
-			if(info)
+			if(info && filters_.size())
 			{
 				info->transformFiltered = t;
 			}

guilib/src/PreferencesDialog.cpp

@@ -550,6 +550,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
 	_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
 	_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
+	_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->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());

guilib/src/ui/mainWindow.ui

@@ -55,6 +55,7 @@
      <property name="title">
       <string>Advanced</string>
      </property>
+     <addaction name="actionOpen_working_directory"/>
      <addaction name="actionDump_the_memory"/>
      <addaction name="actionDump_the_prediction_matrix"/>
      <addaction name="actionGenerate_map"/>
@@ -73,7 +74,6 @@
     <addaction name="separator"/>
     <addaction name="actionDelete_memory"/>
     <addaction name="separator"/>
-    <addaction name="actionOpen_working_directory"/>
     <addaction name="menuAdvanced"/>
    </widget>
    <widget class="QMenu" name="menu6">

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-450</y>
+            <y>0</y>
             <width>755</width>
             <height>1591</height>
            </rect>
@@ -6794,13 +6794,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </item>
                        </widget>
                       </item>
-                      <item row="5" column="0">
-                       <widget class="QSpinBox" name="odom_countdown">
-                        <property name="maximum">
-                         <number>999999</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_44">
                         <property name="text">
@@ -6838,7 +6831,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
+                      <item row="9" column="1">
                        <widget class="QLabel" name="label_221">
                         <property name="text">
                          <string>Fill info with data (inliers/outliers features to be shown in Odometry view).</string>
@@ -6848,7 +6841,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test selected odometry</string>
@@ -6875,7 +6868,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -6885,7 +6878,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
@@ -6902,7 +6895,7 @@ 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="QCheckBox" name="odom_fillInfoData">
                         <property name="text">
                          <string/>
@@ -6956,7 +6949,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </item>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_233">
                         <property name="text">
                          <string>Particle filtering to smooth the odometry trajectory. See &quot;Particle Filter&quot; panel for the related parameters.</string>
@@ -6966,13 +6959,37 @@ 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="QCheckBox" name="odom_particleFiltering">
                         <property name="text">
                          <string/>
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="0">
+                       <widget class="QSpinBox" name="odom_countdown">
+                        <property name="maximum">
+                         <number>999999</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_238">
+                        <property name="text">
+                         <string>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)).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="odom_holonomic">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>