Merge pull request #1290 from MRPT/fix/more-unit-tests-fix-docs

Fix/more unit tests fix docs

doc/source/doxygen-docs/changelog.md

@@ -11,6 +11,7 @@
 - BUG FIXES:
   - Fix CSparse "C" linkage build error (OSX Clang). PR [#1280](https://github.com/MRPT/mrpt/pull/1280)
   - Fix missing Python wrapping of poses PDF (poses with uncertainty) composition (\oplus and \ominus) operators. (Closes [#1281](https://github.com/MRPT/mrpt/issues/1281)). PR [#1283](https://github.com/MRPT/mrpt/pull/1283)
+  - Fix wrong Jacobian in mrpt::math::CQuaternion::rpy_and_jacobian() for the case of Gimbal Lock. Thanks @giafranchini for reporting!. PR [#1290](https://github.com/MRPT/mrpt/pull/1290) (Closes [#1289](https://github.com/MRPT/mrpt/issues/1289))
 
 # Version 2.10.1: Released August 10th, 2023
 - Build system:

libs/math/include/mrpt/math/CQuaternion.h

@@ -465,8 +465,10 @@ class CQuaternion : public CVectorFixed<T, 4>
 			if (out_dr_dq)
 			{
 				out_dr_dq->setZero();
-				(*out_dr_dq)(0, 0) = +2 / x();
-				(*out_dr_dq)(0, 2) = -2 * r() / (x() * x());
+				// d{yaw}_d{r}:
+				(*out_dr_dq)(0, 0) = +2 * x() / (r() * r() + x() * x());
+				// d{yaw}_d{x}:
+				(*out_dr_dq)(0, 1) = -2 * r() / (r() * r() + x() * x());
 			}
 		}
 		else if (discr < -0.49999)
@@ -477,8 +479,10 @@ class CQuaternion : public CVectorFixed<T, 4>
 			if (out_dr_dq)
 			{
 				out_dr_dq->setZero();
-				(*out_dr_dq)(0, 0) = -2 / x();
-				(*out_dr_dq)(0, 2) = +2 * r() / (x() * x());
+				// d{yaw}_d{r}:
+				(*out_dr_dq)(0, 0) = -2 * x() / (r() * r() + x() * x());
+				// d{yaw}_d{x}:
+				(*out_dr_dq)(0, 1) = +2 * r() / (r() * r() + x() * x());
 			}
 		}
 		else

libs/poses/include/mrpt/poses/CPose3DPDFGaussian.h

@@ -29,10 +29,8 @@ class CPose3DQuatPDFGaussian;
  *  Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is
  * implemented in the method "CPose3DPDFGaussian::operator+=".
  *
- *  For further details on implemented methods and the theory behind them,
- *  see <a
- * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
- * >this report</a>.
+ * \note Read also: "A tutorial on SE(3) transformation parameterizations and
+ * on-manifold optimization", in \cite blanco_se3_tutorial
  *
  * \sa CPose3D, CPose3DPDF, CPose3DPDFParticles
  * \ingroup poses_pdf_grp

libs/poses/include/mrpt/poses/CPose3DPDFGaussianInf.h

@@ -28,10 +28,8 @@ class CPose3DQuatPDFGaussian;
  *  Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is
  * implemented in the method "CPose3DPDFGaussianInf::operator+=".
  *
- *  For further details on implemented methods and the theory behind them,
- *  see <a
- * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
- * >this report</a>.
+ * \note Read also: "A tutorial on SE(3) transformation parameterizations and
+ * on-manifold optimization", in \cite blanco_se3_tutorial
  *
  * \sa CPose3D, CPose3DPDF, CPose3DPDFParticles, CPose3DPDFGaussian
  * \ingroup poses_pdf_grp

libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h

@@ -32,10 +32,8 @@ class CPose3DPDFGaussian;
  * implemented in the methods "CPose3DQuatPDFGaussian::operator+=" and
  * "CPose3DQuatPDF::jacobiansPoseComposition".
  *
- *  For further details on implemented methods and the theory behind them,
- *  see <a
- * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
- * >this report</a>.
+ * \note Read also: "A tutorial on SE(3) transformation parameterizations and
+ * on-manifold optimization", in \cite blanco_se3_tutorial
  *
  * \sa CPose3DQuat, CPose3DQuatPDF, CPose3DPDF, CPose3DQuatPDFGaussianInf
  * \ingroup poses_pdf_grp

libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussianInf.h

@@ -31,10 +31,8 @@ class CPose3DPDFGaussian;
  * implemented in the methods "CPose3DQuatPDFGaussianInf::operator+=" and
  * "CPose3DQuatPDF::jacobiansPoseComposition".
  *
- *  For further details on implemented methods and the theory behind them,
- *  see <a
- * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
- * >this report</a>.
+ * \note Read also: "A tutorial on SE(3) transformation parameterizations and
+ * on-manifold optimization", in \cite blanco_se3_tutorial
  *
  * \sa CPose3DQuat, CPose3DQuatPDF, CPose3DPDF, CPose3DQuatPDFGaussian
  * \ingroup poses_pdf_grp

libs/poses/src/CPose3DQuatPDF.cpp

@@ -62,7 +62,7 @@ void CPose3DQuatPDF::jacobiansPoseComposition(
 	const double q2y = u.quat().y();
 	const double q2z = u.quat().z();
 
-	CPose3DQuat x_plus_u = x + u;  // needed for the normalization Jacobian:
+	const CPose3DQuat x_plus_u = x + u;	 // for the normalization Jacobian
 	CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
 	x_plus_u.quat().normalizationJacobian(norm_jacob);
 

libs/poses/src/CPose3DQuatPDFGaussian_unittest.cpp

@@ -53,23 +53,54 @@ class Pose3DQuatPDFGaussTests : public ::testing::Test
 		return p6pdf;
 	}
 
-	void test_toFromYPRGauss(double yaw, double pitch, double roll)
+	void test_toFromYPRGauss(
+		double yaw, double pitch, double roll, bool useGimbalLockCov = false)
 	{
+		const bool usesSUT =
+			mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION();
+
 		// Random pose:
 		CPose3DPDFGaussian p1ypr =
-			generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 0.1);
+			generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 1e-2);
+
+		// In gimbal-lock, it's impossible to recover uncertainty for
+		// pitch/roll, since both are fixed values:
+		if (useGimbalLockCov)
+		{
+			for (int i = 0; i < 6; i++)
+				for (int j = 4; j < 6; j++)
+					p1ypr.cov(i, j) = p1ypr.cov(j, i) = 0;
+		}
+
 		CPose3DQuatPDFGaussian p1quat = CPose3DQuatPDFGaussian(p1ypr);
 
 		// Convert back to a 6x6 representation:
 		CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);
 
-		EXPECT_NEAR(0, (p2ypr.cov - p1ypr.cov).array().abs().maxCoeff(), 1e-6)
-			<< "p1ypr: " << endl
-			<< p1ypr << endl
-			<< "p1quat : " << endl
-			<< p1quat << endl
-			<< "p2ypr : " << endl
-			<< p2ypr << endl;
+		std::stringstream onFailMsg;
+		onFailMsg << "p1ypr: " << endl
+				  << p1ypr << endl
+				  << "p1quat : " << endl
+				  << p1quat << endl
+				  << "p2ypr : " << endl
+				  << p2ypr << endl
+				  << "USE_SUT_QUAT2EULER_CONVERSION: "
+				  << mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION()
+				  << endl;
+
+		EXPECT_NEAR(
+			0, (p2ypr.cov - p1ypr.cov).array().abs().maxCoeff(),
+			usesSUT ? 1e-2 : 1e-6)
+			<< onFailMsg.str();
+
+		EXPECT_NEAR(
+			0,
+			(p2ypr.mean.asVectorVal() - p1ypr.mean.asVectorVal())
+				.array()
+				.abs()
+				.maxCoeff(),
+			usesSUT ? 0.1 : 1e-4)
+			<< onFailMsg.str();
 	}
 
 	static void func_compose(
@@ -366,10 +397,16 @@ class Pose3DQuatPDFGaussTests : public ::testing::Test
 
 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
 {
-	test_toFromYPRGauss(-30.0_deg, 10.0_deg, 60.0_deg);
-	test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
-	test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
-	// The formulas break at pitch=90, but this we cannot avoid...
+	for (int USE_SUT = 0; USE_SUT <= 1; USE_SUT++)
+	{
+		mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION(USE_SUT != 0);
+
+		test_toFromYPRGauss(-30.0_deg, 10.0_deg, 60.0_deg);
+		test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
+		test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
+		test_toFromYPRGauss(20.0_deg, 89.999_deg, 0.0_deg, true /*gimbal*/);
+		test_toFromYPRGauss(20.0_deg, -89.999_deg, 0.0_deg, true /*gimbal*/);
+	}
 }
 
 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)

libs/poses/src/CQuaternion_unittest.cpp

@@ -192,3 +192,25 @@ TEST_F(QuaternionTests, ExpAndLnMatches)
 	for (const auto& i : list_test_XYZ)
 		test_ExpAndLnMatches(i[0], i[1], i[2]);
 }
+
+TEST_F(QuaternionTests, ThrowOnNotNormalized)
+{
+	EXPECT_NO_THROW(mrpt::math::CQuaternion(1.0, 0.0, 0.0, 0.0));
+	EXPECT_ANY_THROW(mrpt::math::CQuaternion(0.9, 0.0, 0.0, 0.0));
+	EXPECT_ANY_THROW(mrpt::math::CQuaternion(1.1, 0.0, 0.0, 0.0));
+	EXPECT_ANY_THROW(mrpt::math::CQuaternion(1.0, 0.1, 0.0, 0.0));
+	EXPECT_ANY_THROW(mrpt::math::CQuaternion(1.0, 0.0, -0.1, 0.0));
+	EXPECT_ANY_THROW(mrpt::math::CQuaternion(1.0, 0.0, 0.0, -0.1));
+}
+
+TEST_F(QuaternionTests, ensurePositiveRealPart)
+{
+	{
+		auto q = mrpt::math::CQuaternion(1.0, 0.0, 0.0, 0.0);
+		EXPECT_GE(q.r(), 0.0);
+	}
+	{
+		auto q = mrpt::math::CQuaternion(-1.0, 0.0, 0.0, 0.0);
+		EXPECT_GE(q.r(), 0.0);
+	}
+}