Function object with sane Jacobian

gtsam/navigation/PreintegratedRotation.cpp

@@ -68,17 +68,15 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
 
-Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
+Rot3 PreintegratedRotation::IncrementalRotation::operator()(
-    const Vector3& biasHat, double deltaT,
+    const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
-    OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
-
   // First we compensate the measurements for the bias
-  Vector3 correctedOmega = measuredOmega - biasHat;
+  Vector3 correctedOmega = measuredOmega - bias;
 
   // Then compensate for sensor-body displacement: we express the quantities
   // (originally in the IMU frame) into the body frame
-  if (p_->body_P_sensor) {
+  if (body_P_sensor) {
-    Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
+    const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
     // rotation rate vector in the body frame
     correctedOmega = body_R_sensor * correctedOmega;
   }
@@ -86,7 +84,10 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
   // rotation vector describing rotation increment computed from the
   // current rotation rate measurement
   const Vector3 integratedOmega = correctedOmega * deltaT;
-  return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
+  Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias);  // expensive !!
+  if (H_bias)
+    *H_bias *= -deltaT;  // Correct so accurately reflects bias derivative
+  return incrR;
 }
 
 void PreintegratedRotation::integrateMeasurement(
@@ -94,8 +95,8 @@ void PreintegratedRotation::integrateMeasurement(
     OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
     OptionalJacobian<3, 3> F) {
   Matrix3 D_incrR_integratedOmega;
-  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
+  IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
-                                         D_incrR_integratedOmega);
+  const Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
 
   // If asked, pass first derivative as well
   if (optional_D_incrR_integratedOmega) {

gtsam/navigation/PreintegratedRotation.h

@@ -21,9 +21,9 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/std_optional_serialization.h>
+#include <gtsam/geometry/Pose3.h>
 
 namespace gtsam {
 
@@ -159,19 +159,25 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @{
 
   /**
-   * @brief Take the gyro measurement, correct it using the (constant) bias
+   * @brief Function object for incremental rotation.
-   * estimate and possibly the sensor pose, and then integrate it forward in
-   * time to yield an incremental rotation.
    * @param measuredOmega The measured angular velocity (as given by the sensor)
-   * @param biasHat The bias estimate
+   * @param deltaT The time interval over which the rotation is integrated.
-   * @param deltaT The time interval
+   * @param body_P_sensor Optional transform between body and IMU.
-   * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t.
-   * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor.
-   * @return The incremental rotation
    */
-  Rot3 incrementalRotation(
+  struct IncrementalRotation {
-      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    const Vector3& measuredOmega;
-      OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
+    const double deltaT;
+    const std::optional<Pose3>& body_P_sensor;
+
+    /**
+     * @brief Integrate angular velocity, but corrected by bias.
+     * @param bias The bias estimate
+     * @param H_bias Jacobian of the rotation w.r.t. bias.
+     * @return The incremental rotation
+     */
+    Rot3 operator()(const Vector3& biasHat,
+                    OptionalJacobian<3, 3> H_bias = {}) const;
+  };
 
   /**
    * @brief Calculate an incremental rotation given the gyro measurement and a
@@ -198,6 +204,24 @@ class GTSAM_EXPORT PreintegratedRotation {
 
   /// @}
 
+  /// @name Deprecated API
+  /// @{
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+  /// @deprecated: use IncrementalRotation functor with sane Jacobian
+  inline Rot3 GTSAM_DEPRECATED incrementalRotation(
+      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
+    IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
+    // Backwards compatible "weird" Jacobian, no longer used.
+    if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
+    return incrR;
+  }
+#endif
+
+  /// @}
+
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

gtsam/navigation/tests/testPreintegratedRotation.cpp

@@ -30,40 +30,36 @@ using namespace gtsam;
 namespace simple_roll {
 auto p = std::make_shared<PreintegratedRotationParams>();
 PreintegratedRotation pim(p);
-const double roll = 0.1;
+const double omega = 0.1;
-const Vector3 measuredOmega(roll, 0, 0);
+const Vector3 measuredOmega(omega, 0, 0);
-const Vector3 biasHat(0, 0, 0);
+const Vector3 bias(0, 0, 0);
 const double deltaT = 0.5;
 }  // namespace simple_roll
 
 //******************************************************************************
-TEST(PreintegratedRotation, incrementalRotation) {
+TEST(PreintegratedRotation, IncrementalRotation) {
   using namespace simple_roll;
 
   // Check the value.
-  Matrix3 D_incrR_integratedOmega;
+  Matrix3 H_bias;
-  const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT,
+  PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
-                                             D_incrR_integratedOmega);
+                                               p->getBodyPSensor()};
-  Rot3 expected = Rot3::Roll(roll * deltaT);
+  const Rot3 incrR = f(bias, H_bias);
+  Rot3 expected = Rot3::Roll(omega * deltaT);
   EXPECT(assert_equal(expected, incrR, 1e-9));
 
-  // Lambda for numerical derivative:
+  // Check the derivative:
-  auto f = [&](const Vector3& x, const Vector3& y) {
+  EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
+
+  // Ephemeral test for deprecated Jacobian:
+  Matrix3 D_incrR_integratedOmega;
+  (void)pim.incrementalRotation(measuredOmega, bias, deltaT,
+                                D_incrR_integratedOmega);
+  auto g = [&](const Vector3& x, const Vector3& y) {
     return pim.incrementalRotation(x, y, deltaT, {});
   };
-
-  // NOTE(frank): these derivatives as computed by the function violate the
-  // "Jacobian contract". We should refactor this. It's not clear that the
-  // deltaT factor is actually understood in calling code.
-
-  // Check derivative with respect to measuredOmega
   EXPECT(assert_equal<Matrix3>(
-      numericalDerivative21<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
+      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-      deltaT * D_incrR_integratedOmega));
-
-  // Check derivative with respect to biasHat
-  EXPECT(assert_equal<Matrix3>(
-      numericalDerivative22<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
       -deltaT * D_incrR_integratedOmega));
 }
 
@@ -77,40 +73,35 @@ static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
 namespace roll_in_rotated_frame {
 auto p = paramsWithTransform();
 PreintegratedRotation pim(p);
-const double roll = 0.1;
+const double omega = 0.1;
-const Vector3 measuredOmega(roll, 0, 0);
+const Vector3 measuredOmega(omega, 0, 0);
-const Vector3 biasHat(0, 0, 0);
+const Vector3 bias(0, 0, 0);
 const double deltaT = 0.5;
 }  // namespace roll_in_rotated_frame
 
 //******************************************************************************
-TEST(PreintegratedRotation, incrementalRotationWithTransform) {
+TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
   using namespace roll_in_rotated_frame;
 
   // Check the value.
-  Matrix3 D_incrR_integratedOmega;
+  Matrix3 H_bias;
-  const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT,
+  PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
-                                             D_incrR_integratedOmega);
+                                               p->getBodyPSensor()};
-  Rot3 expected = Rot3::Pitch(roll * deltaT);
+  Rot3 expected = Rot3::Pitch(omega * deltaT);
-  EXPECT(assert_equal(expected, incrR, 1e-9));
+  EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
 
-  // Lambda for numerical derivative:
+  // Check the derivative:
-  auto f = [&](const Vector3& x, const Vector3& y) {
+  EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
+
+  // Ephemeral test for deprecated Jacobian:
+  Matrix3 D_incrR_integratedOmega;
+  (void)pim.incrementalRotation(measuredOmega, bias, deltaT,
+                                D_incrR_integratedOmega);
+  auto g = [&](const Vector3& x, const Vector3& y) {
     return pim.incrementalRotation(x, y, deltaT, {});
   };
-
-  // NOTE(frank): Here, once again, the derivatives are weird, as they do not
-  // take the rotation into account. They *are* the derivatives of the rotated
-  // omegas, but not the derivatives with respect to the function arguments.
-
-  // Check derivative with respect to measuredOmega
   EXPECT(assert_equal<Matrix3>(
-      numericalDerivative21<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
+      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-      deltaT * D_incrR_integratedOmega));
-
-  // Check derivative with respect to biasHat
-  EXPECT(assert_equal<Matrix3>(
-      numericalDerivative22<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
       -deltaT * D_incrR_integratedOmega));
 }