reverted modif on z when honolonomic but added modif on yaw depending on the estimated y value

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 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, 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

@@ -213,28 +213,27 @@ 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)
+
+					if(!_holonomic)
 					{
-						y = filters_[1]->filter(y);
+						float tmpY = x * tan(yaw);
-					}
+						if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
-					else
+						{
-					{
+							y = tmpY;
-						y = x * tan(yaw);
+						}
+						else
+						{
+							yaw = atan(y/x);
+						}
 					}
 
 					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);
-						}
 					}
 				}
 
@@ -245,10 +244,14 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 			}
 			else if(!_holonomic)
 			{
-				y = x * tan(yaw);
+				float tmpY = x * tan(yaw);
-				if(!_force2D)
+				if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
 				{
-					z = x * tan(pitch);
+					y = tmpY;
+				}
+				else
+				{
+					yaw = atan(y/x);
 				}
 			}
 			UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&

guilib/src/PreferencesDialog.cpp

@@ -3188,13 +3188,9 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 	QString str = _ui->lineEdit_openniLocalTransform->text();
 	str.replace("PI_2", QString::number(3.141592/2.0));
 	QStringList list = str.split(' ');
-	if(list.size() != 6)
+	if(list.size() == 6 || list.size() == 9)
 	{
-		UERROR("Local transform is wrong! must have 6 items (%s)", str.toStdString().c_str());
+		std::vector<float> numbers(list.size());
-	}
-	else
-	{
-		std::vector<float> numbers(6);
 		bool ok = false;
 		for(int i=0; i<list.size(); ++i)
 		{
@@ -3208,9 +3204,22 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 		}
 		if(ok)
 		{
-			t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
+			if(list.size() == 6)
+			{
+				t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
+			}
+			else // 9
+			{
+				t = Transform(numbers[0], numbers[1], numbers[2], 0,
+						      numbers[3], numbers[4], numbers[5], 0,
+						      numbers[6], numbers[7], numbers[8], 0);
+			}
 		}
 	}
+	else
+	{
+		UERROR("Local transform is wrong! must have 6 or 9 items (%s)", str.toStdString().c_str());
+	}
 	return t;
 }
 

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-837</y>
             <width>755</width>
             <height>1591</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>23</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2071,14 +2071,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          <item row="1" column="0">
                           <widget class="QLineEdit" name="lineEdit_openniLocalTransform">
                            <property name="text">
-                            <string>0 0 0 -PI_2 0 -PI_2</string>
+                            <string>0 0 1 -1 0 0 0 -1 0</string>
                            </property>
                           </widget>
                          </item>
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_42">
                            <property name="text">
-                            <string>Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw.</string>
+                            <string>Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -6976,7 +6976,7 @@ Lower the ratio -&gt; 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 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>
+                         <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>