holonomic option: makeing z depending on x and pitch too if nonholonomic

corelib/include/rtabmap/core/Parameters.h

@@ -322,7 +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, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)).");
 	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

@@ -225,9 +225,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 
 					if(!_force2D)
 					{
-						z = filters_[2]->filter(z);
 						roll = filters_[3]->filter(roll);
 						pitch = filters_[4]->filter(pitch);
+						if(_holonomic)
+						{
+							z = filters_[2]->filter(z);
+						}
+						else
+						{
+							z = x * tan(pitch);
+						}
 					}
 				}
 
@@ -239,6 +246,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 			else if(!_holonomic)
 			{
 				y = x * tan(yaw);
+				if(!_force2D)
+				{
+					z = x * tan(pitch);
+				}
 			}
 			UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
 					uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),

guilib/src/ui/preferencesDialog.ui

@@ -6976,7 +6976,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
                       <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>
+                         <string>If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>