Merge pull request #1762 from borglab/fix/ahrs

Correctly deal with sensor rotation in AHRS

.gitignore

@@ -19,3 +19,4 @@ CMakeLists.txt.user*
 xcode/
 /Dockerfile
 /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
+.cache/

gtsam/navigation/AHRSFactor.cpp

@@ -49,13 +49,14 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedAhrsMeasurements::integrateMeasurement(
     const Vector3& measuredOmega, double deltaT) {
+  Matrix3 Fr;
+  PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
+                                                  deltaT, &Fr);
 
-  Matrix3 D_incrR_integratedOmega, Fr;
+  // First order uncertainty propagation
-  PreintegratedRotation::integrateMeasurement(measuredOmega,
+  // The deltaT allows to pass from continuous time noise to discrete time
-      biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
+  // noise. Comparing with the IMUFactor.cpp implementation, the latter is an
-
+  // approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt.
-  // first order uncertainty propagation
-  // the deltaT allows to pass from continuous time noise to discrete time noise
   preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
 }
 

gtsam/navigation/PreintegratedRotation.cpp

@@ -68,39 +68,40 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
 
-Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
+namespace internal {
-    const Vector3& biasHat, double deltaT,
+Rot3 IncrementalRotation::operator()(
-    OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
+    const Vector3& bias, OptionalJacobian<3, 3> H_bias) 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
+  // (originally in the IMU frame) into the body frame. If Jacobian is
-  if (p_->body_P_sensor) {
+  // requested, the rotation matrix is obtained as `rotate` Jacobian.
-    Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
+  Matrix3 body_R_sensor;
+  if (body_P_sensor) {
     // rotation rate vector in the body frame
-    correctedOmega = body_R_sensor * correctedOmega;
+    correctedOmega = body_P_sensor->rotation().rotate(
+        correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
   }
 
   // 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
-void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
+    if (body_P_sensor) *H_bias *= body_R_sensor;
-    const Vector3& biasHat, double deltaT,
-    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
-    OptionalJacobian<3, 3> F) {
-  Matrix3 D_incrR_integratedOmega;
-  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
-      D_incrR_integratedOmega);
-
-  // If asked, pass first derivative as well
-  if (optional_D_incrR_integratedOmega) {
-    *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
   }
+  return incrR;
+}
+}  // namespace internal
+
+void PreintegratedRotation::integrateGyroMeasurement(
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    OptionalJacobian<3, 3> F) {
+  Matrix3 H_bias;
+  internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+  const Rot3 incrR = f(biasHat, H_bias);
 
   // Update deltaTij and rotation
   deltaTij_ += deltaT;
@@ -108,10 +109,26 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
-      - D_incrR_integratedOmega * deltaT;
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+void PreintegratedRotation::integrateMeasurement(
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
+    OptionalJacobian<3, 3> F) {
+  integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F);
+
+  // If asked, pass obsolete Jacobians as well
+  if (optional_D_incrR_integratedOmega) {
+    Matrix3 H_bias;
+    internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    const Rot3 incrR = f(biasHat, H_bias);
+    *optional_D_incrR_integratedOmega << H_bias / -deltaT;
+  }
+}
+#endif
+
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
     OptionalJacobian<3, 3> H) const {
   const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;

gtsam/navigation/PreintegratedRotation.h

@@ -21,12 +21,37 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/std_optional_serialization.h>
+#include <gtsam/geometry/Pose3.h>
+#include "gtsam/dllexport.h"
 
 namespace gtsam {
 
+namespace internal {
+/**
+ * @brief Function object for incremental rotation.
+ * @param measuredOmega The measured angular velocity (as given by the sensor)
+ * @param deltaT The time interval over which the rotation is integrated.
+ * @param body_P_sensor Optional transform between body and IMU.
+ */
+struct GTSAM_EXPORT IncrementalRotation {
+  const Vector3& measuredOmega;
+  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& bias,
+                  OptionalJacobian<3, 3> H_bias = {}) const;
+};
+
+}  // namespace internal
+
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegratedRotationParams {
@@ -65,7 +90,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
 
@@ -136,18 +160,10 @@ class GTSAM_EXPORT PreintegratedRotation {
 
   /// @name Access instance variables
   /// @{
-  const std::shared_ptr<Params>& params() const {
+  const std::shared_ptr<Params>& params() const { return p_; }
-    return p_;
+  const double& deltaTij() const { return deltaTij_; }
-  }
+  const Rot3& deltaRij() const { return deltaRij_; }
-  const double& deltaTij() const {
+  const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
-    return deltaTij_;
-  }
-  const Rot3& deltaRij() const {
-    return deltaRij_;
-  }
-  const Matrix3& delRdelBiasOmega() const {
-    return delRdelBiasOmega_;
-  }
   /// @}
 
   /// @name Testable
@@ -159,19 +175,24 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @name Main functionality
   /// @{
 
-  /// Take the gyro measurement, correct it using the (constant) bias estimate
+  /**
-  /// and possibly the sensor pose, and then integrate it forward in time to yield
+   * @brief Calculate an incremental rotation given the gyro measurement and a
-  /// an incremental rotation.
+   * time interval, and update both deltaTij_ and deltaRij_.
-  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+   * @param measuredOmega The measured angular velocity (as given by the sensor)
-                           OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
+   * @param bias The biasHat estimate
+   * @param deltaT The time interval
+   * @param F optional Jacobian of internal compose, used in AhrsFactor.
+   */
+  void integrateGyroMeasurement(const Vector3& measuredOmega,
+                                const Vector3& biasHat, double deltaT,
+                                OptionalJacobian<3, 3> F = {});
 
-  /// Calculate an incremental rotation given the gyro measurement and a time interval,
+  /**
-  /// and update both deltaTij_ and deltaRij_.
+   * @brief Return a bias corrected version of the integrated rotation.
-  void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+   * @param biasOmegaIncr An increment with respect to biasHat used above.
-                            OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
+   * @param H optional Jacobian of the correction w.r.t. the bias increment.
-                            OptionalJacobian<3, 3> F = {});
+   * @note The *key* functionality of this class used in optimizing the bias.
-
+   */
-  /// Return a bias corrected version of the integrated rotation, with optional Jacobian
   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
                              OptionalJacobian<3, 3> H = {}) const;
 
@@ -180,6 +201,31 @@ 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& bias, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
+    internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    Rot3 incrR = f(bias, D_incrR_integratedOmega);
+    // Backwards compatible "weird" Jacobian, no longer used.
+    if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
+    return incrR;
+  }
+
+  /// @deprecated: use integrateGyroMeasurement from now on
+  /// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega.
+  void GTSAM_DEPRECATED integrateMeasurement(
+      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
+
+#endif
+
+  /// @}
+
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -18,47 +18,44 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/navigation/AHRSFactor.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/debug.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/navigation/AHRSFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/BetweenFactor.h>
 
+#include <cmath>
 #include <list>
+#include <memory>
+#include "gtsam/nonlinear/LevenbergMarquardtParams.h"
 
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
 // Convenience for named keys
-using symbol_shorthand::X;
-using symbol_shorthand::V;
 using symbol_shorthand::B;
+using symbol_shorthand::R;
 
-Vector3 kZeroOmegaCoriolis(0,0,0);
+Vector3 kZeroOmegaCoriolis(0, 0, 0);
 
 // Define covariance matrices
-double accNoiseVar = 0.01;
+double gyroNoiseVar = 0.01;
-const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
+const Matrix3 kMeasuredOmegaCovariance = gyroNoiseVar * I_3x3;
 
 //******************************************************************************
 namespace {
-Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
+PreintegratedAhrsMeasurements integrateMeasurements(
-    const Rot3 rot_j, const Vector3& bias) {
+    const Vector3& biasHat, const list<Vector3>& measuredOmegas,
-  return factor.evaluateError(rot_i, rot_j, bias);
+    const list<double>& deltaTs) {
-}
+  PreintegratedAhrsMeasurements result(biasHat, I_3x3);
-
-Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
-    const Rot3 rot_j, const Vector3& bias) {
-  return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
-}
-
-PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
-    const Vector3& bias, const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3::Zero()) {
-  PreintegratedAhrsMeasurements result(bias, I_3x3);
 
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
   list<double>::const_iterator itDeltaT = deltaTs.begin();
@@ -68,79 +65,59 @@ PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
 
   return result;
 }
-
+}  // namespace
-Rot3 evaluatePreintegratedMeasurementsRotation(
-    const Vector3& bias, const list<Vector3>& measuredOmegas,
-    const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3::Zero()) {
-  return Rot3(
-      evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
-          initialRotationRate).deltaRij());
-}
-
-Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
-    const double deltaT) {
-  return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
-}
-
-Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
-  return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
-}
-}
 
 //******************************************************************************
-TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
+TEST(AHRSFactor, PreintegratedAhrsMeasurements) {
   // Linearization point
-  Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
+  Vector3 biasHat(0, 0, 0);  ///< Current estimate of angular rate bias
 
   // Measurements
   Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
   double deltaT = 0.5;
 
   // Expected preintegrated values
-  Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
+  Rot3 expectedDeltaR1 = Rot3::Roll(0.5 * M_PI / 100.0);
-  double expectedDeltaT1(0.5);
 
   // Actual preintegrated values
-  PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
+  PreintegratedAhrsMeasurements actual1(biasHat, kMeasuredOmegaCovariance);
   actual1.integrateMeasurement(measuredOmega, deltaT);
 
   EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
-  DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
+  DOUBLES_EQUAL(deltaT, actual1.deltaTij(), 1e-6);
+
+  // Check the covariance
+  Matrix3 expectedMeasCov = kMeasuredOmegaCovariance * deltaT;
+  EXPECT(assert_equal(expectedMeasCov, actual1.preintMeasCov(), 1e-6));
 
   // Integrate again
-  Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
+  Rot3 expectedDeltaR2 = Rot3::Roll(2.0 * 0.5 * M_PI / 100.0);
-  double expectedDeltaT2(1);
 
   // Actual preintegrated values
   PreintegratedAhrsMeasurements actual2 = actual1;
   actual2.integrateMeasurement(measuredOmega, deltaT);
 
   EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
-  DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
+  DOUBLES_EQUAL(deltaT * 2, actual2.deltaTij(), 1e-6);
 }
 
 //******************************************************************************
-TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
+TEST(AHRSFactor, PreintegratedAhrsMeasurementsConstructor) {
-  Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
+  Matrix3 gyroscopeCovariance = I_3x3 * 0.4;
   Vector3 omegaCoriolis(0.1, 0.5, 0.9);
   PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis);
-  Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias
+  Vector3 bias(1.0, 2.0, 3.0);  ///< Current estimate of angular rate bias
   Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
   double deltaTij = 0.02;
-  Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
+  Matrix3 delRdelBiasOmega = I_3x3 * 0.5;
-  Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
+  Matrix3 preintMeasCov = I_3x3 * 0.2;
   PreintegratedAhrsMeasurements actualPim(
-    std::make_shared<PreintegratedRotationParams>(params),
+      std::make_shared<PreintegratedRotationParams>(params), bias, deltaTij,
-    bias,
+      deltaRij, delRdelBiasOmega, preintMeasCov);
-    deltaTij,
-    deltaRij,
-    delRdelBiasOmega,
-    preintMeasCov);
   EXPECT(assert_equal(gyroscopeCovariance,
-      actualPim.p().getGyroscopeCovariance(), 1e-6));
+                      actualPim.p().getGyroscopeCovariance(), 1e-6));
-  EXPECT(assert_equal(omegaCoriolis,
+  EXPECT(
-      *(actualPim.p().getOmegaCoriolis()), 1e-6));
+      assert_equal(omegaCoriolis, *(actualPim.p().getOmegaCoriolis()), 1e-6));
   EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
   DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
   EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
@@ -151,198 +128,148 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
 /* ************************************************************************* */
 TEST(AHRSFactor, Error) {
   // Linearization point
-  Vector3 bias(0.,0.,0.); // Bias
+  Vector3 bias(0., 0., 0.);  // Bias
-  Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
+  Rot3 Ri(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
-  Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
+  Rot3 Rj(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
 
   // Measurements
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(M_PI / 100, 0, 0);
-  measuredOmega << M_PI / 100, 0, 0;
   double deltaT = 1.0;
-  PreintegratedAhrsMeasurements pim(bias, Z_3x3);
+  PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {});
 
-  Vector3 errorActual = factor.evaluateError(x1, x2, bias);
+  // Check value
-
+  Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
-  // Expected error
+  Vector3 errorExpected(0, 0, 0);
-  Vector3 errorExpected(3);
-  errorExpected << 0, 0, 0;
   EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6));
 
