IMU factor with gravity

Author: nkhedekar

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 boost::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 Vector3& gravity, boost::optional<Matrix&> H1,
+  boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
+  boost::optional<Matrix&> H4, boost::optional<Matrix&> H5, boost::optional<Matrix&> 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 boost::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,106 @@ 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, Vector3> {
+private:
+
+  typedef ImuFactorWithGravity This;
+  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
+      imuBias::ConstantBias, Vector3> Base;
+
+  PreintegratedImuMeasurements _PIM_;
+
+public:
+
+  /** Shorthand for a smart pointer to a factor */
+#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
+  typedef typename boost::shared_ptr<ImuFactorWithGravity> shared_ptr;
+#else
+  typedef boost::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 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 Vector3& gravity, boost::optional<Matrix&> H1 =
+          boost::none, boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
+          boost::none, boost::optional<Matrix&> H5 = boost::none, boost::optional<Matrix&> H6 = boost::none) 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 */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
+    ar & boost::serialization::make_nvp("NoiseModelFactor6",
+         boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(_PIM_);
+  }
+};
+// class ImuFactorWithGravity
+
 template <>
 struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
 
@@ -340,4 +441,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/PreintegrationBase.cpp

@@ -115,17 +115,18 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
 
 //------------------------------------------------------------------------------
 NavState PreintegrationBase::predict(const NavState& state_i,
-    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
-    OptionalJacobian<9, 6> H2) const {
+    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 6> H2, OptionalJacobian<9, 3> H3) const {
   Matrix96 D_biasCorrected_bias;
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
                                              H2 ? &D_biasCorrected_bias : nullptr);
 
   // Correct for initial velocity and gravity
   Matrix9 D_delta_state, D_delta_biasCorrected;
-  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
+  Matrix93 D_delta_gravity;
+  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, n_gravity,
                                   p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
-                                  H2 ? &D_delta_biasCorrected : nullptr);
+                                  H2 ? &D_delta_biasCorrected : nullptr, H3 ? &D_delta_gravity : nullptr);
 
   // Use retract to get back to NavState manifold
   Matrix9 D_predict_state, D_predict_delta;
@@ -136,41 +137,64 @@ NavState PreintegrationBase::predict(const NavState& state_i,
     *H1 = D_predict_state + D_predict_delta * D_delta_state;
   if (H2)
     *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
+  if (H3)
+    *H3 = D_predict_delta * D_delta_gravity;
   return state_j;
 }
 
+//------------------------------------------------------------------------------
+NavState PreintegrationBase::predict(const NavState& state_i,
+  const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
+  OptionalJacobian<9, 6> H2) const {
+    return predict(state_i, bias_i, p().n_gravity, H1, H2, boost::none);
+}
+
 //------------------------------------------------------------------------------
 Vector9 PreintegrationBase::computeError(const NavState& state_i,
                                          const NavState& state_j,
                                          const imuBias::ConstantBias& bias_i,
+                                         const Vector3& n_gravity,
                                          OptionalJacobian<9, 9> H1,
                                          OptionalJacobian<9, 9> H2,
-                                         OptionalJacobian<9, 6> H3) const {
+                                         OptionalJacobian<9, 6> H3,
+                                         OptionalJacobian<9, 3> H4) const {
   // Predict state at time j
   Matrix9 D_predict_state_i;
   Matrix96 D_predict_bias_i;
+  Matrix93 D_predict_gravity;
   NavState predictedState_j = predict(
-      state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0);
+      state_i, bias_i, n_gravity, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0, H4 ? &D_predict_gravity : 0);
 
   // Calculate error
   Matrix9 D_error_state_j, D_error_predict;
   Vector9 error =
       state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
-                               H1 || H3 ? &D_error_predict : 0);
+                               H1 || H3 || H4 ? &D_error_predict : 0);
 
   if (H1) *H1 << D_error_predict * D_predict_state_i;
   if (H2) *H2 << D_error_state_j;
   if (H3) *H3 << D_error_predict * D_predict_bias_i;
+  if (H4) *H4 << D_error_predict * D_predict_gravity;
 
   return error;
 }
 
+//------------------------------------------------------------------------------
+Vector9 PreintegrationBase::computeError(const NavState& state_i,
+  const NavState& state_j,
+  const imuBias::ConstantBias& bias_i,
+  OptionalJacobian<9, 9> H1,
+  OptionalJacobian<9, 9> H2,
+  OptionalJacobian<9, 6> H3) const {
+    return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, boost::none);
+}
+
 //------------------------------------------------------------------------------
 Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
     const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
+    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, OptionalJacobian<9, 6> H1,
     OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
-    OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
+    OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5, OptionalJacobian<9, 3> H6) const {
 
   // Note that derivative of constructors below is not identity for velocity, but
   // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
@@ -179,8 +203,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
 
   // Predict state at time j
   Matrix9 D_error_state_i, D_error_state_j;
-  Vector9 error = computeError(state_i, state_j, bias_i,
-                         H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5);
+  Vector9 error = computeError(state_i, state_j, bias_i, n_gravity,
+                         H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5, H6);
 
   // Separate out derivatives in terms of 5 arguments
   // Note that doing so requires special treatment of velocities, as when treated as
@@ -197,4 +221,13 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
 
 //------------------------------------------------------------------------------
 
+//------------------------------------------------------------------------------
+Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
+  const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+  const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
+  OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
+  OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
+    return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, p().n_gravity, H1, H2, H3, H4, H5, boost::none);
+  }
+
 }  // namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -150,6 +150,12 @@ class GTSAM_EXPORT PreintegrationBase {
   virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = {}) const = 0;
 
+  /// Predict state at time j
+  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
+                   OptionalJacobian<9, 9> H1 = boost::none,
+                   OptionalJacobian<9, 6> H2 = boost::none,
+                   OptionalJacobian<9, 3> H3 = boost::none) const;
+
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                    OptionalJacobian<9, 9> H1 = {},
@@ -161,6 +167,12 @@ class GTSAM_EXPORT PreintegrationBase {
                        OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
                        OptionalJacobian<9, 6> H3) const;
 
+  /// Calculate error given navStates and gravity
+  Vector9 computeError(const NavState& state_i, const NavState& state_j,
+    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
+    OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4) const;
+
   /**
    * Compute errors w.r.t. preintegrated measurements and jacobians
    * wrt pose_i, vel_i, bias_i, pose_j, bias_j
@@ -172,6 +184,13 @@ class GTSAM_EXPORT PreintegrationBase {
       OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, 
       OptionalJacobian<9, 6> H5 = {}) const;
 
+  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
+    const Pose3& pose_j, const Vector3& vel_j,
+    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, OptionalJacobian<9, 6> H1 =
+        boost::none, OptionalJacobian<9, 3> H2 = boost::none,
+    OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
+        boost::none, OptionalJacobian<9, 6> H5 = boost::none, OptionalJacobian<9, 3> H6 = boost::none) const;
+
  private:
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */