Introduce and use NavState for predict

gtsam/navigation/PreintegrationBase.cpp

@@ -129,13 +129,15 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
   }
 }
 
-/// Predict state at time j
 //------------------------------------------------------------------------------
-PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
+NavState PreintegrationBase::predict(const NavState& state_i,
-    const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+    const Rot3& deltaRij_biascorrected,
-    const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected,
+    const Vector3& deltaPij_biascorrected,
     const Vector3& deltaVij_biascorrected) const {
 
+  const Pose3& pose_i = state_i.pose();
+  const Vector3& vel_i = state_i.velocity();
+
   const double dt = deltaTij(), dt2 = dt * dt;
   const Matrix3 Ri = pose_i.rotation().matrix();
 
@@ -158,21 +160,17 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
 
   // TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
   const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR), pose_i.translation() + Point3(dP));
-  return PoseVelocityBias(pose_j, vel_i + dV, bias_i); // bias is predicted as a constant
+  return NavState(pose_j, vel_i + dV);
 }
 
-/// Predict state at time j
 //------------------------------------------------------------------------------
-PoseVelocityBias PreintegrationBase::predict(
+NavState PreintegrationBase::predict(const NavState& state_i,
-    const Pose3& pose_i, const Vector3& vel_i,
     const imuBias::ConstantBias& bias_i) const {
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  return predict(
+  return predict(state_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
-      pose_i, vel_i, bias_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
       biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr));
 }
 
-/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
 //------------------------------------------------------------------------------
 Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
                                                      const Pose3& pose_j, const Vector3& vel_j,
@@ -190,25 +188,28 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
   const Rot3& rot_j = pose_j.rotation();
   const Vector3 pos_j = pose_j.translation().vector();
 
-  // Evaluate residual error, according to [3]
+  /// Compute bias-corrected quantities
-  /* ---------------------------------------------------------------------------------------------------- */
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
   Matrix3 D_biascorrected_biasIncr;
   const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(
       biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
   const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
   const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
-  PoseVelocityBias predictedState_j =
+
-      predict(pose_i, vel_i, bias_i, deltaRij_biascorrected,
+  /// Predict state at time j
+  NavState predictedState_j =
+      predict(NavState(pose_i, vel_i), deltaRij_biascorrected,
               deltaPij_biascorrected, deltaVij_biascorrected);
 
+  // Evaluate residual error, according to [3]
   // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-  const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose.translation().vector());
+  const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose().translation().vector());
 
   // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-  const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity);
+  const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity());
 
-  // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
+  // fR will be computed later.
+  // Note: it is the same as: fR = predictedState_j.pose.rotation().between(Rot_j)
 
   /* ---------------------------------------------------------------------------------------------------- */
   // Get Get so<3> version of bias corrected rotation
@@ -293,6 +294,6 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3&
   q->omegaCoriolis = omegaCoriolis;
   q->use2ndOrderCoriolis = use2ndOrderCoriolis;
   p_ = q;
-  return predict(pose_i, vel_i, bias_i);
+  return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
 }
 }  /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -25,21 +25,44 @@
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/ProductLieGroup.h>
 #include <gtsam/base/Vector.h>
 
 namespace gtsam {
 
+/// Velocity in 3D is just a Vector3
+typedef Vector3 Velocity3;
+
 /**
- * Struct to hold all state variables of returned by Predict function
+ * Navigation state: Pose (rotation, translation) + velocity
  */
+class NavState: private ProductLieGroup<Pose3, Velocity3> {
+protected:
+  typedef ProductLieGroup<Pose3, Velocity3> Base;
+  typedef OptionalJacobian<9, 9> ChartJacobian;
+
+public:
+  // constructors
+  NavState() {}
+  NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {}
+
+  // access
+  const Pose3& pose() const { return first; }
+  const Point3& translation() const { return pose().translation(); }
+  const Rot3& rotation() const { return pose().rotation(); }
+  const Velocity3& velocity() const { return second; }
+};
+
+/// @deprecated
 struct PoseVelocityBias {
   Pose3 pose;
   Vector3 velocity;
   imuBias::ConstantBias bias;
-
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-                   const imuBias::ConstantBias _bias)
       : pose(_pose), velocity(_velocity), bias(_bias) {}
+  PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias)
+      : pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {}
+  NavState navState() const { return NavState(pose,velocity);}
 };
 
 /**
@@ -161,14 +184,12 @@ class PreintegrationBase : public PreintegratedRotation {
   }
 
   /// Predict state at time j, with bias-corrected quantities given
-  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
+  NavState predict(const NavState& navState, const Rot3& deltaRij_biascorrected,
-                           const imuBias::ConstantBias& bias_i,
-                           const Rot3& deltaRij_biascorrected,
       const Vector3& deltaPij_biascorrected,
       const Vector3& deltaVij_biascorrected) const;
 
   /// Predict state at time j
-  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
+  NavState predict(const NavState& state_i,
       const imuBias::ConstantBias& bias_i) const;
 
   /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j