Fix wrong Jacobian formula for gimbal lock. More unit tests.

MRPT · Sep 26, 2023 · 3f39f1a · 3f39f1a
1 parent 4e5f809
commit 3f39f1a
Show file tree

Hide file tree

Showing 4 changed files with 81 additions and 17 deletions.
diff --git a/doc/source/doxygen-docs/changelog.md b/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:

diff --git a/libs/math/include/mrpt/math/CQuaternion.h b/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

diff --git a/libs/poses/src/CPose3DQuatPDFGaussian_unittest.cpp b/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)

diff --git a/libs/poses/src/CQuaternion_unittest.cpp b/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);
+	}
+}