diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bcd291eb2..d84af031e 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())); + } //******************************************************************************