A custom retract does the trick

gtsam/navigation/PreintegrationBase.cpp

@@ -129,10 +129,6 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
   }
 }
 
-static Eigen::Block<Vector9,3,1> dR(Vector9& v) { return v.segment<3>(0); }
-static Eigen::Block<Vector9,3,1> dP(Vector9& v) { return v.segment<3>(3); }
-static Eigen::Block<Vector9,3,1> dV(Vector9& v) { return v.segment<3>(6); }
-
 //------------------------------------------------------------------------------
 Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
   Vector9 result = Vector9::Zero();
@@ -143,14 +139,14 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const {
     const double dt = deltaTij(), dt2 = dt * dt;
 
     const Vector3& omegaCoriolis = *p().omegaCoriolis;
-    dR(result) -= Ri.transpose() * omegaCoriolis * dt;
+    NavState::dR(result) -= Ri.transpose() * omegaCoriolis * dt;
-    dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
+    NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
-    dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt;
+    NavState::dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt;
     if (p().use2ndOrderCoriolis) {
       Vector3 temp = omegaCoriolis.cross(
           omegaCoriolis.cross(pose_i.translation().vector()));
-      dP(result) -= 0.5 * temp * dt2;
+      NavState::dP(result) -= 0.5 * temp * dt2;
-      dV(result) -= temp * dt;
+      NavState::dV(result) -= temp * dt;
     }
   }
   return result;
@@ -169,9 +165,9 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
 
   // Rotation, translation, and velocity:
   Vector9 delta;
-  dR(delta) = Rot3::Logmap(deltaRij_biascorrected);
+  NavState::dR(delta) = Rot3::Logmap(deltaRij_biascorrected);
-  dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2;
+  NavState::dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2;
-  dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt;
+  NavState::dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt;
 
   if (p().omegaCoriolis) delta +=  integrateCoriolis(state_i);
   return delta;
@@ -186,11 +182,7 @@ NavState PreintegrationBase::predict(const NavState& state_i,
   Vector9 delta = recombinedPrediction(state_i, deltaRij_biascorrected,
       deltaPij_biascorrected, deltaVij_biascorrected);
 
-  // TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
+  return state_i.retract(delta);
-  const Pose3& pose_i = state_i.pose();
-  const Vector3& vel_i = state_i.velocity();
-  const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR(delta)), pose_i.translation() + Point3(dP(delta)));
-  return NavState(pose_j, vel_i + dV(delta));
 }
 
 //------------------------------------------------------------------------------
@@ -309,6 +301,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
     -delPdelBiasAcc(), -delPdelBiasOmega(),                         // dfP/dBias
     -delVdelBiasAcc(), -delVdelBiasOmega();                         // dfV/dBias
   }
+  // TODO(frank): Vector9 r = state_i.localCoordinates(predictedState_j); does not work ???
   Vector9 r;
   r << fR, fp, fv;
   return r;

gtsam/navigation/PreintegrationBase.h

@@ -36,23 +36,57 @@ typedef Vector3 Velocity3;
 /**
  * Navigation state: Pose (rotation, translation) + velocity
  */
-class NavState: private ProductLieGroup<Pose3, Velocity3> {
+class NavState: public ProductLieGroup<Pose3, Velocity3> {
 protected:
   typedef ProductLieGroup<Pose3, Velocity3> Base;
   typedef OptionalJacobian<9, 9> ChartJacobian;
+  using Base::first;
+  using Base::second;
 
 public:
   // constructors
   NavState() {}
   NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {}
+  NavState(const Rot3& rot, const Point3& t, const Velocity3& vel): Base(Pose3(rot, t), vel) {}
+  NavState(const Base& product) : Base(product) {}
 
   // 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; }
+
+  /// @name Manifold
+  /// @{
+
+  // NavState tangent space sugar.
+  static Eigen::Block<Vector9,3,1> dR(Vector9& v) { return v.segment<3>(0); }
+  static Eigen::Block<Vector9,3,1> dP(Vector9& v) { return v.segment<3>(3); }
+  static Eigen::Block<Vector9,3,1> dV(Vector9& v) { return v.segment<3>(6); }
+
+  // Specialize Retract/Local that agrees with IMUFactors
+  // TODO(frank): This is a very specific retract. Talk to Luca about implications.
+  NavState retract(Vector9& v, //
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
+    if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet");
+    return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v));
+  }
+  Vector9 localCoordinates(const NavState& g, //
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
+    if (H1||H2) throw std::runtime_error("NavState::localCoordinates derivatives not implemented yet");
+    Vector9 v;
+    dR(v) = rotation().logmap(g.rotation());
+    dP(v) = (g.translation() - translation()).vector();
+    dV(v) = g.velocity() - velocity();
+    return v;
+  }
+  /// @}
 };
 
+// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
+template<>
+struct traits<NavState> : internal::LieGroupTraits<NavState> {};
+
 /// @deprecated
 struct PoseVelocityBias {
   Pose3 pose;