diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 1e27ca1e4..bcd291eb2 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) {