From c22b76506c24da51211b0eb473e92d9df2556233 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:03:40 -0700 Subject: [PATCH 1/7] Punctuation/scope --- .../tests/testPreintegratedRotation.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 1e27ca1e4a..bcd291eb25 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -36,40 +36,33 @@ const Vector3 measuredOmega = trueOmega + bias; const double deltaT = 0.5; } // namespace biased_x_rotation -// Create params where x and y axes are exchanged. -static std::shared_ptr paramsWithTransform() { - auto p = std::make_shared(); - p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); - return p; -} - //****************************************************************************** TEST(PreintegratedRotation, integrateGyroMeasurement) { // Example where IMU is identical to body frame, then omega is roll using namespace biased_x_rotation; auto p = std::make_shared(); - PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; - internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; const Rot3 incrR = f(bias, H_bias); - Rot3 expected = Rot3::Roll(omega * deltaT); - EXPECT(assert_equal(expected, incrR, 1e-9)); + const Rot3 expected = Rot3::Roll(omega * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)) // Check the derivative: - EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) // Check value of deltaRij() after integration. Matrix3 F; + PreintegratedRotation pim(p); pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)) // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) // Check if we make a correction to the bias, the value and Jacobian are // correct. Note that the bias is subtracted from the measurement, and the @@ -78,8 +71,8 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); - EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); - EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)) + EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)) // TODO(frank): again the derivative is not the *sane* one! // auto g = [&](const Vector3& increment) { @@ -89,39 +82,47 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { } //****************************************************************************** + +// Create params where x and y axes are exchanged. +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} + TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Example where IMU is rotated, so measured omega indicates pitch. using namespace biased_x_rotation; auto p = paramsWithTransform(); - PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; - internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; - Rot3 expected = Rot3::Pitch(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + const Rot3 expected = Rot3::Pitch(omega * deltaT); // Pitch, because of sensor-IMU rotation! + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)) // Check the derivative: - EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) // Check value of deltaRij() after integration. Matrix3 F; + PreintegratedRotation pim(p); pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)) // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) // Check the bias correction in same way, but will now yield pitch change. Matrix3 H; const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); - EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)); - EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)); + EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)) + EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)) // TODO(frank): again the derivative is not the *sane* one! // auto g = [&](const Vector3& increment) { From c2145bdffccfba6558dd42807e4d3c36075255b1 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:55:06 -0700 Subject: [PATCH 2/7] Simplify code as we know the Jacobian = R --- gtsam/navigation/PreintegratedRotation.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 04e201a347..6c8e510e53 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -75,13 +75,11 @@ Rot3 IncrementalRotation::operator()( Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame. If Jacobian is - // requested, the rotation matrix is obtained as `rotate` Jacobian. - Matrix3 body_R_sensor; + // (originally in the IMU frame) into the body frame. + // Note that the rotate Jacobian is just body_P_sensor->rotation().matrix(). if (body_P_sensor) { // rotation rate vector in the body frame - correctedOmega = body_P_sensor->rotation().rotate( - correctedOmega, {}, H_bias ? &body_R_sensor : nullptr); + correctedOmega = body_P_sensor->rotation() * correctedOmega; } // rotation vector describing rotation increment computed from the @@ -90,7 +88,7 @@ Rot3 IncrementalRotation::operator()( Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! if (H_bias) { *H_bias *= -deltaT; // Correct so accurately reflects bias derivative - if (body_P_sensor) *H_bias *= body_R_sensor; + if (body_P_sensor) *H_bias *= body_P_sensor->rotation().matrix(); } return incrR; } From 37696b274e1ce9be3169ca5959cb5da50139a0d8 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:55:20 -0700 Subject: [PATCH 3/7] Adding more tests --- .../tests/testPreintegratedRotation.cpp | 84 ++++++++++++++++--- 1 file changed, 74 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bcd291eb25..d84af031ef 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -74,11 +74,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)) - // TODO(frank): again the derivative is not the *sane* one! - // auto g = [&](const Vector3& increment) { - // return pim.biascorrectedDeltaRij(increment, {}); - // }; - // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); } //****************************************************************************** @@ -124,11 +131,68 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)) - // TODO(frank): again the derivative is not the *sane* one! - // auto g = [&](const Vector3& increment) { - // return pim.biascorrectedDeltaRij(increment, {}); - // }; - // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); +} + +// Create params we have a non-axis-aligned rotation and even an offset. +static std::shared_ptr paramsWithArbitraryTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Expmap({1,2,3}), {4,5,6}}); + return p; +} + +TEST(PreintegratedRotation, integrateGyroMeasurementWithArbitraryTransform) { + // Example with a non-axis-aligned transform and some position. + using namespace biased_x_rotation; + auto p = paramsWithArbitraryTransform(); + + // Check the derivative: + Matrix3 H_bias; + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + f(bias, H_bias); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) + + // Check derivative of deltaRij() after integration. + Matrix3 F; + PreintegratedRotation pim(p); + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) + + // Check the bias correction in same way, but will now yield pitch change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + } //****************************************************************************** From 1c40b17fac65dc752f31340daf73a434590c11ad Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Tue, 13 Aug 2024 17:32:14 -0700 Subject: [PATCH 4/7] Some tests on Expmap/expmap chain rule --- gtsam/geometry/tests/testPose3.cpp | 25 +++++++++++++++++++ gtsam/geometry/tests/testRot3.cpp | 40 ++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 93cf999723..065d43bf95 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1207,6 +1207,31 @@ TEST(Pose3, Print) { EXPECT(assert_print_equal(expected, pose)); } +/* ************************************************************************* */ +TEST(Pose3, ExpmapChainRule) { + // Muliply with an arbitrary matrix and exponentiate + Matrix6 M; + M << 1, 2, 3, 4, 5, 6, // + 7, 8, 9, 1, 2, 3, // + 4, 5, 6, 7, 8, 9, // + 1, 2, 3, 4, 5, 6, // + 7, 8, 9, 1, 2, 3, // + 4, 5, 6, 7, 8, 9; + auto g = [&](const Vector6& omega) { + return Pose3::Expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix6 expected = numericalDerivative11(g, Z_6x1); + EXPECT(assert_equal(expected, M)); // Pose3::ExpmapDerivative(Z_6x1) is identity + + // Test the derivatives at another value + const Vector6 delta{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; + const Matrix6 expected2 = numericalDerivative11(g, delta); + const Matrix6 analytic = Pose3::ExpmapDerivative(M*delta) * M; + EXPECT(assert_equal(expected2, analytic, 1e-5)); // note tolerance +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1232348f03..9555a24457 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -956,6 +956,46 @@ TEST(Rot3, determinant) { } } +/* ************************************************************************* */ +TEST(Rot3, ExpmapChainRule) { + // Muliply with an arbitrary matrix and exponentiate + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + auto g = [&](const Vector3& omega) { + return Rot3::Expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix3 expected = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(expected, M)); // SO3::ExpmapDerivative(Z_3x1) is identity + + // Test the derivatives at another value + const Vector3 delta{0.1,0.2,0.3}; + const Matrix3 expected2 = numericalDerivative11(g, delta); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M)); +} + +/* ************************************************************************* */ +TEST(Rot3, expmapChainRule) { + // Muliply an arbitrary rotation with exp(M*x) + // Perhaps counter-intuitively, this has the same derivatives as above + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + const Rot3 R = Rot3::Expmap({1, 2, 3}); + auto g = [&](const Vector3& omega) { + return R.expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix3 expected = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(expected, M)); + + // Test the derivatives at another value + const Vector3 delta{0.1,0.2,0.3}; + const Matrix3 expected2 = numericalDerivative11(g, delta); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M)); +} + /* ************************************************************************* */ int main() { TestResult tr; From ac96b59469a1360ff4c8c06320b8447b94dd6b49 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Wed, 14 Aug 2024 10:50:32 -0700 Subject: [PATCH 5/7] Fix jacobian tests --- .../tests/testPreintegratedRotation.cpp | 39 +++++++++---------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index d84af031ef..468701e3c6 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -74,18 +74,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)) + // Check the derivative matches the numerical one auto g = [&](const Vector3& increment) { return pim.biascorrectedDeltaRij(increment, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); - + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); + // Let's integrate a second IMU measurement and check the Jacobian update: pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + expectedH = numericalDerivative11(g, biasOmegaIncr); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** @@ -131,18 +131,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)) + // Check the derivative matches the *expectedH* one auto g = [&](const Vector3& increment) { return pim.biascorrectedDeltaRij(increment, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); // Let's integrate a second IMU measurement and check the Jacobian update: pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); } // Create params we have a non-axis-aligned rotation and even an offset. @@ -180,19 +180,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithArbitraryTransform) { const Vector3 biasOmegaIncr(delta, 0, 0); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + // Check the derivative matches the numerical one auto g = [&](const Vector3& increment) { return pim.biascorrectedDeltaRij(increment, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); // Let's integrate a second IMU measurement and check the Jacobian update: pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); - + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** From 6412517d3fe2df44a37bfae8c939aa05b2b9e24c Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Wed, 14 Aug 2024 17:53:18 -0700 Subject: [PATCH 6/7] Fix and comment another test --- .../tests/testManifoldPreintegration.cpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 4016240cf8..82f9876fb0 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -118,17 +118,18 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { // Integrate a single measurement const double omega = 0.1; const Vector3 trueOmega(omega, 0, 0); - const Vector3 bias(1, 2, 3); - const Vector3 measuredOmega = trueOmega + bias; + const Vector3 biasOmega(1, 2, 3); + const Vector3 measuredOmega = trueOmega + biasOmega; const double deltaT = 0.5; // Check the accumulated rotation. Rot3 expected = Rot3::Roll(omega * deltaT); - pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Vector biasOmegaHat = biasOmega; + pim.integrateGyroMeasurement(measuredOmega, biasOmegaHat, deltaT); EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); // Now do the same for a ManifoldPreintegration object - imuBias::ConstantBias biasHat {Z_3x1, bias}; + imuBias::ConstantBias biasHat {Z_3x1, biasOmega}; ManifoldPreintegration manifoldPim(testing::Params(), biasHat); manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT); EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9)); @@ -136,17 +137,21 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { // Check their internal Jacobians are the same: EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega())); - // Check PreintegratedRotation::biascorrectedDeltaRij. - Matrix3 H; + // Let's check the derivatives a delta away from the bias hat const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); + imuBias::ConstantBias bias_i {Z_3x1, biasOmegaHat + biasOmegaIncr}; + + // Check PreintegratedRotation::biascorrectedDeltaRij. + // Note that biascorrectedDeltaRij expects the biasHat to be subtracted already + Matrix3 H; Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT); EXPECT(assert_equal(expected2, corrected, 1e-9)); // Check ManifoldPreintegration::biasCorrectedDelta. - imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr}; + // Note that, confusingly, biasCorrectedDelta will subtract biasHat inside Matrix96 H2; Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2); Vector9 expected3; @@ -154,12 +159,11 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { EXPECT(assert_equal(expected3, biasCorrected, 1e-9)); // Check that this one is sane: - auto g = [&](const Vector3& increment) { - return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {}); + auto g = [&](const Vector3& b) { + return manifoldPim.biasCorrectedDelta({Z_3x1, b}, {}); }; - EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), - H2.rightCols<3>(), - 1e-4)); // NOTE(frank): reduced tolerance + EXPECT(assert_equal(numericalDerivative11(g, bias_i.gyroscope()), + H2.rightCols<3>())); } /* ************************************************************************* */ From 475b37f7d6aacf86e9624dd8ae97475396f581f0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 26 Aug 2024 15:08:04 -0700 Subject: [PATCH 7/7] Fix tolerance for ubuntu quaternion cases --- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 065d43bf95..c9851f7616 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1223,7 +1223,7 @@ TEST(Pose3, ExpmapChainRule) { // Test the derivatives at zero const Matrix6 expected = numericalDerivative11(g, Z_6x1); - EXPECT(assert_equal(expected, M)); // Pose3::ExpmapDerivative(Z_6x1) is identity + EXPECT(assert_equal(expected, M, 1e-5)); // Pose3::ExpmapDerivative(Z_6x1) is identity // Test the derivatives at another value const Vector6 delta{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 9555a24457..ce056edb68 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -958,7 +958,7 @@ TEST(Rot3, determinant) { /* ************************************************************************* */ TEST(Rot3, ExpmapChainRule) { - // Muliply with an arbitrary matrix and exponentiate + // Multiply with an arbitrary matrix and exponentiate Matrix3 M; M << 1, 2, 3, 4, 5, 6, 7, 8, 9; auto g = [&](const Vector3& omega) { @@ -967,17 +967,17 @@ TEST(Rot3, ExpmapChainRule) { // Test the derivatives at zero const Matrix3 expected = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(expected, M)); // SO3::ExpmapDerivative(Z_3x1) is identity + EXPECT(assert_equal(expected, M, 1e-5)); // SO3::ExpmapDerivative(Z_3x1) is identity // Test the derivatives at another value const Vector3 delta{0.1,0.2,0.3}; const Matrix3 expected2 = numericalDerivative11(g, delta); - EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M)); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); } /* ************************************************************************* */ TEST(Rot3, expmapChainRule) { - // Muliply an arbitrary rotation with exp(M*x) + // Multiply an arbitrary rotation with exp(M*x) // Perhaps counter-intuitively, this has the same derivatives as above Matrix3 M; M << 1, 2, 3, 4, 5, 6, 7, 8, 9; @@ -988,12 +988,12 @@ TEST(Rot3, expmapChainRule) { // Test the derivatives at zero const Matrix3 expected = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(expected, M)); + EXPECT(assert_equal(expected, M, 1e-5)); // Test the derivatives at another value const Vector3 delta{0.1,0.2,0.3}; const Matrix3 expected2 = numericalDerivative11(g, delta); - EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M)); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); } /* ************************************************************************* */