From a4e81421db8a3eb74c6c2f7250f04bdeb7b22b0e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 27 Sep 2023 00:28:17 +0200 Subject: [PATCH 1/3] const correctness --- libs/poses/src/CPose3DQuatPDF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/poses/src/CPose3DQuatPDF.cpp b/libs/poses/src/CPose3DQuatPDF.cpp index ad5805f3e7..cafddb5c27 100644 --- a/libs/poses/src/CPose3DQuatPDF.cpp +++ b/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); From 4e5f8094ed435db862838c4e6fcf0ecf05ba3b8b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 27 Sep 2023 00:30:53 +0200 Subject: [PATCH 2/3] fix obsolete URL --- libs/poses/include/mrpt/poses/CPose3DPDFGaussian.h | 6 ++---- libs/poses/include/mrpt/poses/CPose3DPDFGaussianInf.h | 6 ++---- libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h | 6 ++---- libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussianInf.h | 6 ++---- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/libs/poses/include/mrpt/poses/CPose3DPDFGaussian.h b/libs/poses/include/mrpt/poses/CPose3DPDFGaussian.h index 014a9decf6..6008771861 100644 --- a/libs/poses/include/mrpt/poses/CPose3DPDFGaussian.h +++ b/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 this report. + * \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 diff --git a/libs/poses/include/mrpt/poses/CPose3DPDFGaussianInf.h b/libs/poses/include/mrpt/poses/CPose3DPDFGaussianInf.h index a6e3581b78..cca9efda85 100644 --- a/libs/poses/include/mrpt/poses/CPose3DPDFGaussianInf.h +++ b/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 this report. + * \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 diff --git a/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h b/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h index 7bb9d6fc01..b1eb421052 100644 --- a/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussian.h +++ b/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 this report. + * \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 diff --git a/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussianInf.h b/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussianInf.h index 7efa3ffd75..05f908108a 100644 --- a/libs/poses/include/mrpt/poses/CPose3DQuatPDFGaussianInf.h +++ b/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 this report. + * \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 From 3f39f1acef461848beb931d8a6124bc8c7787c63 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 27 Sep 2023 01:28:39 +0200 Subject: [PATCH 3/3] Fix wrong Jacobian formula for gimbal lock. More unit tests. --- doc/source/doxygen-docs/changelog.md | 1 + libs/math/include/mrpt/math/CQuaternion.h | 12 ++-- .../src/CPose3DQuatPDFGaussian_unittest.cpp | 63 +++++++++++++++---- libs/poses/src/CQuaternion_unittest.cpp | 22 +++++++ 4 files changed, 81 insertions(+), 17 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index f0abd003a3..6603c415fa 100644 --- 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 index 7e61f728d3..cf95b515bc 100644 --- a/libs/math/include/mrpt/math/CQuaternion.h +++ b/libs/math/include/mrpt/math/CQuaternion.h @@ -465,8 +465,10 @@ class CQuaternion : public CVectorFixed 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 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 index db7bc6f6fe..b74ac5b858 100755 --- 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 index 9591b14437..48309a7c72 100644 --- 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); + } +}