IMU factor with Gravity Estimation

gtsam/navigation/ImuFactor.cpp

@@ -255,5 +255,102 @@ Vector ImuFactor2::evaluateError(const NavState& state_i,
 
 //------------------------------------------------------------------------------
 
+//------------------------------------------------------------------------------
+// ImuFactorWithGravity methods
+//------------------------------------------------------------------------------
+ImuFactorWithGravity::ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, Key gravity,
+  const PreintegratedImuMeasurements& pim) :
+  Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+      pose_j, vel_j, bias, gravity), _PIM_(pim) {
+}
+
+//------------------------------------------------------------------------------
+NonlinearFactor::shared_ptr ImuFactorWithGravity::clone() const {
+return std::static_pointer_cast<NonlinearFactor>(
+    NonlinearFactor::shared_ptr(new This(*this)));
+}
+
+//------------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& os, const ImuFactorWithGravity& f) {
+f._PIM_.print("preintegrated measurements:\n");
+os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
+return os;
+}
+
+//------------------------------------------------------------------------------
+void ImuFactorWithGravity::print(const string& s, const KeyFormatter& keyFormatter) const {
+cout << (s.empty() ? s : s + "\n") << "ImuFactorWithGravity(" << keyFormatter(this->key<1>())
+     << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
+     << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>())
+     << ")\n";
+cout << *this << endl;
+}
+
+//------------------------------------------------------------------------------
+bool ImuFactorWithGravity::equals(const NonlinearFactor& other, double tol) const {
+const This *e = dynamic_cast<const This*>(&other);
+const bool base = Base::equals(*e, tol);
+const bool pim = _PIM_.equals(e->_PIM_, tol);
+return e != nullptr && base && pim;
+}
+
+//------------------------------------------------------------------------------
+Vector ImuFactorWithGravity::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
+  const Pose3& pose_j, const Vector3& vel_j,
+  const imuBias::ConstantBias& bias_i, const Unit3& gravity, OptionalMatrixType H1,
+  OptionalMatrixType H2, OptionalMatrixType H3,
+  OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const {
+  std::cout << "Evaluating error" << std::endl;
+    return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, gravity,
+    H1, H2, H3, H4, H5, H6);
+}
+
+//------------------------------------------------------------------------------
+#ifdef GTSAM_TANGENT_PREINTEGRATION
+PreintegratedImuMeasurements ImuFactorWithGravity::Merge(
+  const PreintegratedImuMeasurements& pim01,
+  const PreintegratedImuMeasurements& pim12) {
+if (!pim01.matchesParamsWith(pim12))
+throw std::domain_error(
+    "Cannot merge PreintegratedImuMeasurements with different params");
+
+if (pim01.params()->body_P_sensor)
+throw std::domain_error(
+    "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
+
+// the bias for the merged factor will be the bias from 01
+PreintegratedImuMeasurements pim02 = pim01;
+
+Matrix9 H1, H2;
+pim02.mergeWith(pim12, &H1, &H2);
+
+return pim02;
+}
+
+//------------------------------------------------------------------------------
+ImuFactorWithGravity::shared_ptr ImuFactorWithGravity::Merge(const shared_ptr& f01,
+  const shared_ptr& f12) {
+// IMU bias keys must be the same.
+if (f01->key<5>() != f12->key<5>())
+throw std::domain_error("ImuFactorWithGravity::Merge: IMU bias keys must be the same");
+
+// expect intermediate pose, velocity keys to matchup.
+if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>())
+throw std::domain_error(
+    "ImuFactorWithGravity::Merge: intermediate pose, velocity keys need to match up");
+
+// return new factor
+auto pim02 =
+Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
+return std::make_shared<ImuFactorWithGravity>(f01->key<1>(),  // P0
+    f01->key<2>(),  // V0
+    f12->key<3>(),  // P2
+    f12->key<4>(),  // V2
+    f01->key<5>(),  // B
+    f01->key<6>(),  // G
+    pim02);
+}
+#endif
+
 }
 // namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -69,6 +69,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
   friend class ImuFactor;
   friend class ImuFactor2;
+  friend class ImuFactorWithGravity;
 
 protected:
 
@@ -331,6 +332,113 @@ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imu
 };
 // class ImuFactor2
 