-  // Expected Jacobians
+  // Check Derivatives
-  Matrix H1e = numericalDerivative11<Vector3, Rot3>(
+  Values values;
-      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
+  values.insert(R(1), Ri);
-  Matrix H2e = numericalDerivative11<Vector3, Rot3>(
+  values.insert(R(2), Rj);
-      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
+  values.insert(B(1), bias);
-  Matrix H3e = numericalDerivative11<Vector3, Vector3>(
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
-      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
-
-  // Check rotation Jacobians
-  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
-  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
-
-  // Actual Jacobians
-  Matrix H1a, H2a, H3a;
-  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
-
-  // rotations
-  EXPECT(assert_equal(RH1e, H1a, 1e-5));
-  // 1e-5 needs to be added only when using quaternions for rotations
-
-  EXPECT(assert_equal(H2e, H2a, 1e-5));
-
-  // rotations
-  EXPECT(assert_equal(RH2e, H2a, 1e-5));
-  // 1e-5 needs to be added only when using quaternions for rotations
-
-  EXPECT(assert_equal(H3e, H3a, 1e-5));
-  // 1e-5 needs to be added only when using quaternions for rotations
 }
 
 /* ************************************************************************* */
 TEST(AHRSFactor, ErrorWithBiases) {
   // Linearization point
-
   Vector3 bias(0, 0, 0.3);
-  Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
+  Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
-  Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
+  Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
 
   // Measurements
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
-  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   double deltaT = 1.0;
-
+  PreintegratedAhrsMeasurements pim(Vector3(0, 0, 0), kMeasuredOmegaCovariance);
-  PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
-      Z_3x3);
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
 
-  Vector errorActual = factor.evaluateError(x1, x2, bias);
+  // Check value
-
+  Vector3 errorExpected(0, 0, 0);
-  // Expected error
+  Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
-  Vector errorExpected(3);
-  errorExpected << 0, 0, 0;
   EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 
-  // Expected Jacobians
+  // Check Derivatives
-  Matrix H1e = numericalDerivative11<Vector, Rot3>(
+  Values values;
-      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
+  values.insert(R(1), Ri);
-  Matrix H2e = numericalDerivative11<Vector, Rot3>(
+  values.insert(R(2), Rj);
-      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
+  values.insert(B(1), bias);
-  Matrix H3e = numericalDerivative11<Vector, Vector3>(
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
-      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
-
-  // Check rotation Jacobians
-  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
-  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
-  Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
-      std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
-
-  // Actual Jacobians
-  Matrix H1a, H2a, H3a;
-  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
-
-  EXPECT(assert_equal(H1e, H1a));
-  EXPECT(assert_equal(H2e, H2a));
-  EXPECT(assert_equal(H3e, H3a));
 }
 
 //******************************************************************************
-TEST( AHRSFactor, PartialDerivativeExpmap ) {
+TEST(AHRSFactor, PartialDerivativeExpmap) {
   // Linearization point
-  Vector3 biasOmega(0,0,0);
+  Vector3 biasOmega(0, 0, 0);
 
   // Measurements
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(0.1, 0, 0);
-  measuredOmega << 0.1, 0, 0;
   double deltaT = 0.5;
 
+  auto f = [&](const Vector3& biasOmega) {
+    return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
+  };
+
   // Compute numerical derivatives
-  Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
+  Matrix expectedH = numericalDerivative11<Rot3, Vector3>(f, biasOmega);
-      std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
 
-  const Matrix3 Jr = Rot3::ExpmapDerivative(
+  const Matrix3 Jr =
-      (measuredOmega - biasOmega) * deltaT);
+      Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
 
-  Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
+  Matrix3 actualH = -Jr * deltaT;  // the delta bias appears with the minus sign
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
+  EXPECT(assert_equal(expectedH, actualH, 1e-3));
   // 1e-3 needs to be added only when using quaternions for rotations
-
 }
 
 //******************************************************************************
-TEST( AHRSFactor, PartialDerivativeLogmap ) {
+TEST(AHRSFactor, PartialDerivativeLogmap) {
   // Linearization point
-  Vector3 thetahat;
+  Vector3 thetaHat(0.1, 0.1, 0);  ///< Current estimate of rotation rate bias
-  thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
 
-  // Measurements
+  auto f = [thetaHat](const Vector3 deltaTheta) {
-  Vector3 deltatheta;
+    return Rot3::Logmap(
-  deltatheta << 0, 0, 0;
+        Rot3::Expmap(thetaHat).compose(Rot3::Expmap(deltaTheta)));
+  };
 
   // Compute numerical derivatives
-  Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
+  Vector3 deltaTheta(0, 0, 0);
-      std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
+  Matrix expectedH = numericalDerivative11<Vector3, Vector3>(f, deltaTheta);
 
-  const Vector3 x = thetahat; // parametrization of so(3)
+  const Vector3 x = thetaHat;          // parametrization of so(3)
-  const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
+  const Matrix3 X = skewSymmetric(x);  // element of Lie algebra so(3): X = x^
-  double normx = x.norm();
+  double norm = x.norm();
-  const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
+  const Matrix3 actualH =
-      + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
+      I_3x3 + 0.5 * X +
-          * X;
+      (1 / (norm * norm) - (1 + cos(norm)) / (2 * norm * sin(norm))) * X * X;
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
+  EXPECT(assert_equal(expectedH, actualH));
-
 }
 
 //******************************************************************************
-TEST( AHRSFactor, fistOrderExponential ) {
+TEST(AHRSFactor, fistOrderExponential) {
   // Linearization point
-  Vector3 biasOmega(0,0,0);
+  Vector3 biasOmega(0, 0, 0);
 
   // Measurements
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(0.1, 0, 0);
-  measuredOmega << 0.1, 0, 0;
   double deltaT = 1.0;
 
   // change w.r.t. linearization point
   double alpha = 0.0;
-  Vector3 deltabiasOmega;
+  Vector3 deltaBiasOmega(alpha, alpha, alpha);
-  deltabiasOmega << alpha, alpha, alpha;
 
-  const Matrix3 Jr = Rot3::ExpmapDerivative(
+  const Matrix3 Jr =
-      (measuredOmega - biasOmega) * deltaT);
+      Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
 
-  Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
+  Matrix3 delRdelBiasOmega =
+      -Jr * deltaT;  // the delta bias appears with the minus sign
 
-  const Matrix expectedRot = Rot3::Expmap(
+  const Matrix expectedRot =
-      (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
+      Rot3::Expmap((measuredOmega - biasOmega - deltaBiasOmega) * deltaT)
+          .matrix();
 
   const Matrix3 hatRot =
       Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
-  const Matrix3 actualRot = hatRot
+  const Matrix3 actualRot =
-      * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
+      hatRot * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
 
   // Compare Jacobians
   EXPECT(assert_equal(expectedRot, actualRot));
 }
 
 //******************************************************************************
-TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
+TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
   // Linearization point
-  Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
+  Vector3 bias = Vector3::Zero();  ///< Current estimate of rotation rate bias
 
   Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
 
@@ -361,98 +288,76 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
 
   // Actual preintegrated values
   PreintegratedAhrsMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
+      integrateMeasurements(bias, measuredOmegas, deltaTs);
-          Vector3(M_PI / 100.0, 0.0, 0.0));
+
+  auto f = [&](const Vector3& bias) {
+    return integrateMeasurements(bias, measuredOmegas, deltaTs).deltaRij();
+  };
 
   // Compute numerical derivatives
-  Matrix expectedDelRdelBias =
+  Matrix expectedDelRdelBias = numericalDerivative11<Rot3, Vector3>(f, bias);
-      numericalDerivative11<Rot3, Vector3>(
-          std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
-              measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
 
   // Compare Jacobians
-  EXPECT(
+  EXPECT(assert_equal(expectedDelRdelBiasOmega,
-      assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
+                      preintegrated.delRdelBiasOmega(), 1e-3));
   // 1e-3 needs to be added only when using quaternions for rotations
 }
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-
 //******************************************************************************
-TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
+TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
-
   Vector3 bias(0, 0, 0.3);
-  Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
+  Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
-  Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
+  Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
 
   // Measurements
   Vector3 omegaCoriolis;
   omegaCoriolis << 0, 0.1, 0.1;
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
-  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   double deltaT = 1.0;
 
-  const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
+  auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
-      Point3(1, 0, 0));
+  p->gyroscopeCovariance = kMeasuredOmegaCovariance;
-
+  p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0));
-  PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
+  PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
 
   pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Check preintegrated covariance
-  EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));
+  EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov()));
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis);
 
-  // Expected Jacobians
+  // Check Derivatives
-  Matrix H1e = numericalDerivative11<Vector, Rot3>(
+  Values values;
-      std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
+  values.insert(R(1), Ri);
-  Matrix H2e = numericalDerivative11<Vector, Rot3>(
+  values.insert(R(2), Rj);
-      std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
+  values.insert(B(1), bias);
-  Matrix H3e = numericalDerivative11<Vector, Vector3>(
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
-      std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
-
-  // Check rotation Jacobians
-  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
-  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
-      std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
-  Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
-      std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
-
-  // Actual Jacobians
-  Matrix H1a, H2a, H3a;
-  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
-
-  EXPECT(assert_equal(H1e, H1a));
-  EXPECT(assert_equal(H2e, H2a));
-  EXPECT(assert_equal(H3e, H3a));
 }
+
 //******************************************************************************
-TEST (AHRSFactor, predictTest) {
+TEST(AHRSFactor, predictTest) {
-  Vector3 bias(0,0,0);
+  Vector3 bias(0, 0, 0);
 
   // Measurements
-  Vector3 measuredOmega;
+  Vector3 measuredOmega(0, 0, M_PI / 10.0);
-  measuredOmega << 0, 0, M_PI / 10.0;
   double deltaT = 0.2;
-  PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
+  PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
   for (int i = 0; i < 1000; ++i) {
     pim.integrateMeasurement(measuredOmega, deltaT);
   }
   // Check preintegrated covariance
-  Matrix expectedMeasCov(3,3);
+  Matrix expectedMeasCov(3, 3);
-  expectedMeasCov = 200*kMeasuredAccCovariance;
+  expectedMeasCov = 200 * kMeasuredOmegaCovariance;
   EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
 
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
 
   // Predict
   Rot3 x;
-  Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
+  Rot3 expectedRot = Rot3::Ypr(20 * M_PI, 0, 0);
   Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
   EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
 
@@ -462,29 +367,26 @@ TEST (AHRSFactor, predictTest) {
 
   // Actual Jacobians
   Matrix H;
-  (void) pim.predict(bias,H);
+  (void)pim.predict(bias, H);
   EXPECT(assert_equal(expectedH, H, 1e-8));
 }
 //******************************************************************************
-#include <gtsam/linear/GaussianFactorGraph.h>
+TEST(AHRSFactor, graphTest) {
-#include <gtsam/nonlinear/Marginals.h>
-
-TEST (AHRSFactor, graphTest) {
   // linearization point
-  Rot3 x1(Rot3::RzRyRx(0, 0, 0));
+  Rot3 Ri(Rot3::RzRyRx(0, 0, 0));
-  Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
+  Rot3 Rj(Rot3::RzRyRx(0, M_PI / 4, 0));
-  Vector3 bias(0,0,0);
+  Vector3 bias(0, 0, 0);
 
   // PreIntegrator
   Vector3 biasHat(0, 0, 0);
-  PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
+  PreintegratedAhrsMeasurements pim(biasHat, kMeasuredOmegaCovariance);
 
   // Pre-integrate measurements
   Vector3 measuredOmega(0, M_PI / 20, 0);
   double deltaT = 1;
 
   // Create Factor
-  noiseModel::Base::shared_ptr model = //
+  noiseModel::Base::shared_ptr model =  //
       noiseModel::Gaussian::Covariance(pim.preintMeasCov());
   NonlinearFactorGraph graph;
   Values values;
@@ -492,16 +394,88 @@ TEST (AHRSFactor, graphTest) {
     pim.integrateMeasurement(measuredOmega, deltaT);
   }
 
-  // pim.print("Pre integrated measurementes");
+  // pim.print("Pre integrated measurements");
-  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
+  AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
-  values.insert(X(1), x1);
+  values.insert(R(1), Ri);
-  values.insert(X(2), x2);
+  values.insert(R(2), Rj);
   values.insert(B(1), bias);
   graph.push_back(factor);
   LevenbergMarquardtOptimizer optimizer(graph, values);
   Values result = optimizer.optimize();
   Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
-  EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
+  EXPECT(assert_equal(expectedRot, result.at<Rot3>(R(2))));
+}
+
+/* ************************************************************************* */
+TEST(AHRSFactor, bodyPSensorWithBias) {
+  using noiseModel::Diagonal;
+
+  int numRotations = 10;
+  const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6);
+  SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
+
+  // Measurements in the sensor frame:
+  const double omega = 0.1;
+  const Vector3 realOmega(omega, 0, 0);
+  const Vector3 realBias(1, 2, 3);  // large !
+  const Vector3 measuredOmega = realOmega + realBias;
+
+  auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
+  p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0));
+  p->gyroscopeCovariance = 1e-8 * I_3x3;
+  double deltaT = 0.005;
+
+  // Specify noise values on priors
+  const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001);
+  const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1);
+  SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
+  SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
+
+  // Create a factor graph with priors on initial pose, velocity and bias
+  NonlinearFactorGraph graph;
+  Values values;
+
+  graph.addPrior(R(0), Rot3(), priorNoisePose);
+  values.insert(R(0), Rot3());
+
+  // The key to this test is that we specify the bias, in the sensor frame, as
+  // known a priori. We also create factors below that encode our assumption
+  // that this bias is constant over time In theory, after optimization, we
+  // should recover that same bias estimate
+  graph.addPrior(B(0), realBias, priorNoiseBias);
+  values.insert(B(0), realBias);
+
+  // Now add IMU factors and bias noise models
+  const Vector3 zeroBias(0, 0, 0);
+  for (int i = 1; i < numRotations; i++) {
+    PreintegratedAhrsMeasurements pim(p, realBias);
+    for (int j = 0; j < 200; ++j)
+      pim.integrateMeasurement(measuredOmega, deltaT);
+
+    // Create factors
+    graph.emplace_shared<AHRSFactor>(R(i - 1), R(i), B(i - 1), pim);
+    graph.emplace_shared<BetweenFactor<Vector3> >(B(i - 1), B(i), zeroBias,
+                                                  biasNoiseModel);
+
+    values.insert(R(i), Rot3());
+    values.insert(B(i), realBias);
+  }
+
+  // Finally, optimize, and get bias at last time step
+  LevenbergMarquardtParams params;
+  // params.setVerbosityLM("SUMMARY");
+  Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
+  const Vector3 biasActual = result.at<Vector3>(B(numRotations - 1));
+
+  // Bias should be a self-fulfilling prophesy:
+  EXPECT(assert_equal(realBias, biasActual, 1e-3));
+
+  // Check that the successive rotations are all `omega` apart:
+  for (int i = 0; i < numRotations; i++) {
+    Rot3 expectedRot = Rot3::Pitch(omega * i);
+    Rot3 actualRot = result.at<Rot3>(R(i));
+    EXPECT(assert_equal(expectedRot, actualRot, 1e-3));
+  }
 }
 
 //******************************************************************************

gtsam/navigation/tests/testImuFactor.cpp

@@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
   const Matrix3 Jr =
       Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
 
-  Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
+  Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
 }
 
 /* ************************************************************************* */
 TEST(ImuFactor, PartialDerivativeLogmap) {
   // Linearization point
-  Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
+  Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
 
   // Measurements
-  Vector3 deltatheta(0, 0, 0);
+  Vector3 deltaTheta(0, 0, 0);
 
-  auto evaluateLogRotation = [=](const Vector3 deltatheta) {
+  auto evaluateLogRotation = [=](const Vector3 delta) {
     return Rot3::Logmap(
-        Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
+        Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
   };
 
   // Compute numerical derivatives
-  Matrix expectedDelFdeltheta =
+  Matrix expectedDelFdelTheta =
-      numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);
+      numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
 
-  Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
+  Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
+  EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
 }
 
 /* ************************************************************************* */
@@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) {
 
   // change w.r.t. linearization point
   double alpha = 0.0;
-  Vector3 deltabiasOmega;
+  Vector3 deltaBiasOmega;
-  deltabiasOmega << alpha, alpha, alpha;
+  deltaBiasOmega << alpha, alpha, alpha;
 
   const Matrix3 Jr = Rot3::ExpmapDerivative(
       (measuredOmega - biasOmega) * deltaT);
@@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) {
   Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
 
   const Matrix expectedRot = Rot3::Expmap(
-      (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
+      (measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
 
   const Matrix3 hatRot =
       Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
   const Matrix3 actualRot = hatRot
-      * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
+      * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
-  // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
 
   // This is a first order expansion so the equality is only an approximation
   EXPECT(assert_equal(expectedRot, actualRot));
@@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   using noiseModel::Diagonal;
   typedef Bias Bias;
 
-  int numFactors = 10;
+  int numPoses = 10;
   Vector6 noiseBetweenBiasSigma;
   noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
       3.0e-6, 3.0e-6);
@@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
   Vector3 zeroVel(0, 0, 0);
 
-  // Create a factor graph with priors on initial pose, vlocity and bias
+  // Create a factor graph with priors on initial pose, velocity and bias
   NonlinearFactorGraph graph;
   Values values;
 
@@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
 
   // Now add IMU factors and bias noise models
   Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-  for (int i = 1; i < numFactors; i++) {
+  for (int i = 1; i < numPoses; i++) {
-    PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
+    PreintegratedImuMeasurements pim(p, priorBias);
-        priorBias);
     for (int j = 0; j < 200; ++j)
       pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
@@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   }
 
   // Finally, optimize, and get bias at last time step
-  Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
+  Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
-  Bias biasActual = results.at<Bias>(B(numFactors - 1));
+  Bias biasActual = result.at<Bias>(B(numPoses - 1));
 
   // And compare it with expected value (our prior)
   Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
@@ -851,11 +849,11 @@ struct ImuFactorMergeTest {
                         actual_pim02.preintegrated(), tol));
     EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
 
-    ImuFactor::shared_ptr factor01 =
+    auto factor01 =
         std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
-    ImuFactor::shared_ptr factor12 =
+    auto factor12 =
         std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
-    ImuFactor::shared_ptr factor02_expected = std::make_shared<ImuFactor>(
+    auto factor02_expected = std::make_shared<ImuFactor>(
         X(0), V(0), X(2), V(2), B(0), pim02_expected);
 
     ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);

gtsam/navigation/tests/testManifoldPreintegration.cpp

@@ -109,6 +109,59 @@ TEST(ManifoldPreintegration, computeError) {
   EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
 }
 
+/* ************************************************************************* */
+TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
+  // Create a PreintegratedRotation object
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  PreintegratedRotation pim(p);
+
+  // 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 double deltaT = 0.5;
+
+  // Check the accumulated rotation.
+  Rot3 expected = Rot3::Roll(omega * deltaT);
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
+  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
+
+  // Now do the same for a ManifoldPreintegration object
+  imuBias::ConstantBias biasHat {Z_3x1, bias};
+  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;
+  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));
+  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};
+  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}, {});
+  };
+  EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, Z_3x1),
+                              H2.rightCols<3>(),
+                              1e-4));  // NOTE(frank): reduced tolerance
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/tests/testPreintegratedRotation.cpp

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testPreintegratedRotation.cpp
+ * @brief  Unit test for PreintegratedRotation
+ * @author Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/navigation/PreintegratedRotation.h>
+
+#include <memory>
+
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/Vector.h"
+
+using namespace gtsam;
+
+//******************************************************************************
+// Example where gyro measures small rotation about x-axis, with bias.
+namespace biased_x_rotation {
+const double omega = 0.1;
+const Vector3 trueOmega(omega, 0, 0);
+const Vector3 bias(1, 2, 3);
+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 Rot3 incrR = f(bias, H_bias);
+  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));
+
+  // Check value of deltaRij() after integration.
+  Matrix3 F;
+  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 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
+  // integration time is taken into account, so we expect -deltaT*delta change.
+  Matrix3 H;
+  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));
+}
+
+//******************************************************************************
+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));
+
+  // Check the derivative:
+  EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
+
+  // Check value of deltaRij() after integration.
+  Matrix3 F;
+  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));
+
+  // 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));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************