Merge pull request #1797 from borglab/fix/ahrs_delR

Fix and add tests for AHRS

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<Pose3, Vector6>(g, Z_6x1);
+  EXPECT(assert_equal<Matrix6>(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};
+  const Matrix6 expected2 = numericalDerivative11<Pose3, Vector6>(g, delta);
+  const Matrix6 analytic = Pose3::ExpmapDerivative(M*delta) * M;
+  EXPECT(assert_equal<Matrix6>(expected2, analytic, 1e-5)); // note tolerance
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testRot3.cpp

@@ -956,6 +956,46 @@ TEST(Rot3, determinant) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Rot3, ExpmapChainRule) {
+  // Multiply 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<Rot3, Vector3>(g, Z_3x1);
+  EXPECT(assert_equal<Matrix3>(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<Rot3, Vector3>(g, delta);
+  EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, expmapChainRule) {
+  // 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;
+  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<Rot3, Vector3>(g, Z_3x1);
+  EXPECT(assert_equal<Matrix3>(expected, M, 1e-5));
+
+  // Test the derivatives at another value
+  const Vector3 delta{0.1,0.2,0.3};
+  const Matrix3 expected2 = numericalDerivative11<Rot3, Vector3>(g, delta);
+  EXPECT(assert_equal<Matrix3>(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

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;
 }

gtsam/navigation/tests/testManifoldPreintegration.cpp

@@ -118,48 +118,52 @@ 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));
 
   // 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;
   expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1;
   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<Matrix>(numericalDerivative11<Vector9, Vector3>(g, Z_3x1),
-                              H2.rightCols<3>(),
-                              1e-4));  // NOTE(frank): reduced tolerance
+  EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, bias_i.gyroscope()),
+                              H2.rightCols<3>()));                      
 }
 
 /* ************************************************************************* */

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<PreintegratedRotationParams> paramsWithTransform() {
-  auto p = std::make_shared<PreintegratedRotationParams>();
-  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<PreintegratedRotationParams>();
-  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<Rot3, Vector3>(f, bias), H_bias));
+  EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(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<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F))
 
   // Make sure delRdelBiasOmega is H_bias after integration.
-  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+  EXPECT(assert_equal<Matrix3>(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,56 +71,127 @@ 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));
-
-  // TODO(frank): again the derivative is not the *sane* one!
-  // auto g = [&](const Vector3& increment) {
-  //   return pim.biascorrectedDeltaRij(increment, {});
-  // };
-  // EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
+  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, {});
+  };
+  Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  EXPECT(assert_equal(expectedH, H));
+
+  // Let's integrate a second IMU measurement and check the Jacobian update:
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
+  expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
+  EXPECT(assert_equal(expectedH, H));
 }
 
 //******************************************************************************
+
+// Create params where x and y axes are exchanged.
+static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  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<Rot3, Vector3>(f, bias), H_bias));
+  EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(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))
+
+  // Check that system matrix F is the first derivative of compose:
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F))
+
+  // Make sure delRdelBiasOmega is H_bias after integration.
+  EXPECT(assert_equal<Matrix3>(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))
+
+  // Check the derivative matches the *expectedH* one
+  auto g = [&](const Vector3& increment) {
+    return pim.biascorrectedDeltaRij(increment, {});
+  };
+  Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  EXPECT(assert_equal(expectedH, H));
+
+  // Let's integrate a second IMU measurement and check the Jacobian update:
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
+  corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
+  expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  EXPECT(assert_equal(expectedH, H));
+}
+
+// Create params we have a non-axis-aligned rotation and even an offset.
+static std::shared_ptr<PreintegratedRotationParams> paramsWithArbitraryTransform() {
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  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<Rot3, Vector3>(f, bias), H_bias))
+
+  // Check derivative of deltaRij() after integration.
+  Matrix3 F;
+  PreintegratedRotation pim(p);
   pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
-  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
 
   // Check that system matrix F is the first derivative of compose:
-  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F))
 
   // Make sure delRdelBiasOmega is H_bias after integration.
-  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+  EXPECT(assert_equal<Matrix3>(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));
-
-  // TODO(frank): again the derivative is not the *sane* one!
-  // auto g = [&](const Vector3& increment) {
-  //   return pim.biascorrectedDeltaRij(increment, {});
-  // };
-  // EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
+
+  // Check the derivative matches the numerical one
+  auto g = [&](const Vector3& increment) {
+    return pim.biascorrectedDeltaRij(increment, {});
+  };
+  Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  EXPECT(assert_equal(expectedH, H));
+
+  // Let's integrate a second IMU measurement and check the Jacobian update:
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
+  corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
+  expectedH = numericalDerivative11<Rot3, Vector3>(g, biasOmegaIncr);
+  EXPECT(assert_equal(expectedH, H));
 }
 
 //******************************************************************************