+/**
+ * ImuFactorWithGravity is a 6-ways factor involving previous state (pose and velocity of
+ * the vehicle at previous time step), current state (pose and velocity at
+ * current time step), the bias estimate and the gravity. Following the preintegration
+ * scheme proposed in [2], the ImuFactorWithGravity includes many IMU measurements, which
+ * are "summarized" using the PreintegratedIMUMeasurements class.
+ * Note that this factor does not model "temporal consistency" of the biases
+ * (which are usually slowly varying quantities), which is up to the caller.
+ * See also CombinedImuFactor for a class that does this for you.
+ *
+ * @ingroup navigation
+ */
+class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
+    imuBias::ConstantBias, Unit3> {
+private:
+
+  typedef ImuFactorWithGravity This;
+  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
+      imuBias::ConstantBias, Unit3> Base;
+
+  PreintegratedImuMeasurements _PIM_;
+
+public:
+
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
+  /** Shorthand for a smart pointer to a factor */
+#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
+  typedef typename std::shared_ptr<ImuFactorWithGravity> shared_ptr;
+#else
+  typedef std::shared_ptr<ImuFactorWithGravity> shared_ptr;
+#endif
+
+  /** Default constructor - only use for serialization */
+  ImuFactorWithGravity() {}
+
+  /**
+   * Constructor
+   * @param pose_i Previous pose key
+   * @param vel_i  Previous velocity key
+   * @param pose_j Current pose key
+   * @param vel_j  Current velocity key
+   * @param bias   Previous bias key
+   * @param gravity Gravity key
+   * @param preintegratedMeasurements The preintegreated measurements since the
+   * last pose.
+   */
+  ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, Key gravity,
+      const PreintegratedImuMeasurements& preintegratedMeasurements);
+
+  ~ImuFactorWithGravity() override {
+  }
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
+
+  /// @name Testable
+  /// @{
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactorWithGravity&);
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+                                            DefaultKeyFormatter) const override;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
+  /// @}
+
+  /** Access the preintegrated measurements. */
+
+  const PreintegratedImuMeasurements& preintegratedMeasurements() const {
+    return _PIM_;
+  }
+
+  /** implement functions needed to derive from Factor */
+
+  /// vector of errors
+  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
+                       const Pose3& pose_j, const Vector3& vel_j,
+                       const imuBias::ConstantBias& bias_i,
+                       const Unit3& gravity, OptionalMatrixType H1,
+                       OptionalMatrixType H2, OptionalMatrixType H3,
+                       OptionalMatrixType H4, OptionalMatrixType H5,
+                       OptionalMatrixType H6) const override;
+
+#ifdef GTSAM_TANGENT_PREINTEGRATION
+  /// Merge two pre-integrated measurement classes
+  static PreintegratedImuMeasurements Merge(
+      const PreintegratedImuMeasurements& pim01,
+      const PreintegratedImuMeasurements& pim12);
+
+  /// Merge two factors
+  static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
+#endif
+
+ private:
+  /** Serialization function */
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
+    ar & boost::serialization::make_nvp("NoiseModelFactor6",
+         boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(_PIM_);
+  }
+#endif
+};
+// class ImuFactorWithGravity
+
 template <>
 struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
 
@@ -340,4 +448,7 @@ struct traits<ImuFactor> : public Testable<ImuFactor> {};
 template <>
 struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
 
+template <>
+struct traits<ImuFactorWithGravity> : public Testable<ImuFactorWithGravity> {};
+
 } /// namespace gtsam

gtsam/navigation/NavState.cpp

@@ -439,24 +439,29 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
     const Vector3& n_gravity, const std::optional<Vector3>& omegaCoriolis,
     bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
-    OptionalJacobian<9, 9> H2) const {
+    OptionalJacobian<9, 9> H2, OptionalJacobian<9, 2> H3) const {
   const Rot3& nRb = R_;
   const Velocity3& n_v = v_; // derivative is Ri !
   const double dt22 = 0.5 * dt * dt;
 
+  // The gravity is actually made of two components - the constant magnitude and variable direction
+  Matrix32 D_grav_gdirection = Unit3(n_gravity).basis();
+
   Vector9 xi;
   Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
+  Matrix3 D_nRb_unrot_grav_P, D_nRb_unrot_grav_V; // Both of these are actually the same
   dR(xi) = dR(pim);
   dP(xi) = dP(pim)
-      + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
-      + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
-  dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
+          + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
+      + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0, H3 ? &D_nRb_unrot_grav_P : 0);
+  dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0,
+                                  H3 ? &D_nRb_unrot_grav_V : 0);
 
   if (omegaCoriolis) {
     xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
   }
 
-  if (H1 || H2) {
+  if (H1 || H2 || H3) {
     Matrix3 Ri = nRb.matrix();
 
     if (H1) {
@@ -469,6 +474,14 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt,
     if (H2) {
       H2->setIdentity();
     }
+    if (H3) {
+      // The rotation part of xi does not depend on gravity:
+      H3->block<3,2>(0,0) = Matrix32::Zero();
+      // The position part: derivative is dt22 * (∂(nRb.unrotate(n_gravity))/∂n_gravity)
+      H3->block<3,2>(3,0) = dt22 * D_nRb_unrot_grav_P * D_grav_gdirection * n_gravity.norm();
+      // The velocity part: derivative is dt * D_nRb_unrot_grav_V
+      H3->block<3,2>(6,0) = dt * D_nRb_unrot_grav_V * D_grav_gdirection * n_gravity.norm();
+    }
   }
 
   return xi;

gtsam/navigation/NavState.h

@@ -262,7 +262,7 @@ class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
   Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
       const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
           false, OptionalJacobian<9, 9> H1 = {},
-      OptionalJacobian<9, 9> H2 = {}) const;
+      OptionalJacobian<9, 9> H2 = {}, OptionalJacobian<9,2> H3 = {}) const;
 
   /// @}