Merge branch 'develop' into feature/Feature/FixedValues

Conflicts:
	gtsam_unstable/geometry/Event.cpp

README.md

@@ -31,6 +31,7 @@ Prerequisites:
 
 - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 
 Optional prerequisites - used automatically if findable by CMake:
 

gtsam.h

@@ -2474,18 +2474,18 @@ class ConstantBias {
 class PoseVelocityBias{
     PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
   };
-class ImuFactorPreintegratedMeasurements {
+class PreintegratedImuMeasurements {
   // Standard Constructor
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
+  PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
+  PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
-  // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
+  // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
 
   double deltaTij() const;
-  Matrix deltaRij() const;
+  gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
   Vector biasHatVector() const;
@@ -2498,25 +2498,24 @@ class ImuFactorPreintegratedMeasurements {
 
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
   gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
       Vector gravity, Vector omegaCoriolis) const;
 };
 
 virtual class ImuFactor : gtsam::NonlinearFactor {
   ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
   ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
+      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
       const gtsam::Pose3& body_P_sensor);
   // Standard Interface
-  gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
-class CombinedImuFactorPreintegratedMeasurements {
+class PreintegratedCombinedMeasurements {
   // Standard Constructor
-  CombinedImuFactorPreintegratedMeasurements(
+  PreintegratedCombinedMeasurements(
       const gtsam::imuBias::ConstantBias& bias,
       Matrix measuredAccCovariance,
       Matrix measuredOmegaCovariance,
@@ -2524,7 +2523,7 @@ class CombinedImuFactorPreintegratedMeasurements {
       Matrix biasAccCovariance,
       Matrix biasOmegaCovariance,
       Matrix biasAccOmegaInit);
-  CombinedImuFactorPreintegratedMeasurements(
+  PreintegratedCombinedMeasurements(
       const gtsam::imuBias::ConstantBias& bias,
       Matrix measuredAccCovariance,
       Matrix measuredOmegaCovariance,
@@ -2533,14 +2532,14 @@ class CombinedImuFactorPreintegratedMeasurements {
       Matrix biasOmegaCovariance,
       Matrix biasAccOmegaInit,
       bool use2ndOrderIntegration);
-  // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
+  // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol);
 
   double deltaTij() const;
-  Matrix deltaRij() const;
+  gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
   Vector biasHatVector() const;
@@ -2553,53 +2552,51 @@ class CombinedImuFactorPreintegratedMeasurements {
 
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
   gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
       Vector gravity, Vector omegaCoriolis) const;
 };
 
 virtual class CombinedImuFactor : gtsam::NonlinearFactor {
   CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+      const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
   // Standard Interface
-  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
 };
 
 #include <gtsam/navigation/AHRSFactor.h>
-class AHRSFactorPreintegratedMeasurements {
+class PreintegratedAhrsMeasurements {
   // Standard Constructor
-  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
-  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
-  AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
+  PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
 
   // get Data
-  Matrix deltaRij() const;
+  gtsam::Rot3 deltaRij() const;
   double deltaTij() const;
   Vector biasHat() const;
 
   // Standard Interface
   void integrateMeasurement(Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
   void resetIntegration() ;
 };
 
 virtual class AHRSFactor : gtsam::NonlinearFactor {
   AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
   AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
       const gtsam::Pose3& body_P_sensor);
 
   // Standard Interface
-  gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
   Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
       Vector bias) const;
   gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
       Vector omegaCoriolis) const;
 };
 

gtsam/base/ProductLieGroup.h

@@ -55,7 +55,7 @@ public:
         traits<H>::Compose(this->second,other.second));
   }
   ProductLieGroup inverse() const {
-    return ProductLieGroup(this->first.inverse(), this->second.inverse());
+    return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
   }
   ProductLieGroup compose(const ProductLieGroup& g) const {
     return (*this) * g;
@@ -74,17 +74,21 @@ public:
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
-  static ProductLieGroup Retract(const TangentVector& v) {
+  ProductLieGroup retract(const TangentVector& v, //
-    return ProductLieGroup::ChartAtOrigin::Retract(v);
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
+    if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
+    G g = traits<G>::Retract(this->first, v.template head<dimension1>());
+    H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
+    return ProductLieGroup(g,h);
   }
-  static TangentVector LocalCoordinates(const ProductLieGroup& g) {
+  TangentVector localCoordinates(const ProductLieGroup& g, //
-    return ProductLieGroup::ChartAtOrigin::Local(g);
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
-  }
+    if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
-  ProductLieGroup retract(const TangentVector& v) const {
+    typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
-    return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
+    typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
-  }
+    TangentVector v;
-  TangentVector localCoordinates(const ProductLieGroup& g) const {
+    v << v1, v2;
-    return ProductLieGroup::ChartAtOrigin::Local(between(g));
+    return v;
   }
   /// @}
 
@@ -147,51 +151,19 @@ public:
     v << v1, v2;
     return v;
   }
-  struct ChartAtOrigin {
-    static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
-      return Logmap(m, Hm);
-    }
-    static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
-      return Expmap(v, Hv);
-    }
-  };
   ProductLieGroup expmap(const TangentVector& v) const {
     return compose(ProductLieGroup::Expmap(v));
   }
   TangentVector logmap(const ProductLieGroup& g) const {
     return ProductLieGroup::Logmap(between(g));
   }
-  static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
-    return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
-  }
-  static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
-    return ProductLieGroup::ChartAtOrigin::Local(g,H1);
-  }
-  ProductLieGroup retract(const TangentVector& v, //
-      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
-    Jacobian D_g_v;
-    ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
-    ProductLieGroup h = compose(g,H1,H2);
-    if (H2) *H2 = (*H2) * D_g_v;
-    return h;
-  }
-  TangentVector localCoordinates(const ProductLieGroup& g, //
-      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
-    ProductLieGroup h = between(g,H1,H2);
-    Jacobian D_v_h;
-    TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
-    if (H1) *H1 = D_v_h * (*H1);
-    if (H2) *H2 = D_v_h * (*H2);
-    return v;
-  }
   /// @}
 
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept
 template<typename G, typename H>
-struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
+struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
-    ProductLieGroup<G, H> > {
+
-};
 } // namespace gtsam
 

gtsam/base/Testable.h

@@ -123,8 +123,8 @@ namespace gtsam {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
-      if (!actual || !expected) return false;
+      if (!actual || !expected) return true;
-      return (traits<V>::Equals(*actual,*expected, tol_));
+      return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
     }
   };
 

gtsam/geometry/Pose3.cpp

@@ -387,7 +387,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   Matrix3 UVtranspose = U * V.transpose();
   Matrix3 detWeighting = I_3x3;
   detWeighting(2, 2) = UVtranspose.determinant();
-  Rot3 R(Matrix(V * detWeighting * U.transpose()));
+  Rot3 R(Matrix3(V * detWeighting * U.transpose()));
   Point3 t = Point3(cq) - R * Point3(cp);
   return Pose3(R, t);
 }

gtsam/geometry/Rot3.h

@@ -85,11 +85,33 @@ namespace gtsam {
         double R21, double R22, double R23,
         double R31, double R32, double R33);
 
-    /** constructor from a rotation matrix */
+    /**
-    Rot3(const Matrix3& R);
+     * Constructor from a rotation matrix
+     * Version for generic matrices. Need casting to Matrix3
+     * in quaternion mode, since Eigen's quaternion doesn't
+     * allow assignment/construction from a generic matrix.
+     * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
+     */
+    template<typename Derived>
+    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
+      #ifdef GTSAM_USE_QUATERNIONS
+        quaternion_=Matrix3(R);
+      #else
+        rot_ = R;
+      #endif
+    }
 
-    /** constructor from a rotation matrix */
+    /**
-    Rot3(const Matrix& R);
+     * Constructor from a rotation matrix
+     * Overload version for Matrix3 to avoid casting in quaternion mode.
+     */
+    inline Rot3(const Matrix3& R) {
+      #ifdef GTSAM_USE_QUATERNIONS
+        quaternion_=R;
+      #else
+        rot_ = R;
+      #endif
+    }
 
     /** Constructor from a quaternion.  This can also be called using a plain
      * Vector, due to implicit conversion from Vector to Quaternion
@@ -330,6 +352,17 @@ namespace gtsam {
     Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
         OptionalJacobian<3,3> H2 = boost::none) const;
 
+    /// operator* for Vector3
+    inline Vector3 operator*(const Vector3& v) const {
+      return rotate(Point3(v)).vector();
+    }
+
+    /// rotate for Vector3
+    Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
+        OptionalJacobian<3, 3> H2 = boost::none) const {
+      return rotate(Point3(v), H1, H2).vector();
+    }
+
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
@@ -337,6 +370,12 @@ namespace gtsam {
     Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
         OptionalJacobian<3,3> H2=boost::none) const;
 
+    /// unrotate for Vector3
+    Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
+        OptionalJacobian<3, 3> H2 = boost::none) const {
+      return unrotate(Point3(v), H1, H2).vector();
+    }
+
     /// @}
     /// @name Group Action on Unit3
     /// @{

gtsam/geometry/Rot3M.cpp

@@ -50,18 +50,6 @@ Rot3::Rot3(double R11, double R12, double R13,
         R31, R32, R33;
 }
 
-/* ************************************************************************* */
-Rot3::Rot3(const Matrix3& R) {
-  rot_ = R;
-}
-
-/* ************************************************************************* */
-Rot3::Rot3(const Matrix& R) {
-  if (R.rows()!=3 || R.cols()!=3)
-    throw invalid_argument("Rot3 constructor expects 3*3 matrix");
-  rot_ = R;
-}
-
 /* ************************************************************************* */
 Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
 }
@@ -131,10 +119,8 @@ Matrix3 Rot3::transpose() const {
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
     OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
-  if (H1 || H2) {
   if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
   if (H2) *H2 = rot_;
-    }
   return Point3(rot_ * p.vector());
 }
 

gtsam/geometry/Rot3Q.cpp

@@ -48,14 +48,6 @@ namespace gtsam {
             R21, R22, R23,
             R31, R32, R33).finished()) {}
 
-  /* ************************************************************************* */
-  Rot3::Rot3(const Matrix3& R) :
-      quaternion_(R) {}
-
-  /* ************************************************************************* */
-  Rot3::Rot3(const Matrix& R) :
-      quaternion_(Matrix3(R)) {}
-
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) :
       quaternion_(q) {

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -30,7 +30,7 @@ using namespace gtsam;
 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
 
 // Camera situated at 0.5 meters high, looking down
-static const Pose3 pose1((Matrix)(Matrix(3,3) <<
+static const Pose3 pose1((Matrix3() <<
               1., 0., 0.,
               0.,-1., 0.,
               0., 0.,-1.

gtsam/geometry/tests/testPose3.cpp

@@ -182,8 +182,8 @@ TEST(Pose3, expmaps_galore_full)
   xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished();
   actual = Pose3::Expmap(xi);
   EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
-  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
+  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9));
-  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
+  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -28,7 +28,7 @@ using namespace gtsam;
 
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1((Matrix)(Matrix(3,3) <<
+static const Pose3 pose1((Matrix3() <<
               1., 0., 0.,
               0.,-1., 0.,
               0., 0.,-1.

gtsam/linear/Sampler.cpp

@@ -14,9 +14,6 @@
  * @author Alex Cunningham
  */
 
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
-
 #include <gtsam/linear/Sampler.h>
 namespace gtsam {
 
@@ -51,7 +48,7 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
     } else {
       typedef boost::normal_distribution<double> Normal;
       Normal dist(0.0, sigma);
-      boost::variate_generator<boost::minstd_rand&, Normal> norm(generator_, dist);
+      boost::variate_generator<boost::mt19937_64&, Normal> norm(generator_, dist);
       result(i) = norm();
     }
   }

gtsam/linear/Sampler.h

@@ -20,7 +20,7 @@
 
 #include <gtsam/linear/NoiseModel.h>
 
-#include <boost/random/linear_congruential.hpp>
+#include <boost/random.hpp>
 
 namespace gtsam {
 
@@ -37,7 +37,7 @@ protected:
   noiseModel::Diagonal::shared_ptr model_;
 
   /** generator */
-  boost::minstd_rand generator_;
+  boost::mt19937_64 generator_;
 
 public:
   typedef boost::shared_ptr<Sampler> shared_ptr;

gtsam/navigation/AHRSFactor.cpp

@@ -27,83 +27,50 @@ namespace gtsam {
 //------------------------------------------------------------------------------
 // Inner class PreintegratedMeasurements
 //------------------------------------------------------------------------------
-AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
+void PreintegratedAhrsMeasurements::print(const string& s) const {
-    const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
-    PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
-{
-  preintMeasCov_.setZero();
-}
-
-//------------------------------------------------------------------------------
-AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
-    PreintegratedRotation(I_3x3), biasHat_(Vector3())
-{
-  preintMeasCov_.setZero();
-}
-
-//------------------------------------------------------------------------------
-void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
   PreintegratedRotation::print(s);
   cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
   cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
-bool AHRSFactor::PreintegratedMeasurements::equals(
+bool PreintegratedAhrsMeasurements::equals(
-    const PreintegratedMeasurements& other, double tol) const {
+    const PreintegratedAhrsMeasurements& other, double tol) const {
-  return PreintegratedRotation::equals(other, tol)
+  return PreintegratedRotation::equals(other, tol) &&
-      && equal_with_abs_tol(biasHat_, other.biasHat_, tol);
+         equal_with_abs_tol(biasHat_, other.biasHat_, tol);
 }
 
 //------------------------------------------------------------------------------
-void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
+void PreintegratedAhrsMeasurements::resetIntegration() {
   PreintegratedRotation::resetIntegration();
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
-void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
+void PreintegratedAhrsMeasurements::integrateMeasurement(
-    const Vector3& measuredOmega, double deltaT,
+    const Vector3& measuredOmega, double deltaT) {
-    boost::optional<const Pose3&> body_P_sensor) {
 
-  // First we compensate the measurements for the bias
+  Matrix3 D_incrR_integratedOmega, Fr;
-  Vector3 correctedOmega = measuredOmega - biasHat_;
+  PreintegratedRotation::integrateMeasurement(measuredOmega,
-
+      biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
-  // Then compensate for sensor-body displacement: we express the quantities
-  // (originally in the IMU frame) into the body frame
-  if (body_P_sensor) {
-    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-    // rotation rate vector in the body frame
-    correctedOmega = body_R_sensor * correctedOmega;
-  }
-
-  // rotation vector describing rotation increment computed from the
-  // current rotation rate measurement
-  const Vector3 theta_incr = correctedOmega * deltaT;
-  Matrix3 D_Rincr_integratedOmega;
-  const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
-
-  // Update Jacobian
-  update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
-
-  // Update rotation and deltaTij.
-  Matrix3 Fr; // Jacobian of the update
-  updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
 
   // first order uncertainty propagation
   // the deltaT allows to pass from continuous time noise to discrete time noise
-  preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
+  preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
-      + gyroscopeCovariance() * deltaT;
 }
 
 //------------------------------------------------------------------------------
-Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
+Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias,
-    boost::optional<Matrix&> H) const {
+    OptionalJacobian<3,3> H) const {
   const Vector3 biasOmegaIncr = bias - biasHat_;
-  return biascorrectedThetaRij(biasOmegaIncr, H);
+  const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H);
+  Matrix3 D_omega_biascorrected;
+  const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
+  if (H) (*H) = D_omega_biascorrected * (*H);
+  return omega;
 }
 //------------------------------------------------------------------------------
-Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
+Vector PreintegratedAhrsMeasurements::DeltaAngles(
     const Vector& msr_gyro_t, const double msr_dt,
     const Vector3& delta_angles) {
 
@@ -121,22 +88,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
 //------------------------------------------------------------------------------
 // AHRSFactor methods
 //------------------------------------------------------------------------------
-AHRSFactor::AHRSFactor() :
+AHRSFactor::AHRSFactor(
-    _PIM_(Vector3(), Z_3x3) {
+    Key rot_i, Key rot_j, Key bias,
-}
+    const PreintegratedAhrsMeasurements& preintegratedMeasurements)
+    : Base(noiseModel::Gaussian::Covariance(
+               preintegratedMeasurements.preintMeasCov_),
+           rot_i, rot_j, bias),
+      _PIM_(preintegratedMeasurements) {}
 
-AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
-    const PreintegratedMeasurements& preintegratedMeasurements,
-    const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
-    Base(
-        noiseModel::Gaussian::Covariance(
-            preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
-        preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
-        body_P_sensor) {
-}
-
-//------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
+//------------------------------------------------------------------------------
   return boost::static_pointer_cast<gtsam::NonlinearFactor>(
       gtsam::NonlinearFactor::shared_ptr(new This(*this)));
 }
@@ -147,20 +108,13 @@ void AHRSFactor::print(const string& s,
   cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
       << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
   _PIM_.print("  preintegrated measurements:");
-  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
   noiseModel_->print("  noise model: ");
-  if (body_P_sensor_)
-    body_P_sensor_->print("  sensor pose in body frame: ");
 }
 
 //------------------------------------------------------------------------------
 bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
   const This *e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
+  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
-      && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-      && ((!body_P_sensor_ && !e->body_P_sensor_)
-          || (body_P_sensor_ && e->body_P_sensor_
-              && body_P_sensor_->equals(*e->body_P_sensor_)));
 }
 
 //------------------------------------------------------------------------------
@@ -172,8 +126,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
   const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
 
   // Coriolis term
-  const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
+  const Vector3 coriolis = _PIM_.integrateCoriolis(Ri);
-  const Matrix3 coriolisHat = skewSymmetric(coriolis);
   const Vector3 correctedOmega = biascorrectedOmega - coriolis;
 
   // Prediction
@@ -191,7 +144,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
   if (H1) {
     // dfR/dRi
     H1->resize(3, 3);
-    Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat;
+    Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
     (*H1)
         << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
   }
@@ -199,7 +152,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
   if (H2) {
     // dfR/dPosej
     H2->resize(3, 3);
-    (*H2) << D_fR_fRrot * Matrix3::Identity();
+    (*H2) << D_fR_fRrot;
   }
 
   if (H3) {
@@ -215,15 +168,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
 }
 
 //------------------------------------------------------------------------------
-Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
+Rot3 AHRSFactor::Predict(
-    const PreintegratedMeasurements preintegratedMeasurements,
+    const Rot3& rot_i, const Vector3& bias,
-    const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
+    const PreintegratedAhrsMeasurements preintegratedMeasurements) {
-
   const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
 
   // Coriolis term
-  const Vector3 coriolis = //
+  const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i);
-      preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
 
   const Vector3 correctedOmega = biascorrectedOmega - coriolis;
   const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@@ -231,4 +182,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
   return rot_i.compose(correctedDeltaRij);
 }
 
-} //namespace gtsam
+//------------------------------------------------------------------------------
+AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
+                       const PreintegratedMeasurements& pim,
+                       const Vector3& omegaCoriolis,
+                       const boost::optional<Pose3>& body_P_sensor)
+    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias),
+      _PIM_(pim) {
+  boost::shared_ptr<PreintegratedMeasurements::Params> p =
+      boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
+  p->body_P_sensor = body_P_sensor;
+  _PIM_.p_ = p;
+}
+
+//------------------------------------------------------------------------------
+Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
+                         const PreintegratedMeasurements pim,
+                         const Vector3& omegaCoriolis,
+                         const boost::optional<Pose3>& body_P_sensor) {
+  boost::shared_ptr<PreintegratedMeasurements::Params> p =
+      boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
+  p->omegaCoriolis = omegaCoriolis;
+  p->body_P_sensor = body_P_sensor;
+  PreintegratedMeasurements newPim = pim;
+  newPim.p_ = p;
+  return Predict(rot_i, bias, newPim);
+}
+
+}  // namespace gtsam

gtsam/navigation/AHRSFactor.h

@@ -26,91 +26,94 @@
 
 namespace gtsam {
 
-class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
+/**
-public:
+ * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope
-
-  /**
-   * CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope
  * measurements (rotation rates) and the corresponding covariance matrix.
-   * The measurements are then used to build the Preintegrated AHRS factor.
+ * Can be built incrementally so as to avoid costly integration at time of factor construction.
-   * Can be built incrementally so as to avoid costly integration at time of
-   * factor construction.
  */
-  class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
+class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
+
+ protected:
+
+  Vector3 biasHat_; ///< Angular rate bias values used during preintegration.
+  Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
+
+  /// Default constructor, only for serialization
+  PreintegratedAhrsMeasurements() {}
 
   friend class AHRSFactor;
 
-  protected:
-    Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
-    Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
-
  public:
 
-    /// Default constructor
-    PreintegratedMeasurements();
-
   /**
    *  Default constructor, initialize with no measurements
    *  @param bias Current estimate of acceleration and rotation rate biases
-     *  @param measuredOmegaCovariance Covariance matrix of measured angular rate
    */
-    PreintegratedMeasurements(const Vector3& bias,
+  PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
-        const Matrix3& measuredOmegaCovariance);
+      const Vector3& biasHat) :
+      PreintegratedRotation(p), biasHat_(biasHat) {
+    resetIntegration();
+  }
 
-    Vector3 biasHat() const {
+  const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
-      return biasHat_;
+  const Vector3& biasHat() const { return biasHat_; }
-    }
+  const Matrix3& preintMeasCov() const { return preintMeasCov_; }
-    const Matrix3& preintMeasCov() const {
-      return preintMeasCov_;
-    }
 
   /// print
   void print(const std::string& s = "Preintegrated Measurements: ") const;
 
   /// equals
-    bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
+  bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
 
-    /// TODO: Document
+  /// Reset inetgrated quantities to zero
   void resetIntegration();
 
   /**
    * Add a single Gyroscope measurement to the preintegration.
    * @param measureOmedga Measured angular velocity (in body frame)
    * @param deltaT Time step
-     * @param body_P_sensor Optional sensor frame
    */
-    void integrateMeasurement(const Vector3& measuredOmega, double deltaT,
+  void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
-        boost::optional<const Pose3&> body_P_sensor = boost::none);
 
   /// Predict bias-corrected incremental rotation
   /// TODO: The matrix Hbias is the derivative of predict? Unit-test?
-    Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
+  Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
-        boost::none) const;
 
   // This function is only used for test purposes
   // (compare numerical derivatives wrt analytic ones)
   static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
       const Vector3& delta_angles);
 
-  private:
+  /// @deprecated constructor
+  PreintegratedAhrsMeasurements(const Vector3& biasHat,
+                                const Matrix3& measuredOmegaCovariance)
+      : PreintegratedRotation(boost::make_shared<Params>()),
+        biasHat_(biasHat) {
+    p_->gyroscopeCovariance = measuredOmegaCovariance;
+    resetIntegration();
+  }
+
+private:
 
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
+    ar & BOOST_SERIALIZATION_NVP(p_);
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
   }
-  };
+};
+
+class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
 
-private:
   typedef AHRSFactor This;
   typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
 
-  PreintegratedMeasurements _PIM_;
+  PreintegratedAhrsMeasurements _PIM_;
-  Vector3 gravity_;
+
-  Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+  /** Default constructor - only use for serialization */
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
+  AHRSFactor() {}
 
 public:
 
@@ -121,22 +124,15 @@ public:
   typedef boost::shared_ptr<AHRSFactor> shared_ptr;
 #endif
 
-  /** Default constructor - only use for serialization */
-  AHRSFactor();
-
   /**
    * Constructor
    * @param rot_i previous rot key
    * @param rot_j current rot key
    * @param bias  previous bias key
    * @param preintegratedMeasurements preintegrated measurements
-   * @param omegaCoriolis rotation rate of the inertial frame
-   * @param body_P_sensor Optional pose of the sensor frame in the body frame
    */
   AHRSFactor(Key rot_i, Key rot_j, Key bias,
-      const PreintegratedMeasurements& preintegratedMeasurements,
+      const PreintegratedAhrsMeasurements& preintegratedMeasurements);
-      const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none);
 
   virtual ~AHRSFactor() {
   }
@@ -152,14 +148,10 @@ public:
   virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
 
   /// Access the preintegrated measurements.
-  const PreintegratedMeasurements& preintegratedMeasurements() const {
+  const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
     return _PIM_;
   }
 
-  const Vector3& omegaCoriolis() const {
-    return omegaCoriolis_;
-  }
-
   /** implement functions needed to derive from Factor */
 
   /// vector of errors
@@ -169,10 +161,25 @@ public:
           boost::none) const;
 
   /// predicted states from IMU
+  /// TODO(frank): relationship with PIM predict ??
+  static Rot3 Predict(
+      const Rot3& rot_i, const Vector3& bias,
+      const PreintegratedAhrsMeasurements preintegratedMeasurements);
+
+  /// @deprecated name
+  typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
+
+  /// @deprecated constructor
+  AHRSFactor(Key rot_i, Key rot_j, Key bias,
+      const PreintegratedMeasurements& preintegratedMeasurements,
+      const Vector3& omegaCoriolis,
+      const boost::optional<Pose3>& body_P_sensor = boost::none);
+
+  /// @deprecated static function
   static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
       const PreintegratedMeasurements preintegratedMeasurements,
       const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none);
+      const boost::optional<Pose3>& body_P_sensor = boost::none);
 
 private:
 
@@ -184,13 +191,9 @@ private:
         & boost::serialization::make_nvp("NoiseModelFactor3",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
-    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
 
 };
 // AHRSFactor
 
-typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
-
 } //namespace gtsam

gtsam/navigation/CombinedImuFactor.cpp

@@ -29,157 +29,125 @@ namespace gtsam {
 using namespace std;
 
 //------------------------------------------------------------------------------
-// Inner class CombinedPreintegratedMeasurements
+// Inner class PreintegratedCombinedMeasurements
 //------------------------------------------------------------------------------
-CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
+void PreintegratedCombinedMeasurements::print(
-    const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
-    const Matrix3& measuredOmegaCovariance,
-    const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
-    const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit,
-    const bool use2ndOrderIntegration) :
-    PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
-        integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_(
-        biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_(
-        biasAccOmegaInit) {
-  preintMeasCov_.setZero();
-}
-
-//------------------------------------------------------------------------------
-void CombinedImuFactor::CombinedPreintegratedMeasurements::print(
     const string& s) const {
   PreintegrationBase::print(s);
-  cout << "  biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
-  cout << "  biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
-  cout << "  biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
   cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
-bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(
+bool PreintegratedCombinedMeasurements::equals(
-    const CombinedPreintegratedMeasurements& expected, double tol) const {
+    const PreintegratedCombinedMeasurements& other, double tol) const {
-  return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_,
+  return PreintegrationBase::equals(other, tol) &&
-      tol)
+         equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
-      && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_,
-          tol)
-      && equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
-      && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
-      && PreintegrationBase::equals(expected, tol);
 }
 
 //------------------------------------------------------------------------------
-void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() {
+void PreintegratedCombinedMeasurements::resetIntegration() {
   PreintegrationBase::resetIntegration();
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
-void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
+// sugar for derivative blocks
-    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+#define D_R_R(H) (H)->block<3,3>(0,0)
-    boost::optional<const Pose3&> body_P_sensor,
+#define D_R_t(H) (H)->block<3,3>(0,3)
-    boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
+#define D_R_v(H) (H)->block<3,3>(0,6)
+#define D_t_R(H) (H)->block<3,3>(3,0)
+#define D_t_t(H) (H)->block<3,3>(3,3)
+#define D_t_v(H) (H)->block<3,3>(3,6)
+#define D_v_R(H) (H)->block<3,3>(6,0)
+#define D_v_t(H) (H)->block<3,3>(6,3)
+#define D_v_v(H) (H)->block<3,3>(6,6)
+#define D_a_a(H) (H)->block<3,3>(9,9)
+#define D_g_g(H) (H)->block<3,3>(12,12)
 
-  // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
+//------------------------------------------------------------------------------
-  // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
+void PreintegratedCombinedMeasurements::integrateMeasurement(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
 
-  Vector3 correctedAcc, correctedOmega;
+  const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
-  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
-      correctedAcc, correctedOmega, body_P_sensor);
 
-  const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+  // Update preintegrated measurements.
-  Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
+  Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
-  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
+  Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
-
+  Matrix93 G1,G2;
-  // Update Jacobians
+  PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
-  /* ----------------------------------------------------------------------------------------------------------------------- */
+      &D_incrR_integratedOmega, &F_9x9, &G1, &G2);
-  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
-      deltaT);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
   // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
   // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
   /* ----------------------------------------------------------------------------------------------------------------------- */
-  const Matrix3 R_i = deltaRij(); // store this
-  // Update preintegrated measurements. TODO Frank moved from end of this function !!!
-  Matrix9 F_9x9;
-  updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
 
   // Single Jacobians to propagate covariance
-  Matrix3 H_vel_biasacc = -R_i * deltaT;
+  Matrix3 H_vel_biasacc = -dRij * deltaT;
-  Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
+  Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix F(15, 15);
+  Eigen::Matrix<double,15,15> F;
-  // for documentation:
-  //  F << I_3x3,    I_3x3 * deltaT,   Z_3x3,             Z_3x3,            Z_3x3,
-  //       Z_3x3,    I_3x3,            H_vel_angles,      H_vel_biasacc,    Z_3x3,
-  //       Z_3x3,    Z_3x3,            H_angles_angles,   Z_3x3,            H_angles_biasomega,
-  //       Z_3x3,    Z_3x3,            Z_3x3,             I_3x3,            Z_3x3,
-  //       Z_3x3,    Z_3x3,            Z_3x3,             Z_3x3,            I_3x3;
   F.setZero();
   F.block<9, 9>(0, 0) = F_9x9;
+  F.block<3, 3>(0, 12) = H_angles_biasomega;
+  F.block<3, 3>(6, 9) = H_vel_biasacc;
   F.block<6, 6>(9, 9) = I_6x6;
-  F.block<3, 3>(3, 9) = H_vel_biasacc;
-  F.block<3, 3>(6, 12) = H_angles_biasomega;
 
   // first order uncertainty propagation
   // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
-  Matrix G_measCov_Gt = Matrix::Zero(15, 15);
+  Eigen::Matrix<double,15,15> G_measCov_Gt;
+  G_measCov_Gt.setZero(15, 15);
 
-//   BLOCK DIAGONAL TERMS
+  // BLOCK DIAGONAL TERMS
-  G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance();
+  D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
-  G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
+  D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
-      * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0))
+      * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
       * (H_vel_biasacc.transpose());
-  G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
+  D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
-      * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3))
+      * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
       * (H_angles_biasomega.transpose());
-  G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_;
+  D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance;
-  G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_;
+  D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance;
-  // OFF BLOCK DIAGONAL TERMS
-  Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0)
-      * H_angles_biasomega.transpose();
-  G_measCov_Gt.block<3, 3>(3, 6) = block23;
-  G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
 
-  // F_test and G_test are used for testing purposes and are not needed by the factor
+  // OFF BLOCK DIAGONAL TERMS
-  if (F_test) {
+  Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
-    F_test->resize(15, 15);
+      * H_angles_biasomega.transpose();
-    (*F_test) << F;
+  D_v_R(&G_measCov_Gt) = temp;
-  }
+  D_R_v(&G_measCov_Gt) = temp.transpose();
-  if (G_test) {
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
-    G_test->resize(15, 21);
-    // This is for testing & documentation
-    ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
-    (*G_test) << //
-        I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
-    Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
-    Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
-    Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
-    Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
-  }
 }
 
+//------------------------------------------------------------------------------
+PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
+    const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
+    const Matrix3& measuredOmegaCovariance,
+    const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
+    const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
+    const bool use2ndOrderIntegration) {
+  if (!use2ndOrderIntegration)
+    throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
+  biasHat_ = biasHat;
+  boost::shared_ptr<Params> p = Params::MakeSharedD();
+  p->gyroscopeCovariance = measuredOmegaCovariance;
+  p->accelerometerCovariance = measuredAccCovariance;
+  p->integrationCovariance = integrationErrorCovariance;
+  p->biasAccCovariance = biasAccCovariance;
+  p->biasOmegaCovariance = biasOmegaCovariance;
+  p->biasAccOmegaInit = biasAccOmegaInit;
+  p_ = p;
+  resetIntegration();
+  preintMeasCov_.setZero();
+}
 //------------------------------------------------------------------------------
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------
-CombinedImuFactor::CombinedImuFactor() :
+CombinedImuFactor::CombinedImuFactor(
-    ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3,
+    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
-        Z_3x3, Z_6x6) {
+    const PreintegratedCombinedMeasurements& pim)
-}
+    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-
+           pose_j, vel_j, bias_i, bias_j),
-//------------------------------------------------------------------------------
+      _PIM_(pim) {}
-CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
-    Key vel_j, Key bias_i, Key bias_j,
-    const CombinedPreintegratedMeasurements& preintegratedMeasurements,
-    const Vector3& gravity, const Vector3& omegaCoriolis,
-    boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
-    Base(
-        noiseModel::Gaussian::Covariance(
-            preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
-        vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis,
-        body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
-}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@@ -194,17 +162,14 @@ void CombinedImuFactor::print(const string& s,
       << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
       << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
       << keyFormatter(this->key6()) << ")\n";
-  ImuFactorBase::print("");
   _PIM_.print("  preintegrated measurements:");
   this->noiseModel_->print("  noise model: ");
 }
 
 //------------------------------------------------------------------------------
-bool CombinedImuFactor::equals(const NonlinearFactor& expected,
+bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
-    double tol) const {
+  const This* e = dynamic_cast<const This*>(&other);
-  const This *e = dynamic_cast<const This*>(&expected);
+  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
-      && ImuFactorBase::equals(*e, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -224,8 +189,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   Matrix93 D_r_vel_i, D_r_vel_j;
 
   // error wrt preintegrated measurements
-  Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
+  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
-      bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_,
       H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
       H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
 
@@ -234,25 +198,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
     H1->resize(15, 6);
     H1->block<9, 6>(0, 0) = D_r_pose_i;
     // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
-    H1->block<6, 6>(9, 0) = Z_6x6;
+    H1->block<6, 6>(9, 0).setZero();
   }
   if (H2) {
     H2->resize(15, 3);
     H2->block<9, 3>(0, 0) = D_r_vel_i;
     // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
-    H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
+    H2->block<6, 3>(9, 0).setZero();
   }
   if (H3) {
     H3->resize(15, 6);
     H3->block<9, 6>(0, 0) = D_r_pose_j;
     // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
-    H3->block<6, 6>(9, 0) = Z_6x6;
+    H3->block<6, 6>(9, 0).setZero();
   }
   if (H4) {
     H4->resize(15, 3);
     H4->block<9, 3>(0, 0) = D_r_vel_j;
     // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
-    H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
+    H4->block<6, 3>(9, 0).setZero();
   }
   if (H5) {
     H5->resize(15, 6);
@@ -262,15 +226,47 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   }
   if (H6) {
     H6->resize(15, 6);
-    H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6);
+    H6->block<9, 6>(0, 0).setZero();
     // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
     H6->block<6, 6>(9, 0) = Hbias_j;
   }
 
   // overall error
   Vector r(15);
-  r << r_pvR, fbias; // vector of size 15
+  r << r_Rpv, fbias; // vector of size 15
   return r;
 }
 
+//------------------------------------------------------------------------------
+CombinedImuFactor::CombinedImuFactor(
+    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+    const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
+    const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
+    const bool use2ndOrderCoriolis)
+    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+           pose_j, vel_j, bias_i, bias_j),
+      _PIM_(pim) {
+  boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
+      boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
+  p->n_gravity = n_gravity;
+  p->omegaCoriolis = omegaCoriolis;
+  p->body_P_sensor = body_P_sensor;
+  p->use2ndOrderCoriolis = use2ndOrderCoriolis;
+  _PIM_.p_ = p;
+}
+//------------------------------------------------------------------------------
+void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+                                Pose3& pose_j, Vector3& vel_j,
+                                const imuBias::ConstantBias& bias_i,
+                                CombinedPreintegratedMeasurements& pim,
+                                const Vector3& n_gravity,
+                                const Vector3& omegaCoriolis,
+                                const bool use2ndOrderCoriolis) {
+  // use deprecated predict
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
+      omegaCoriolis, use2ndOrderCoriolis);
+  pose_j = pvb.pose;
+  vel_j = pvb.velocity;
+}
+
 } /// namespace gtsam

gtsam/navigation/CombinedImuFactor.h

@@ -24,21 +24,17 @@
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/navigation/PreintegrationBase.h>
-#include <gtsam/navigation/ImuFactorBase.h>
+#include <gtsam/base/Matrix.h>
-#include <gtsam/base/debug.h>
 
 namespace gtsam {
 
-/**
+/*
- *
- * @addtogroup SLAM
- *
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
  * conditionally independent sets in factor graphs: a unifying perspective based
  * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
  *
- ** REFERENCES:
+ * REFERENCES:
  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
  *     Volume 2, 2008.
  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
@@ -46,14 +42,137 @@ namespace gtsam {
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
+ *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
+ *     Robotics: Science and Systems (RSS), 2015.
  */
 
+/**
+ * PreintegratedCombinedMeasurements integrates the IMU measurements
+ * (rotation rates and accelerations) and the corresponding covariance matrix.
+ * The measurements are then used to build the CombinedImuFactor. Integration
+ * is done incrementally (ideally, one integrates the measurement as soon as
+ * it is received from the IMU) so as to avoid costly integration at time of
+ * factor construction.
+ *
+ * @addtogroup SLAM
+ */
+class PreintegratedCombinedMeasurements : public PreintegrationBase {
+
+  /// Parameters for pre-integration:
+  /// Usage: Create just a single Params and pass a shared pointer to the constructor
+  struct Params : PreintegrationBase::Params {
+    Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
+    Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
+    Matrix6 biasAccOmegaInit;     ///< covariance of bias used for pre-integration
+
+    /// See two named constructors below for good values of n_gravity in body frame
+    Params(const Vector3& n_gravity) :
+        PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
+            I_3x3), biasAccOmegaInit(I_6x6) {
+    }
+
+    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
+    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, g));
+    }
+
+    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, -g));
+    }
+
+   private:
+    /// Default constructor makes unitialized params struct
+    Params() {}
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template <class ARCHIVE>
+    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+      ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
+      ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
+      ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
+      ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
+    }
+  };
+
+ protected:
+  /* Covariance matrix of the preintegrated measurements
+   * COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
+   * (first-order propagation from *measurementCovariance*).
+   * PreintegratedCombinedMeasurements also include the biases and keep the correlation
+   * between the preintegrated measurements and the biases
+   */
+  Eigen::Matrix<double, 15, 15> preintMeasCov_;
+
+  PreintegratedCombinedMeasurements() {}
+
+  friend class CombinedImuFactor;
+
+ public:
+  /**
+   *  Default constructor, initializes the class with no measurements
+   *  @param bias Current estimate of acceleration and rotation rate biases
+   */
+  PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
+                                    const imuBias::ConstantBias& biasHat)
+      : PreintegrationBase(p, biasHat) {
+    preintMeasCov_.setZero();
+  }
+
+  Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
+
+  /// print
+  void print(const std::string& s = "Preintegrated Measurements:") const;
+
+  /// equals
+  bool equals(const PreintegratedCombinedMeasurements& expected,
+              double tol = 1e-9) const;
+
+  /// Re-initialize PreintegratedCombinedMeasurements
+  void resetIntegration();
+
+  /**
+   * Add a single IMU measurement to the preintegration.
+   * @param measuredAcc Measured acceleration (in body frame, as given by the
+   * sensor)
+   * @param measuredOmega Measured angular velocity (as given by the sensor)
+   * @param deltaT Time interval between two consecutive IMU measurements
+   * @param body_P_sensor Optional sensor frame (pose of the IMU in the body
+   * frame)
+   */
+  void integrateMeasurement(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, double deltaT);
+
+  /// methods to access class variables
+  Matrix preintMeasCov() const { return preintMeasCov_; }
+
+  /// deprecated constructor
+  /// NOTE(frank): assumes Z-Down convention, only second order integration supported
+  PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
+      const Matrix3& measuredAccCovariance,
+      const Matrix3& measuredOmegaCovariance,
+      const Matrix3& integrationErrorCovariance,
+      const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
+      const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
+
+ private:
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
+    ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
+  }
+};
+
 /**
  * CombinedImuFactor is a 6-ways factor involving previous state (pose and
  * velocity of the vehicle, as well as bias at previous time step), and current
  * state (pose, velocity, bias at current time step). Following the pre-
  * integration scheme proposed in [2], the CombinedImuFactor includes many IMU
- * measurements, which are "summarized" using the CombinedPreintegratedMeasurements
+ * measurements, which are "summarized" using the PreintegratedCombinedMeasurements
  * class. There are 3 main differences wrpt the ImuFactor class:
  * 1) The factor is 6-ways, meaning that it also involves both biases (previous
  *    and current time step).Therefore, the factor internally imposes the biases
@@ -61,105 +180,26 @@ namespace gtsam {
  *    "biasOmegaCovariance" described the random walk that models bias evolution.
  * 2) The preintegration covariance takes into account the noise in the bias
  *    estimate used for integration.
- * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves
+ * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves
  *    the correlation between the bias uncertainty and the preintegrated
  *    measurements uncertainty.
+ *
+ * @addtogroup SLAM
  */
 class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
-    Vector3, imuBias::ConstantBias, imuBias::ConstantBias>, public ImuFactorBase {
+    Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
 public:
 
-  /**
-   * CombinedPreintegratedMeasurements integrates the IMU measurements
-   * (rotation rates and accelerations) and the corresponding covariance matrix.
-   * The measurements are then used to build the CombinedImuFactor. Integration
-   * is done incrementally (ideally, one integrates the measurement as soon as
-   * it is received from the IMU) so as to avoid costly integration at time of
-   * factor construction.
-   */
-  class CombinedPreintegratedMeasurements: public PreintegrationBase {
-
-    friend class CombinedImuFactor;
-
-  protected:
-
-    Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk
-    Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
-    Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
-
-    Eigen::Matrix<double, 15, 15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
-    ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
-    ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
-    ///< between the preintegrated measurements and the biases
-
-  public:
-
-    /**
-     *  Default constructor, initializes the class with no measurements
-     *  @param bias                       Current estimate of acceleration and rotation rate biases
-     *  @param measuredAccCovariance      Covariance matrix of measuredAcc
-     *  @param measuredOmegaCovariance    Covariance matrix of measured Angular Rate
-     *  @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
-     *  @param biasAccCovariance          Covariance matrix of biasAcc (random walk describing BIAS evolution)
-     *  @param biasOmegaCovariance        Covariance matrix of biasOmega (random walk describing BIAS evolution)
-     *  @param biasAccOmegaInit           Covariance of biasAcc & biasOmega when preintegrating measurements
-     *  @param use2ndOrderIntegration     Controls the order of integration
-     *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
-     */
-    CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias,
-        const Matrix3& measuredAccCovariance,
-        const Matrix3& measuredOmegaCovariance,
-        const Matrix3& integrationErrorCovariance,
-        const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
-        const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration =
-            false);
-
-    /// print
-    void print(const std::string& s = "Preintegrated Measurements:") const;
-
-    /// equals
-    bool equals(const CombinedPreintegratedMeasurements& expected, double tol =
-        1e-9) const;
-
-    /// Re-initialize CombinedPreintegratedMeasurements
-    void resetIntegration();
-
-    /**
-     * Add a single IMU measurement to the preintegration.
-     * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
-     * @param measuredOmega Measured angular velocity (as given by the sensor)
-     * @param deltaT Time interval between two consecutive IMU measurements
-     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
-     */
-    void integrateMeasurement(const Vector3& measuredAcc,
-        const Vector3& measuredOmega, double deltaT,
-        boost::optional<const Pose3&> body_P_sensor = boost::none,
-        boost::optional<Matrix&> F_test = boost::none,
-        boost::optional<Matrix&> G_test = boost::none);
-
-    /// methods to access class variables
-    Matrix preintMeasCov() const {
-      return preintMeasCov_;
-    }
-
-  private:
-
-    /// Serialization function
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-      ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
-    }
-  };
-
 private:
 
   typedef CombinedImuFactor This;
   typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
       imuBias::ConstantBias, imuBias::ConstantBias> Base;
 
-  CombinedPreintegratedMeasurements _PIM_;
+  PreintegratedCombinedMeasurements _PIM_;
+
+  /** Default constructor - only use for serialization */
+  CombinedImuFactor() {}
 
 public:
 
@@ -170,9 +210,6 @@ public:
   typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
 #endif
 
-  /** Default constructor - only use for serialization */
-  CombinedImuFactor();
-
   /**
    * Constructor
    * @param pose_i Previous pose key
@@ -181,21 +218,13 @@ public:
    * @param vel_j  Current velocity key
    * @param bias_i Previous bias key
    * @param bias_j Current bias key
-   * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements
+   * @param PreintegratedCombinedMeasurements Combined IMU measurements
-   * @param gravity Gravity vector expressed in the global frame
-   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
-   * @param body_P_sensor Optional pose of the sensor frame in the body frame
-   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
    */
-  CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
+  CombinedImuFactor(
-      Key bias_j,
+      Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
-      const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+      const PreintegratedCombinedMeasurements& preintegratedMeasurements);
-      const Vector3& gravity, const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none,
-      const bool use2ndOrderCoriolis = false);
 
-  virtual ~CombinedImuFactor() {
+  virtual ~CombinedImuFactor() {}
-  }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@@ -211,7 +240,7 @@ public:
 
   /** Access the preintegrated measurements. */
 
-  const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
+  const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
     return _PIM_;
   }
 
@@ -226,20 +255,22 @@ public:
       boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
           boost::none, boost::optional<Matrix&> H6 = boost::none) const;
 
-  /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
+  /// @deprecated typename
+  typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
+
+  /// @deprecated constructor
+  CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
+                    Key bias_j, const CombinedPreintegratedMeasurements& pim,
+                    const Vector3& n_gravity, const Vector3& omegaCoriolis,
+                    const boost::optional<Pose3>& body_P_sensor = boost::none,
+                    const bool use2ndOrderCoriolis = false);
+
+  // @deprecated use PreintegrationBase::predict
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
                       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-      const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
+                      CombinedPreintegratedMeasurements& pim,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+                      const Vector3& n_gravity, const Vector3& omegaCoriolis,
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
+                      const bool use2ndOrderCoriolis = false);
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
-    PoseVelocityBias PVB(
-        PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-            use2ndOrderCoriolis, deltaPij_biascorrected_out,
-            deltaVij_biascorrected_out));
-    pose_j = PVB.pose;
-    vel_j = PVB.velocity;
-  }
 
 private:
 
@@ -250,13 +281,8 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor6",
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
-    ar & BOOST_SERIALIZATION_NVP(gravity_);
-    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
 };
 // class CombinedImuFactor
 
-typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
-
 } /// namespace gtsam

gtsam/navigation/ImuBias.h

@@ -78,20 +78,18 @@ public:
 
   /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
   Vector3 correctAccelerometer(const Vector3& measurement,
-      boost::optional<Matrix&> H = boost::none) const {
+      OptionalJacobian<3, 6> H = boost::none) const {
     if (H) {
-      H->resize(3, 6);
+      (*H) << -I_3x3, Z_3x3;
-      (*H) << -Matrix3::Identity(), Matrix3::Zero();
     }
     return measurement - biasAcc_;
   }
 
   /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
   Vector3 correctGyroscope(const Vector3& measurement,
-      boost::optional<Matrix&> H = boost::none) const {
+      OptionalJacobian<3, 6> H = boost::none) const {
     if (H) {
-      H->resize(3, 6);
+      (*H) << Z_3x3, -I_3x3;
-      (*H) << Matrix3::Zero(), -Matrix3::Identity();
     }
     return measurement - biasGyro_;
   }

gtsam/navigation/ImuFactor.cpp

@@ -31,113 +31,91 @@ using namespace std;
 //------------------------------------------------------------------------------
 // Inner class PreintegratedMeasurements
 //------------------------------------------------------------------------------
-ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
+void PreintegratedImuMeasurements::print(const string& s) const {
-    const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
-    const Matrix3& measuredOmegaCovariance,
-    const Matrix3& integrationErrorCovariance,
-    const bool use2ndOrderIntegration) :
-    PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
-        integrationErrorCovariance, use2ndOrderIntegration) {
-  preintMeasCov_.setZero();
-}
-
-//------------------------------------------------------------------------------
-void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
   PreintegrationBase::print(s);
+  cout << "    preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
 }
 
 //------------------------------------------------------------------------------
-bool ImuFactor::PreintegratedMeasurements::equals(
+bool PreintegratedImuMeasurements::equals(
-    const PreintegratedMeasurements& expected, double tol) const {
+    const PreintegratedImuMeasurements& other, double tol) const {
-  return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
+  return PreintegrationBase::equals(other, tol)
-      && PreintegrationBase::equals(expected, tol);
+      && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
 }
 
 //------------------------------------------------------------------------------
-void ImuFactor::PreintegratedMeasurements::resetIntegration() {
+void PreintegratedImuMeasurements::resetIntegration() {
   PreintegrationBase::resetIntegration();
   preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
-void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
+void PreintegratedImuMeasurements::integrateMeasurement(
-    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
-    boost::optional<const Pose3&> body_P_sensor, OptionalJacobian<9, 9> F_test,
-    OptionalJacobian<9, 9> G_test) {
 
-  Vector3 correctedAcc, correctedOmega;
+  static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
-  correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
-      correctedAcc, correctedOmega, body_P_sensor);
-
-  const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-  Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
-  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
-
-  // Update Jacobians
-  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
 
   // Update preintegrated measurements (also get Jacobian)
-  const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
   Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
-  updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
+  Matrix93 G1, G2;
+  Matrix3 D_incrR_integratedOmega;
+  PreintegrationBase::update(measuredAcc, measuredOmega, dt,
+      &D_incrR_integratedOmega, &F, &G1, &G2);
 
   // first order covariance propagation:
-  // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
+  // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
-  /* ----------------------------------------------------------------------------------------------------------------------- */
+  /* --------------------------------------------------------------------------------------------*/
-  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
   // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-  // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise,
+#ifdef OLD_JACOBIAN_CALCULATION
-  // as G and measurementCovariance are block-diagonal matrices
+  Matrix9 G;
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+  G << G1, Gi, G2;
-  preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
+  Matrix9 Cov;
-  preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance()
+  Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
-      * R_i.transpose() * deltaT;
+      Z_3x3, p().integrationCovariance * dt, Z_3x3,
-  preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
+      Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
-      * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
+#else
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+      + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
+      + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
+      + G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
+#endif
+}
 
-  Matrix3 F_pos_noiseacc;
+//------------------------------------------------------------------------------
-  if (use2ndOrderIntegration()) {
+PreintegratedImuMeasurements::PreintegratedImuMeasurements(
-    F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
+    const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
-    preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
+    const Matrix3& measuredOmegaCovariance,
-        * accelerometerCovariance() * F_pos_noiseacc.transpose();
+    const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
-    Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
+  if (!use2ndOrderIntegration)
-    preintMeasCov_.block<3, 3>(0, 3) += temp;
+    throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
-    preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
+  biasHat_ = biasHat;
-  }
+  boost::shared_ptr<Params> p = Params::MakeSharedD();
+  p->gyroscopeCovariance = measuredOmegaCovariance;
+  p->accelerometerCovariance = measuredAccCovariance;
+  p->integrationCovariance = integrationErrorCovariance;
+  p_ = p;
+  resetIntegration();
+}
 
-  // F_test and G_test are given as output for testing purposes and are not needed by the factor
+//------------------------------------------------------------------------------
-  if (F_test) { // This in only for testing
+void PreintegratedImuMeasurements::integrateMeasurement(
-    (*F_test) << F;
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-  }
+    boost::optional<Pose3> body_P_sensor) {
-  if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
+  // modify parameters to accommodate deprecated method:-(
-    if (!use2ndOrderIntegration())
+  p_->body_P_sensor = body_P_sensor;
-      F_pos_noiseacc = Z_3x3;
+  integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-
-    //           intNoise          accNoise           omegaNoise
-    (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
-    Z_3x3, R_i * deltaT, Z_3x3, // vel
-    Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
-  }
 }
 
 //------------------------------------------------------------------------------
 // ImuFactor methods
-//------------------------------------------------------------------------------
-ImuFactor::ImuFactor() :
-    ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {
-}
-
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-    const PreintegratedMeasurements& preintegratedMeasurements,
+    const PreintegratedImuMeasurements& pim) :
-    const Vector3& gravity, const Vector3& omegaCoriolis,
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-    boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
+        pose_j, vel_j, bias), _PIM_(pim) {
-    Base(
-        noiseModel::Gaussian::Covariance(
-            preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
-        vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor,
-        use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
 }
 
 //------------------------------------------------------------------------------
@@ -152,17 +130,18 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
       << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
       << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
       << ")\n";
-  ImuFactorBase::print("");
+  Base::print("");
   _PIM_.print("  preintegrated measurements:");
   // Print standard deviations on covariance only
-  cout << "  noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
+  cout << "  noise model sigmas: " << this->noiseModel_->sigmas().transpose()
+      << endl;
 }
 
 //------------------------------------------------------------------------------
-bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
+bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
-  const This *e = dynamic_cast<const This*>(&expected);
+  const This *e = dynamic_cast<const This*>(&other);
   return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
-      && ImuFactorBase::equals(*e, tol);
+      && Base::equals(*e, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -171,9 +150,36 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
     const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
     boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
-
   return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
-      gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
+      H1, H2, H3, H4, H5);
 }
 
-} /// namespace gtsam
+//------------------------------------------------------------------------------
+ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
+    const PreintegratedMeasurements& pim, const Vector3& n_gravity,
+    const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
+    const bool use2ndOrderCoriolis) :
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+        pose_j, vel_j, bias), _PIM_(pim) {
+  boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
+      PreintegratedMeasurements::Params>(pim.p());
+  p->n_gravity = n_gravity;
+  p->omegaCoriolis = omegaCoriolis;
+  p->body_P_sensor = body_P_sensor;
+  p->use2ndOrderCoriolis = use2ndOrderCoriolis;
+  _PIM_.p_ = p;
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+    Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
+    PreintegratedMeasurements& pim, const Vector3& n_gravity,
+    const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
+  // use deprecated predict
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
+      omegaCoriolis, use2ndOrderCoriolis);
+  pose_j = pvb.pose;
+  vel_j = pvb.velocity;
+}
+
+} // namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -24,21 +24,21 @@
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/navigation/PreintegrationBase.h>
-#include <gtsam/navigation/ImuFactorBase.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
 
-/**
+/*
- *
- * @addtogroup SLAM
- *
  * If you are using the factor, please cite:
- * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
- * independent sets in factor graphs: a unifying perspective based on smart factors,
+ * conditionally independent sets in factor graphs: a unifying perspective based
- * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
  *
- ** REFERENCES:
+ * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
+ * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
+ * Robotics: Science and Systems (RSS), 2015.
+ *
+ * REFERENCES:
  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
  *     Volume 2, 2008.
  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
@@ -46,86 +46,83 @@ namespace gtsam {
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
+ *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
+ *     Robotics: Science and Systems (RSS), 2015.
  */
 
 /**
- * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
+ * PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
- * the vehicle at previous time step), current state (pose and velocity at
- * current time step), and the bias estimate. Following the preintegration
- * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
- * are "summarized" using the PreintegratedMeasurements 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.
- */
-class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
-    imuBias::ConstantBias>, public ImuFactorBase {
-public:
-
-  /**
-   * PreintegratedMeasurements accumulates (integrates) the IMU measurements
  * (rotation rates and accelerations) and the corresponding covariance matrix.
  * The measurements are then used to build the Preintegrated IMU factor.
  * Integration is done incrementally (ideally, one integrates the measurement
  * as soon as it is received from the IMU) so as to avoid costly integration
  * at time of factor construction.
+ *
+ * @addtogroup SLAM
  */
-  class PreintegratedMeasurements: public PreintegrationBase {
+class PreintegratedImuMeasurements: public PreintegrationBase {
 
   friend class ImuFactor;
 
-  protected:
+protected:
 
-    Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
+  Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
   ///< (first-order propagation from *measurementCovariance*).
 
-  public:
+  /// Default constructor for serialization
+  PreintegratedImuMeasurements() {}
+
+public:
 
  /**
-     *  Default constructor, initializes the class with no measurements
+   *  Constructor, initializes the class with no measurements
    *  @param bias Current estimate of acceleration and rotation rate biases
-     *  @param measuredAccCovariance      Covariance matrix of measuredAcc
+   *  @param p    Parameters, typically fixed in a single application
-     *  @param measuredOmegaCovariance    Covariance matrix of measured Angular Rate
-     *  @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
-     *  @param use2ndOrderIntegration     Controls the order of integration
-     *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
    */
-    PreintegratedMeasurements(const imuBias::ConstantBias& bias,
+  PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
-        const Matrix3& measuredAccCovariance,
+      const imuBias::ConstantBias& biasHat) :
-        const Matrix3& measuredOmegaCovariance,
+      PreintegrationBase(p, biasHat) {
-        const Matrix3& integrationErrorCovariance,
+    preintMeasCov_.setZero();
-        const bool use2ndOrderIntegration = false);
+  }
 
   /// print
   void print(const std::string& s = "Preintegrated Measurements:") const;
 
   /// equals
-    bool equals(const PreintegratedMeasurements& expected,
+  bool equals(const PreintegratedImuMeasurements& expected,
       double tol = 1e-9) const;
 
-    /// Re-initialize PreintegratedMeasurements
+  /// Re-initialize PreintegratedIMUMeasurements
   void resetIntegration();
 
   /**
    * Add a single IMU measurement to the preintegration.
    * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
-     * @param deltaT Time interval between this and the last IMU measurement
+   * @param dt Time interval between this and the last IMU measurement
-     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
-     * @param Fout, Gout Jacobians used internally (only needed for testing)
    */
   void integrateMeasurement(const Vector3& measuredAcc,
-        const Vector3& measuredOmega, double deltaT,
+      const Vector3& measuredOmega, double dt);
-        boost::optional<const Pose3&> body_P_sensor = boost::none,
-        OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout =
-            boost::none);
 
-    /// methods to access class variables
+  /// Return pre-integrated measurement covariance
-    Matrix preintMeasCov() const {
+  Matrix preintMeasCov() const { return preintMeasCov_; }
-      return preintMeasCov_;
-    }
 
-  private:
+  /// @deprecated constructor
+  /// NOTE(frank): assumes Z-Down convention, only second order integration supported
+  PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
+      const Matrix3& measuredAccCovariance,
+      const Matrix3& measuredOmegaCovariance,
+      const Matrix3& integrationErrorCovariance,
+      bool use2ndOrderIntegration = true);
+
+  /// @deprecated version of integrateMeasurement with body_P_sensor
+  /// Use parameters instead
+  void integrateMeasurement(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, double dt,
+      boost::optional<Pose3> body_P_sensor);
+
+private:
 
   /// Serialization function
   friend class boost::serialization::access;
@@ -134,7 +131,23 @@ public:
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
     ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
   }
-  };
+};
+
+/**
+ * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
+ * the vehicle at previous time step), current state (pose and velocity at
+ * current time step), and the bias estimate. Following the preintegration
+ * scheme proposed in [2], the ImuFactor 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.
+ *
+ * @addtogroup SLAM
+ */
+class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
+    imuBias::ConstantBias> {
+public:
 
 private:
 
@@ -142,7 +155,7 @@ private:
   typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
       imuBias::ConstantBias> Base;
 
-  PreintegratedMeasurements _PIM_;
+  PreintegratedImuMeasurements _PIM_;
 
 public:
 
@@ -163,17 +176,9 @@ public:
    * @param pose_j Current pose key
    * @param vel_j  Current velocity key
    * @param bias   Previous bias key
-   * @param preintegratedMeasurements Preintegrated IMU measurements
-   * @param gravity Gravity vector expressed in the global frame
-   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
-   * @param body_P_sensor Optional pose of the sensor frame in the body frame
-   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
    */
   ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-      const PreintegratedMeasurements& preintegratedMeasurements,
+      const PreintegratedImuMeasurements& preintegratedMeasurements);
-      const Vector3& gravity, const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none,
-      const bool use2ndOrderCoriolis = false);
 
   virtual ~ImuFactor() {
   }
@@ -192,7 +197,7 @@ public:
 
   /** Access the preintegrated measurements. */
 
-  const PreintegratedMeasurements& preintegratedMeasurements() const {
+  const PreintegratedImuMeasurements& preintegratedMeasurements() const {
     return _PIM_;
   }
 
@@ -206,20 +211,22 @@ public:
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
           boost::none, boost::optional<Matrix&> H5 = boost::none) const;
 
-  /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
+  /// @deprecated typename
+  typedef PreintegratedImuMeasurements PreintegratedMeasurements;
+
+  /// @deprecated constructor, in the new one gravity, coriolis settings are in Params
+  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
+      const PreintegratedMeasurements& preintegratedMeasurements,
+      const Vector3& n_gravity, const Vector3& omegaCoriolis,
+      const boost::optional<Pose3>& body_P_sensor = boost::none,
+      const bool use2ndOrderCoriolis = false);
+
+  /// @deprecated use PreintegrationBase::predict,
+  /// in the new one gravity, coriolis settings are in Params
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-      const PreintegratedMeasurements& PIM, const Vector3& gravity,
+      PreintegratedMeasurements& pim, const Vector3& n_gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
-    PoseVelocityBias PVB(
-        PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-            use2ndOrderCoriolis, deltaPij_biascorrected_out,
-            deltaVij_biascorrected_out));
-    pose_j = PVB.pose;
-    vel_j = PVB.velocity;
-  }
 
 private:
 
@@ -230,13 +237,8 @@ private:
     ar & boost::serialization::make_nvp("NoiseModelFactor5",
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
-    ar & BOOST_SERIALIZATION_NVP(gravity_);
-    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
-    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
 };
 // class ImuFactor
 
-typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
-
 } /// namespace gtsam

gtsam/navigation/ImuFactorBase.h

@@ -1,94 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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  PreintegrationBase.h
- *  @author Luca Carlone
- *  @author Stephen Williams
- *  @author Richard Roberts
- *  @author Vadim Indelman
- *  @author David Jensen
- *  @author Frank Dellaert
- **/
-
-#pragma once
-
-/* GTSAM includes */
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/navigation/PreintegrationBase.h>
-
-namespace gtsam {
-
-class ImuFactorBase {
-
-protected:
-
-  Vector3 gravity_;
-  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
-  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
-
-public:
-
-  /** Default constructor - only use for serialization */
-  ImuFactorBase() :
-      gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
-          boost::none), use2ndOrderCoriolis_(false) {
-  }
-
-  /**
-   *  Default constructor, stores basic quantities required by the Imu factors
-   * @param gravity Gravity vector expressed in the global frame
-   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
-   * @param body_P_sensor Optional pose of the sensor frame in the body frame
-   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
-   */
-  ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
-      boost::optional<const Pose3&> body_P_sensor = boost::none,
-      const bool use2ndOrderCoriolis = false) :
-      gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
-          body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
-  }
-
-  /// Methods to access class variables
-  const Vector3& gravity() const {
-    return gravity_;
-  }
-  const Vector3& omegaCoriolis() const {
-    return omegaCoriolis_;
-  }
-
-  /// Needed for testable
-  //------------------------------------------------------------------------------
-  void print(const std::string& /*s*/) const {
-    std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-    std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
-        << std::endl;
-    std::cout << "  use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]"
-        << std::endl;
-    if (this->body_P_sensor_)
-      this->body_P_sensor_->print("  sensor pose in body frame: ");
-  }
-
-  /// Needed for testable
-  //------------------------------------------------------------------------------
-  bool equals(const ImuFactorBase& expected, double tol) const {
-    return equal_with_abs_tol(gravity_, expected.gravity_, tol)
-        && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
-        && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
-        && ((!body_P_sensor_ && !expected.body_P_sensor_)
-            || (body_P_sensor_ && expected.body_P_sensor_
-                && body_P_sensor_->equals(*expected.body_P_sensor_)));
-  }
-
-};
-
-} /// namespace gtsam

gtsam/navigation/NavState.cpp

@@ -0,0 +1,358 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    NavState.h
+ * @brief   Navigation state composing of attitude, position, and velocity
+ * @author  Frank Dellaert
+ * @date    July 2015
+ **/
+
+#include <gtsam/navigation/NavState.h>
+
+using namespace std;
+
+namespace gtsam {
+
+#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
+
+//------------------------------------------------------------------------------
+NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
+    OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
+  if (H1)
+    *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
+  if (H2)
+    *H2 << Z_3x3, Z_3x3, pose.rotation().transpose();
+  return NavState(pose, vel);
+}
+
+//------------------------------------------------------------------------------
+const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
+  if (H)
+    *H << I_3x3, Z_3x3, Z_3x3;
+  return R_;
+}
+
+//------------------------------------------------------------------------------
+const Point3& NavState::position(OptionalJacobian<3, 9> H) const {
+  if (H)
+    *H << Z_3x3, R(), Z_3x3;
+  return t_;
+}
+
+//------------------------------------------------------------------------------
+const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const {
+  if (H)
+    *H << Z_3x3, Z_3x3, R();
+  return v_;
+}
+
+//------------------------------------------------------------------------------
+Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
+  const Rot3& nRb = R_;
+  const Vector3& n_v = v_;
+  Matrix3 D_bv_nRb;
+  Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0);
+  if (H)
+    *H << D_bv_nRb, Z_3x3, I_3x3;
+  return b_v;
+}
+
+//------------------------------------------------------------------------------
+Matrix7 NavState::matrix() const {
+  Matrix3 R = this->R();
+  Matrix7 T;
+  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
+  return T;
+}
+
+//------------------------------------------------------------------------------
+void NavState::print(const string& s) const {
+  attitude().print(s + ".R");
+  position().print(s + ".p");
+  gtsam::print((Vector) v_, s + ".v");
+}
+
+//------------------------------------------------------------------------------
+bool NavState::equals(const NavState& other, double tol) const {
+  return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
+      && equal_with_abs_tol(v_, other.v_, tol);
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::inverse() const {
+  TIE(nRb, n_t, n_v, *this);
+  const Rot3 bRn = nRb.inverse();
+  return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::operator*(const NavState& bTc) const {
+  TIE(nRb, n_t, n_v, *this);
+  TIE(bRc, b_t, b_v, bTc);
+  return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
+}
+
+//------------------------------------------------------------------------------
+NavState::PositionAndVelocity //
+NavState::operator*(const PositionAndVelocity& b_tv) const {
+  TIE(nRb, n_t, n_v, *this);
+  const Point3& b_t = b_tv.first;
+  const Velocity3& b_v = b_tv.second;
+  return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
+}
+
+//------------------------------------------------------------------------------
+Point3 NavState::operator*(const Point3& b_t) const {
+  return Point3(R_ * b_t + t_);
+}
+
+//------------------------------------------------------------------------------
+Velocity3 NavState::operator*(const Velocity3& b_v) const {
+  return Velocity3(R_ * b_v + v_);
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
+    OptionalJacobian<9, 9> H) {
+  Matrix3 D_R_xi;
+  const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0);
+  const Point3 p = Point3(dP(xi));
+  const Vector v = dV(xi);
+  const NavState result(R, p, v);
+  if (H) {
+    *H << D_R_xi, Z_3x3, Z_3x3, //
+    Z_3x3, R.transpose(), Z_3x3, //
+    Z_3x3, Z_3x3, R.transpose();
+  }
+  return result;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
+    OptionalJacobian<9, 9> H) {
+  Vector9 xi;
+  Matrix3 D_xi_R;
+  xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v();
+  if (H) {
+    *H << D_xi_R, Z_3x3, Z_3x3, //
+    Z_3x3, x.R(), Z_3x3, //
+    Z_3x3, Z_3x3, x.R();
+  }
+  return xi;
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
+  if (H)
+    throw runtime_error("NavState::Expmap derivative not implemented yet");
+
+  Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
+  Eigen::Block<const Vector9, 3, 1> v = dP(xi);
+  Eigen::Block<const Vector9, 3, 1> a = dV(xi);
+
+  // NOTE(frank): See Pose3::Expmap
+  Rot3 nRb = Rot3::Expmap(n_omega_nb);
+  double theta2 = n_omega_nb.dot(n_omega_nb);
+  if (theta2 > numeric_limits<double>::epsilon()) {
+    // Expmap implements a "screw" motion in the direction of n_omega_nb
+    Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
+    Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
+    Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
+        / theta2;
+    Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
+    Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
+    Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
+    return NavState(nRb, n_t, n_v);
+  } else {
+    return NavState(nRb, Point3(v), a);
+  }
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
+  if (H)
+    throw runtime_error("NavState::Logmap derivative not implemented yet");
+
+  TIE(nRb, n_p, n_v, nTb);
+  Vector3 n_t = n_p.vector();
+
+  // NOTE(frank): See Pose3::Logmap
+  Vector9 xi;
+  Vector3 n_omega_nb = Rot3::Logmap(nRb);
+  double theta = n_omega_nb.norm();
+  if (theta * theta <= numeric_limits<double>::epsilon()) {
+    xi << n_omega_nb, n_t, n_v;
+  } else {
+    Matrix3 W = skewSymmetric(n_omega_nb / theta);
+    // Formula from Agrawal06iros, equation (14)
+    // simplified with Mathematica, and multiplying in n_t to avoid matrix math
+    double factor = (1 - theta / (2. * tan(0.5 * theta)));
+    Vector3 n_x = W * n_t;
+    Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
+    Vector3 n_y = W * n_v;
+    Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
+    xi << n_omega_nb, v, a;
+  }
+  return xi;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::AdjointMap() const {
+  // NOTE(frank): See Pose3::AdjointMap
+  const Matrix3 nRb = R();
+  Matrix3 pAr = skewSymmetric(t()) * nRb;
+  Matrix3 vAr = skewSymmetric(v()) * nRb;
+  Matrix9 adj;
+  //     nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
+  adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Matrix7 NavState::wedge(const Vector9& xi) {
+  const Matrix3 Omega = skewSymmetric(dR(xi));
+  Matrix7 T;
+  T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
+  return T;
+}
+
+//------------------------------------------------------------------------------
+// sugar for derivative blocks
+#define D_R_R(H) (H)->block<3,3>(0,0)
+#define D_R_t(H) (H)->block<3,3>(0,3)
+#define D_R_v(H) (H)->block<3,3>(0,6)
+#define D_t_R(H) (H)->block<3,3>(3,0)
+#define D_t_t(H) (H)->block<3,3>(3,3)
+#define D_t_v(H) (H)->block<3,3>(3,6)
+#define D_v_R(H) (H)->block<3,3>(6,0)
+#define D_v_t(H) (H)->block<3,3>(6,3)
+#define D_v_v(H) (H)->block<3,3>(6,6)
+
+//------------------------------------------------------------------------------
+NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
+    const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
+    OptionalJacobian<9, 3> G2) const {
+
+  Vector9 xi;
+  Matrix39 D_xiP_state;
+  Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0);
+  double dt22 = 0.5 * dt * dt;
+
+  // Integrate on tangent space. TODO(frank): coriolis?
+  dR(xi) << dt * b_omega;
+  dP(xi) << dt * b_v + dt22 * b_acceleration;
+  dV(xi) << dt * b_acceleration;
+
+  // Bring back to manifold
+  Matrix9 D_newState_xi;
+  NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0);
+
+  // Derivative wrt state is computed by retract directly
+  // However, as dP(xi) also depends on state, we need to add that contribution
+  if (F) {
+    F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
+  }
+  // derivative wrt acceleration
+  if (G1) {
+    // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
+    // D_dPxi_acc = dt22 * I_3x3
+    // D_newState_dVxi = D_newState_xi.rightCols<3>()
+    // D_dVxi_acc = dt * I_3x3
+    // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
+    *G1 = D_newState_xi.middleCols<3>(3) * dt22
+        + D_newState_xi.rightCols<3>() * dt;
+  }
+  // derivative wrt omega
+  if (G2) {
+    // D_newState_dRxi = D_newState_xi.leftCols<3>()
+    // D_dRxi_omega = dt * I_3x3
+    // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
+    *G2 = D_newState_xi.leftCols<3>() * dt;
+  }
+  return newState;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
+    OptionalJacobian<9, 9> H) const {
+  TIE(nRb, n_t, n_v, *this);
+  const double dt2 = dt * dt;
+  const Vector3 omega_cross_vel = omega.cross(n_v);
+
+  Vector9 xi;
+  Matrix3 D_dP_R;
+  dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
+  dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
+  dV(xi) << ((-2.0 * dt) * omega_cross_vel);
+  if (secondOrder) {
+    const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector()));
+    dP(xi) -= (0.5 * dt2) * omega_cross2_t;
+    dV(xi) -= dt * omega_cross2_t;
+  }
+  if (H) {
+    H->setZero();
+    const Matrix3 Omega = skewSymmetric(omega);
+    const Matrix3 D_cross_state = Omega * R();
+    H->setZero();
+    D_R_R(H) << D_dP_R;
+    D_t_v(H) << (-dt2) * D_cross_state;
+    D_v_v(H) << (-2.0 * dt) * D_cross_state;
+    if (secondOrder) {
+      const Matrix3 D_cross2_state = Omega * D_cross_state;
+      D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
+      D_v_t(H) -= dt * D_cross2_state;
+    }
+  }
+  return xi;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::correctPIM(const Vector9& pim, double dt,
+    const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
+    bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 9> H2) const {
+  const Rot3& nRb = R_;
+  const Velocity3& n_v = v_; // derivative is Ri !
+  const double dt22 = 0.5 * dt * dt;
+
+  Vector9 xi;
+  Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
+  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);
+
+  if (omegaCoriolis) {
+    xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
+  }
+
+  if (H1 || H2) {
+    Matrix3 Ri = nRb.matrix();
+
+    if (H1) {
+      if (!omegaCoriolis)
+        H1->setZero(); // if coriolis H1 is already initialized
+      D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
+      D_t_v(H1) += dt * D_dP_nv * Ri;
+      D_v_R(H1) += dt * D_dV_Ri;
+    }
+    if (H2) {
+      H2->setIdentity();
+    }
+  }
+
+  return xi;
+}
+//------------------------------------------------------------------------------
+
+}/// namespace gtsam

gtsam/navigation/NavState.h

@@ -0,0 +1,257 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    NavState.h
+ * @brief   Navigation state composing of attitude, position, and velocity
+ * @author  Frank Dellaert
+ * @date    July 2015
+ **/
+
+#pragma once
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/ProductLieGroup.h>
+
+namespace gtsam {
+
+/// Velocity is currently typedef'd to Vector3
+typedef Vector3 Velocity3;
+
+/**
+ * Navigation state: Pose (rotation, translation) + velocity
+ * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity
+ */
+class NavState: public LieGroup<NavState, 9> {
+private:
+
+  // TODO(frank):
+  // - should we rename t_ to p_? if not, we should rename dP do dT
+  Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
+  Point3 t_; ///< position n_t, in nav frame
+  Velocity3 v_; ///< velocity n_v in nav frame
+
+public:
+
+  typedef std::pair<Point3, Velocity3> PositionAndVelocity;
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  NavState() :
+      v_(Vector3::Zero()) {
+  }
+  /// Construct from attitude, position, velocity
+  NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
+      R_(R), t_(t), v_(v) {
+  }
+  /// Construct from pose and velocity
+  NavState(const Pose3& pose, const Velocity3& v) :
+      R_(pose.rotation()), t_(pose.translation()), v_(v) {
+  }
+  /// Construct from Matrix group representation (no checking)
+  NavState(const Matrix7& T) :
+      R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
+  }
+  /// Construct from SO(3) and R^6
+  NavState(const Matrix3& R, const Vector9 tv) :
+      R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
+  }
+  /// Named constructor with derivatives
+  static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
+      OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
+
+  /// @}
+  /// @name Component Access
+  /// @{
+
+  inline const Rot3& attitude() const {
+    return R_;
+  }
+  inline const Point3& position() const {
+    return t_;
+  }
+  inline const Velocity3& velocity() const {
+    return v_;
+  }
+  const Rot3& attitude(OptionalJacobian<3, 9> H) const;
+  const Point3& position(OptionalJacobian<3, 9> H) const;
+  const Velocity3& velocity(OptionalJacobian<3, 9> H) const;
+
+  const Pose3 pose() const {
+    return Pose3(attitude(), position());
+  }
+
+  /// @}
+  /// @name Derived quantities
+  /// @{
+
+  /// Return rotation matrix. Induces computation in quaternion mode
+  Matrix3 R() const {
+    return R_.matrix();
+  }
+  /// Return quaternion. Induces computation in matrix mode
+  Quaternion quaternion() const {
+    return R_.toQuaternion();
+  }
+  /// Return position as Vector3
+  Vector3 t() const {
+    return t_.vector();
+  }
+  /// Return velocity as Vector3. Computation-free.
+  const Vector3& v() const {
+    return v_;
+  }
+  // Return velocity in body frame
+  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
+
+  /// Return matrix group representation, in MATLAB notation:
+  /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
+  /// With this embedding in GL(3), matrix product agrees with compose
+  Matrix7 matrix() const;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print
+  void print(const std::string& s = "") const;
+
+  /// equals
+  bool equals(const NavState& other, double tol = 1e-8) const;
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// identity for group operation
+  static NavState identity() {
+    return NavState();
+  }
+
+  /// inverse transformation with derivatives
+  NavState inverse() const;
+
+  /// Group compose is the semi-direct product as specified below:
+  /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
+  NavState operator*(const NavState& bTc) const;
+
+  /// Native group action is on position/velocity pairs *in the body frame* as follows:
+  /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
+  PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
+
+  /// Act on position alone, n_t = nRb * b_t + n_t
+  Point3 operator*(const Point3& b_t) const;
+
+  /// Act on velocity alone, n_v = nRb * b_v + n_v
+  Velocity3 operator*(const Velocity3& b_v) const;
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  // Tangent space sugar.
+  // TODO(frank): move to private navstate namespace in cpp
+  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);
+  }
+  static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
+    return v.segment<3>(0);
+  }
+  static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
+    return v.segment<3>(3);
+  }
+  static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
+    return v.segment<3>(6);
+  }
+
+  // Chart at origin, constructs components separately (as opposed to Expmap)
+  struct ChartAtOrigin {
+    static NavState Retract(const Vector9& xi, //
+        OptionalJacobian<9, 9> H = boost::none);
+    static Vector9 Local(const NavState& x, //
+        OptionalJacobian<9, 9> H = boost::none);
+  };
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  /// Exponential map at identity - create a NavState from canonical coordinates
+  static NavState Expmap(const Vector9& xi, //
+      OptionalJacobian<9, 9> H = boost::none);
+
+  /// Log map at identity - return the canonical coordinates for this NavState
+  static Vector9 Logmap(const NavState& p, //
+      OptionalJacobian<9, 9> H = boost::none);
+
+  /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
+  /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
+  Matrix9 AdjointMap() const;
+
+  /// wedge creates Lie algebra element from tangent vector
+  static Matrix7 wedge(const Vector9& xi);
+
+  /// @}
+  /// @name Dynamics
+  /// @{
+
+  /// Integrate forward in time given angular velocity and acceleration in body frame
+  /// Uses second order integration for position, returns derivatives except dt.
+  NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
+      const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
+      OptionalJacobian<9, 3> G2) const;
+
+  /// Compute tangent space contribution due to Coriolis forces
+  Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
+      OptionalJacobian<9, 9> H = boost::none) const;
+
+  /// Correct preintegrated tangent vector with our velocity and rotated gravity,
+  /// taking into account Coriolis forces if omegaCoriolis is given.
+  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
+      const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
+          false, OptionalJacobian<9, 9> H1 = boost::none,
+      OptionalJacobian<9, 9> H2 = boost::none) const;
+
+private:
+  /// @{
+  /// serialization
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(R_);
+    ar & BOOST_SERIALIZATION_NVP(t_);
+    ar & BOOST_SERIALIZATION_NVP(v_);
+  }
+  /// @}
+};
+
+// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
+template<>
+struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> {
+};
+
+// Partial specialization of wedge
+// TODO: deprecate, make part of traits
+template<>
+inline Matrix wedge<NavState>(const Vector& xi) {
+  return NavState::wedge(xi);
+}
+
+} // namespace gtsam

gtsam/navigation/PreintegratedRotation.cpp

@@ -0,0 +1,111 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  PreintegratedRotation.cpp
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include "PreintegratedRotation.h"
+
+using namespace std;
+
+namespace gtsam {
+
+void PreintegratedRotation::Params::print(const string& s) const {
+  cout << s << endl;
+  cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
+  if (omegaCoriolis)
+    cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
+        << endl;
+  if (body_P_sensor)
+    body_P_sensor->print("body_P_sensor");
+}
+
+void PreintegratedRotation::resetIntegration() {
+  deltaTij_ = 0.0;
+  deltaRij_ = Rot3();
+  delRdelBiasOmega_ = Z_3x3;
+}
+
+void PreintegratedRotation::print(const string& s) const {
+  cout << s << endl;
+  cout << "    deltaTij [" << deltaTij_ << "]" << endl;
+  cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
+}
+
+bool PreintegratedRotation::equals(const PreintegratedRotation& other,
+    double tol) const {
+  return deltaRij_.equals(other.deltaRij_, tol)
+      && fabs(deltaTij_ - other.deltaTij_) < tol
+      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
+}
+
+Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
+    const Vector3& biasHat, double deltaT,
+    OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
+
+  // First we compensate the measurements for the bias
+  Vector3 correctedOmega = measuredOmega - biasHat;
+
+  // Then compensate for sensor-body displacement: we express the quantities
+  // (originally in the IMU frame) into the body frame
+  if (p_->body_P_sensor) {
+    Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
+    // rotation rate vector in the body frame
+    correctedOmega = body_R_sensor * correctedOmega;
+  }
+
+  // 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 !!
+}
+
+void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
+    const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
+    Matrix3* F) {
+
+  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
+      D_incrR_integratedOmega);
+
+  // Update deltaTij and rotation
+  deltaTij_ += deltaT;
+  deltaRij_ = deltaRij_.compose(incrR, F);
+
+  // Update Jacobian
+  const Matrix3 incrRt = incrR.transpose();
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+      - *D_incrR_integratedOmega * deltaT;
+}
+
+Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
+    OptionalJacobian<3, 3> H) const {
+  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
+  const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,
+      boost::none, H);
+  if (H)
+    (*H) *= delRdelBiasOmega_;
+  return deltaRij_biascorrected;
+}
+
+Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const {
+  if (!p_->omegaCoriolis)
+    return Vector3::Zero();
+  return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
+}
+
+} // namespace gtsam

gtsam/navigation/PreintegratedRotation.h

@@ -21,7 +21,8 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/Matrix.h>
 
 namespace gtsam {
 
@@ -31,124 +32,104 @@ namespace gtsam {
  * It includes the definitions of the preintegrated rotation.
  */
 class PreintegratedRotation {
-
-  Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
-  double deltaTij_; ///< Time interval from i to j
-
-  /// Jacobian of preintegrated rotation w.r.t. angular rate bias
-  Matrix3 delRdelBiasOmega_;
-  Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
-
 public:
 
-  /**
+  /// Parameters for pre-integration:
-   *  Default constructor, initializes the variables in the base class
+  /// Usage: Create just a single Params and pass a shared pointer to the constructor
-   */
+  struct Params {
-  PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
+    Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
-      deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
+    boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
-          measuredOmegaCovariance) {
+    boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
+
+    Params() :
+        gyroscopeCovariance(I_3x3) {
     }
 
-  /// methods to access class variables
+    virtual void print(const std::string& s) const;
-  Matrix3 deltaRij() const {
+
-    return deltaRij_.matrix();
+  private:
-  } // expensive
+    /** Serialization function */
-  Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
+    friend class boost::serialization::access;
-    return Rot3::Logmap(deltaRij_, H);
+    template<class ARCHIVE>
-  } // super-expensive
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
+      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
+      ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
+    }
+  };
+
+protected:
+  double deltaTij_; ///< Time interval from i to j
+  Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
+  Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+
+  /// Parameters
+  boost::shared_ptr<Params> p_;
+
+  /// Default constructor for serialization
+  PreintegratedRotation() {
+  }
+
+public:
+  /// Default constructor, resets integration to zero
+  explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
+      p_(p) {
+    resetIntegration();
+  }
+
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration();
+
+  /// @name Access instance variables
+  /// @{
   const double& deltaTij() const {
     return deltaTij_;
   }
+  const Rot3& deltaRij() const {
+    return deltaRij_;
+  }
   const Matrix3& delRdelBiasOmega() const {
     return delRdelBiasOmega_;
   }
-  const Matrix3& gyroscopeCovariance() const {
+  /// @}
-    return gyroscopeCovariance_;
-  }
 
-  /// Needed for testable
+  /// @name Testable
-  void print(const std::string& s) const {
+  /// @{
-    std::cout << s << std::endl;
-    std::cout << "    deltaTij [" << deltaTij_ << "]" << std::endl;
-    std::cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
-  }
 
-  /// Needed for testable
+  void print(const std::string& s) const;
-  bool equals(const PreintegratedRotation& expected, double tol) const {
+  bool equals(const PreintegratedRotation& other, double tol) const;
-    return deltaRij_.equals(expected.deltaRij_, tol)
-        && fabs(deltaTij_ - expected.deltaTij_) < tol
-        && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
-            tol)
-        && equal_with_abs_tol(gyroscopeCovariance_,
-            expected.gyroscopeCovariance_, tol);
-  }
 
-  /// Re-initialize PreintegratedMeasurements
+  /// @}
-  void resetIntegration() {
-    deltaRij_ = Rot3();
-    deltaTij_ = 0.0;
-    delRdelBiasOmega_ = Z_3x3;
-  }
 
-  /// Update preintegrated measurements
+  /// Take the gyro measurement, correct it using the (constant) bias estimate
-  void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
+  /// and possibly the sensor pose, and then integrate it forward in time to yield
-      OptionalJacobian<3, 3> H = boost::none) {
+  /// an incremental rotation.
-    deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
+  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
-    deltaTij_ += deltaT;
+      double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
-  }
 
-  /**
+  /// Calculate an incremental rotation given the gyro measurement and a time interval,
-   *  Update Jacobians to be used during preintegration
+  /// and update both deltaTij_ and deltaRij_.
-   *  TODO: explain arguments
+  void integrateMeasurement(const Vector3& measuredOmega,
-   */
+      const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
-  void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
+      Matrix3* F);
-      const Rot3& incrR, double deltaT) {
-    const Matrix3 incrRt = incrR.transpose();
-    delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-        - D_Rincr_integratedOmega * deltaT;
-  }
 
-  /// Return a bias corrected version of the integrated rotation - expensive
+  /// Return a bias corrected version of the integrated rotation, with optional Jacobian
-  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
+  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
-    return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
+      OptionalJacobian<3, 3> H = boost::none) const;
-  }
-
-  /// Get so<3> version of bias corrected rotation, with optional Jacobian
-  // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
-  Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
-      OptionalJacobian<3, 3> H = boost::none) const {
-    // First, we correct deltaRij using the biasOmegaIncr, rotated
-    const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
-
-    if (H) {
-      Matrix3 Jrinv_theta_bc;
-      // This was done via an expmap, now we go *back* to so<3>
-      const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
-          Jrinv_theta_bc);
-      const Matrix3 Jr_JbiasOmegaIncr = //
-          Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
-      (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
-      return biascorrectedOmega;
-    }
-    //else
-    return Rot3::Logmap(deltaRij_biascorrected);
-  }
 
   /// Integrate coriolis correction in body frame rot_i
-  Vector3 integrateCoriolis(const Rot3& rot_i,
+  Vector3 integrateCoriolis(const Rot3& rot_i) const;
-      const Vector3& omegaCoriolis) const {
-    return rot_i.transpose() * omegaCoriolis * deltaTij();
-  }
 
 private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
-    ar & BOOST_SERIALIZATION_NVP(deltaRij_);
+    ar & BOOST_SERIALIZATION_NVP(p_);
     ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaRij_);
     ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
   }
 };
 
-} /// namespace gtsam
+} // namespace gtsam

gtsam/navigation/PreintegrationBase.cpp

@@ -20,338 +20,293 @@
  **/
 
 #include "PreintegrationBase.h"
+#include <boost/make_shared.hpp>
+
+using namespace std;
 
 namespace gtsam {
 
-PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
+//------------------------------------------------------------------------------
-                                       const Matrix3& measuredAccCovariance,
+void PreintegrationBase::Params::print(const string& s) const {
-                                       const Matrix3& measuredOmegaCovariance,
+  PreintegratedRotation::Params::print(s);
-                                       const Matrix3&integrationErrorCovariance,
+  cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
-                                       const bool use2ndOrderIntegration)
+      << endl;
-    : PreintegratedRotation(measuredOmegaCovariance),
+  cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
-      biasHat_(bias),
+      << endl;
-      use2ndOrderIntegration_(use2ndOrderIntegration),
+  if (omegaCoriolis && use2ndOrderCoriolis)
-      deltaPij_(Vector3::Zero()),
+    cout << "Using 2nd-order Coriolis" << endl;
-      deltaVij_(Vector3::Zero()),
+  if (body_P_sensor)
-      delPdelBiasAcc_(Z_3x3),
+    body_P_sensor->print("    ");
-      delPdelBiasOmega_(Z_3x3),
+  cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
-      delVdelBiasAcc_(Z_3x3),
-      delVdelBiasOmega_(Z_3x3),
-      accelerometerCovariance_(measuredAccCovariance),
-      integrationCovariance_(integrationErrorCovariance) {
 }
 
-/// Needed for testable
+//------------------------------------------------------------------------------
-void PreintegrationBase::print(const std::string& s) const {
-  PreintegratedRotation::print(s);
-  std::cout << "    deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
-  std::cout << "    deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
-  biasHat_.print("    biasHat");
-}
-
-/// Needed for testable
-bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const {
-  return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol)
-      && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
-      && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
-      && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
-      && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
-      && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
-      && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
-      && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol)
-      && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
-}
-
-/// Re-initialize PreintegratedMeasurements
 void PreintegrationBase::resetIntegration() {
-  PreintegratedRotation::resetIntegration();
+  deltaTij_ = 0.0;
-  deltaPij_ = Vector3::Zero();
+  deltaXij_ = NavState();
-  deltaVij_ = Vector3::Zero();
+  delRdelBiasOmega_ = Z_3x3;
   delPdelBiasAcc_ = Z_3x3;
   delPdelBiasOmega_ = Z_3x3;
   delVdelBiasAcc_ = Z_3x3;
   delVdelBiasOmega_ = Z_3x3;
 }
 
-/// Update preintegrated measurements
+//------------------------------------------------------------------------------
-void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
+void PreintegrationBase::print(const string& s) const {
-                                                         const Rot3& incrR, const double deltaT,
+  cout << s << endl;
-                                                         OptionalJacobian<9, 9> F) {
+  cout << "    deltaTij [" << deltaTij_ << "]" << endl;
-
+  cout << "    deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
-  const Matrix3 dRij = deltaRij();  // expensive
+  cout << "    deltaPij [ " << deltaPij().transpose() << " ]" << endl;
-  const Vector3 temp = dRij * correctedAcc * deltaT;
+  cout << "    deltaVij [ " << deltaVij().transpose() << " ]" << endl;
-  if (!use2ndOrderIntegration_) {
+  biasHat_.print("    biasHat");
-    deltaPij_ += deltaVij_ * deltaT;
-  } else {
-    deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
-  }
-  deltaVij_ += temp;
-
-  Matrix3 R_i, F_angles_angles;
-  if (F)
-    R_i = deltaRij();  // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
-  updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
-
-  if (F) {
-    const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
-    Matrix3 F_pos_angles;
-    if (use2ndOrderIntegration_)
-      F_pos_angles = 0.5 * F_vel_angles * deltaT;
-    else
-      F_pos_angles = Z_3x3;
-
-    //    pos  vel             angle
-    *F <<  //
-        I_3x3, I_3x3 * deltaT, F_pos_angles,    // pos
-        Z_3x3, I_3x3,          F_vel_angles,    // vel
-        Z_3x3, Z_3x3,          F_angles_angles; // angle
-  }
 }
 
-/// Update Jacobians to be used during preintegration
+//------------------------------------------------------------------------------
-void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc,
+bool PreintegrationBase::equals(const PreintegrationBase& other,
-                                                      const Matrix3& D_Rincr_integratedOmega,
+    double tol) const {
-                                                      const Rot3& incrR, double deltaT) {
+  return fabs(deltaTij_ - other.deltaTij_) < tol
-  const Matrix3 dRij = deltaRij();  // expensive
+      && deltaXij_.equals(other.deltaXij_, tol)
-  const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
+      && biasHat_.equals(other.biasHat_, tol)
-  if (!use2ndOrderIntegration_) {
+      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+      && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+      && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
-  } else {
+      && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
+      && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
-    delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
-  }
-  delVdelBiasAcc_ += -dRij * deltaT;
-  delVdelBiasOmega_ += temp;
-  update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
 }
 
-void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+//------------------------------------------------------------------------------
-    const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc,
+pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
+    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
-  correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+    OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
-  correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+    OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
+    OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
 
-  // Then compensate for sensor-body displacement: we express the quantities
+  // Correct for bias in the sensor frame
+  Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
+  Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
+
+  // Compensate for sensor-body displacement if needed: we express the quantities
   // (originally in the IMU frame) into the body frame
-  if (body_P_sensor) {
+  // Equations below assume the "body" frame is the CG
-    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+  if (p().body_P_sensor) {
-    correctedOmega = body_R_sensor * correctedOmega;  // rotation rate vector in the body frame
+    // Correct omega to rotation rate vector in the body frame
-    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+    const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
-    correctedAcc = body_R_sensor * correctedAcc
+    j_correctedOmega = bRs * j_correctedOmega;
-        - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+
-    // linear acceleration vector in the body frame
+    // Correct acceleration
+    j_correctedAcc = bRs * j_correctedAcc;
+
+    // Jacobians
+    if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
+    if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
+    if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
+
+    // Centrifugal acceleration
+    const Vector3 b_arm = p().body_P_sensor->translation().vector();
+    if (!b_arm.isZero()) {
+      // Subtract out the the centripetal acceleration from the measured one
+      // to get linear acceleration vector in the body frame:
+      const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
+      const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
+      j_correctedAcc -= body_Omega_body * b_velocity_bs;
+      // Update derivative: centrifugal causes the correlation between acc and omega!!!
+      if (D_correctedAcc_measuredOmega) {
+        double wdp = j_correctedOmega.dot(b_arm);
+        *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
+            + j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+            + 2 * b_arm * j_measuredOmega.transpose();
       }
+    }
+  }
+
+  // Do update in one fell swoop
+  return make_pair(j_correctedAcc, j_correctedOmega);
 }
 
-/// Predict state at time j
 //------------------------------------------------------------------------------
-PoseVelocityBias PreintegrationBase::predict(
+NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
-    const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+    const Vector3& j_measuredOmega, const double dt,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
+    OptionalJacobian<9, 9> D_updated_current,
-    boost::optional<Vector3&> deltaPij_biascorrected_out,
+    OptionalJacobian<9, 3> D_updated_measuredAcc,
-    boost::optional<Vector3&> deltaVij_biascorrected_out) const {
+    OptionalJacobian<9, 3> D_updated_measuredOmega) const {
 
-  const imuBias::ConstantBias biasIncr = bias_i - biasHat();
+  Vector3 j_correctedAcc, j_correctedOmega;
-  const Vector3& biasAccIncr = biasIncr.accelerometer();
+  Matrix3 D_correctedAcc_measuredAcc, //
-  const Vector3& biasOmegaIncr = biasIncr.gyroscope();
+      D_correctedAcc_measuredOmega, //
-
+      D_correctedOmega_measuredOmega;
-  const Rot3& Rot_i = pose_i.rotation();
+  bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
-  const Matrix3 Rot_i_matrix = Rot_i.matrix();
+  boost::tie(j_correctedAcc, j_correctedOmega) =
-  const Vector3 pos_i = pose_i.translation().vector();
+      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
-
+          (needDerivs ? &D_correctedAcc_measuredAcc : 0),
-  // Predict state at time j
+          (needDerivs ? &D_correctedAcc_measuredOmega : 0),
-  /* ---------------------------------------------------------------------------------------------------- */
+          (needDerivs ? &D_correctedOmega_measuredOmega : 0));
-  const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+  // Do update in one fell swoop
-      + delPdelBiasOmega() * biasOmegaIncr;
+  Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
-  if (deltaPij_biascorrected_out)  // if desired, store this
+  NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
-    *deltaPij_biascorrected_out = deltaPij_biascorrected;
+              (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
-
+              (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
-  const double dt = deltaTij(), dt2 = dt * dt;
+  if (needDerivs) {
-  Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt
+    *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
-      - omegaCoriolis.cross(vel_i) * dt2  // Coriolis term - we got rid of the 2 wrt ins paper
+    *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
-  + 0.5 * gravity * dt2;
+    if (!p().body_P_sensor->translation().vector().isZero()) {
-
+      *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
-  const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
-      + delVdelBiasOmega() * biasOmegaIncr;
-  if (deltaVij_biascorrected_out)  // if desired, store this
-    *deltaVij_biascorrected_out = deltaVij_biascorrected;
-
-  Vector3 vel_j = Vector3(
-      vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt  // Coriolis term
-      + gravity * dt);
-
-  if (use2ndOrderCoriolis) {
-    pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2;  // 2nd order coriolis term for position
-    vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt;  // 2nd order term for velocity
     }
-
+  }
-  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+  return updated;
-  // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-  const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
-  const Vector3 correctedOmega = biascorrectedOmega
-      - Rot_i_matrix.transpose() * omegaCoriolis * dt;  // Coriolis term
-  const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
-  const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
-
-  const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
-  return PoseVelocityBias(pose_j, vel_j, bias_i);  // bias is predicted as a constant
 }
 
-/// 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,
+void PreintegrationBase::update(const Vector3& j_measuredAcc,
-                                                     const Pose3& pose_j, const Vector3& vel_j,
+    const Vector3& j_measuredOmega, const double dt,
-                                                     const imuBias::ConstantBias& bias_i,
+    Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
-                                                     const Vector3& gravity,
+    Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
-                                                     const Vector3& omegaCoriolis,
-                                                     const bool use2ndOrderCoriolis,
-                                                     OptionalJacobian<9, 6> H1,
-                                                     OptionalJacobian<9, 3> H2,
-                                                     OptionalJacobian<9, 6> H3,
-                                                     OptionalJacobian<9, 3> H4,
-                                                     OptionalJacobian<9, 6> H5) const {
 
-  // We need the mismatch w.r.t. the biases used for preintegration
+  // Save current rotation for updating Jacobians
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+  const Rot3 oldRij = deltaXij_.attitude();
 
-  // we give some shorter name to rotations and translations
+  // Do update
-  const Rot3& Ri = pose_i.rotation();
+  deltaTij_ += dt;
-  const Matrix3 Ri_transpose_matrix = Ri.transpose();
+  deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
+      D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
 
-  const Rot3& Rj = pose_j.rotation();
+  // Update Jacobians
-  const Vector3 pos_j = pose_j.translation().vector();
+  // TODO(frank): we are repeating some computation here: accessible in F ?
+  Vector3 j_correctedAcc, j_correctedOmega;
+  boost::tie(j_correctedAcc, j_correctedOmega) =
+      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
 
-  // Evaluate residual error, according to [3]
+  Matrix3 D_acc_R;
-  /* ---------------------------------------------------------------------------------------------------- */
+  oldRij.rotate(j_correctedAcc, D_acc_R);
-  Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
+  const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
-  PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-                                              use2ndOrderCoriolis, deltaPij_biascorrected,
-                                              deltaVij_biascorrected);
 
-  // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+  const Vector3 integratedOmega = j_correctedOmega * dt;
-  const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector());
+  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
+  const Matrix3 incrRt = incrR.transpose();
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+      - *D_incrR_integratedOmega * dt;
 
-  // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+  double dt22 = 0.5 * dt * dt;
-  const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity);
+  const Matrix3 dRij = oldRij.matrix(); // expensive
-
+  delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
-  // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
+  delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
-
+  delVdelBiasAcc_ += -dRij * dt;
-  /* ---------------------------------------------------------------------------------------------------- */
+  delVdelBiasOmega_ += D_acc_biasOmega * dt;
-  // Get Get so<3> version of bias corrected rotation
-  // If H5 is asked for, we will need the Jacobian, which we store in H5
-  // H5 will then be corrected below to take into account the Coriolis effect
-  Matrix3 D_cThetaRij_biasOmegaIncr;
-  const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
-                                                           H5 ? &D_cThetaRij_biasOmegaIncr : 0);
-
-  // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
-  const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
-  const Vector3 correctedOmega = biascorrectedOmega - coriolis;
-
-  // Calculate Jacobians, matrices below needed only for some Jacobians...
-  Vector3 fR;
-  Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat;
-
-  if (H1 || H2)
-    Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis);
-
-  // This is done to save computation: we ask for the jacobians only when they are needed
-  const double dt = deltaTij(), dt2 = dt*dt;
-  Rot3 fRrot;
-  const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj;
-  if (H1 || H2 || H3 || H4 || H5) {
-    const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
-    // Residual rotation error
-    fRrot = correctedDeltaRij.between(RiBetweenRj);
-    fR = Rot3::Logmap(fRrot, D_fR_fRrot);
-    D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
-  } else {
-    const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
-    // Residual rotation error
-    fRrot = correctedDeltaRij.between(RiBetweenRj);
-    fR = Rot3::Logmap(fRrot);
-  }
-  if (H1) {
-    Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3;
-    if (use2ndOrderCoriolis) {
-      // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
-      const Matrix3 temp = Ritranspose_omegaCoriolisHat
-          * (-Ritranspose_omegaCoriolisHat.transpose());
-      dfPdPi += 0.5 * temp * dt2;
-      dfVdPi += temp * dt;
-    }
-    (*H1) <<
-    // dfP/dRi
-    skewSymmetric(fp + deltaPij_biascorrected),
-    // dfP/dPi
-    dfPdPi,
-    // dfV/dRi
-    skewSymmetric(fv + deltaVij_biascorrected),
-    // dfV/dPi
-    dfVdPi,
-    // dfR/dRi
-    D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
-    // dfR/dPi
-    Z_3x3;
-  }
-  if (H2) {
-    (*H2) <<
-    // dfP/dVi
-    -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2,  // Coriolis term - we got rid of the 2 wrt ins paper
-    // dfV/dVi
-    -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt,  // Coriolis term
-    // dfR/dVi
-    Z_3x3;
-  }
-  if (H3) {
-    (*H3) <<
-    // dfP/dPosej
-    Z_3x3, Ri_transpose_matrix * Rj.matrix(),
-    // dfV/dPosej
-    Matrix::Zero(3, 6),
-    // dfR/dPosej
-    D_fR_fRrot, Z_3x3;
-  }
-  if (H4) {
-    (*H4) <<
-    // dfP/dVj
-    Z_3x3,
-    // dfV/dVj
-    Ri_transpose_matrix,
-    // dfR/dVj
-    Z_3x3;
-  }
-  if (H5) {
-    // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
-    const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
-    (*H5) <<
-    // dfP/dBias
-    -delPdelBiasAcc(), -delPdelBiasOmega(),
-    // dfV/dBias
-    -delVdelBiasAcc(), -delVdelBiasOmega(),
-    // dfR/dBias
-    Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
-  }
-  Vector9 r;
-  r << fp, fv, fR;
-  return r;
 }
 
-ImuBase::ImuBase()
+//------------------------------------------------------------------------------
-    : gravity_(Vector3(0.0, 0.0, 9.81)),
+Vector9 PreintegrationBase::biasCorrectedDelta(
-      omegaCoriolis_(Vector3(0.0, 0.0, 0.0)),
+    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
-      body_P_sensor_(boost::none),
+  // Correct deltaRij, derivative is delRdelBiasOmega_
-      use2ndOrderCoriolis_(false) {
+  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
+  Matrix3 D_correctedRij_bias;
+  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
+  const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
+      H ? &D_correctedRij_bias : 0);
+  if (H)
+    D_correctedRij_bias *= delRdelBiasOmega_;
+
+  Vector9 xi;
+  Matrix3 D_dR_correctedRij;
+  // TODO(frank): could line below be simplified? It is equivalent to
+  //   LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
+  NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
+  NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
+      + delPdelBiasOmega_ * biasIncr.gyroscope();
+  NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
+      + delVdelBiasOmega_ * biasIncr.gyroscope();
+
+  if (H) {
+    Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
+    D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
+    D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
+    D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
+    (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
+  }
+  return xi;
 }
 
-ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
+//------------------------------------------------------------------------------
-                 boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis)
+NavState PreintegrationBase::predict(const NavState& state_i,
-    : gravity_(gravity),
+    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
-      omegaCoriolis_(omegaCoriolis),
+    OptionalJacobian<9, 6> H2) const {
-      body_P_sensor_(body_P_sensor),
+  // correct for bias
-      use2ndOrderCoriolis_(use2ndOrderCoriolis) {
+  Matrix96 D_biasCorrected_bias;
+  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
+      H2 ? &D_biasCorrected_bias : 0);
+
+  // integrate on tangent space
+  Matrix9 D_delta_state, D_delta_biasCorrected;
+  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
+      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
+      H2 ? &D_delta_biasCorrected : 0);
+
+  // Use retract to get back to NavState manifold
+  Matrix9 D_predict_state, D_predict_delta;
+  NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
+  if (H1)
+    *H1 = D_predict_state + D_predict_delta * D_delta_state;
+  if (H2)
+    *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
+  return state_j;
 }
 
-}  /// namespace gtsam
+//------------------------------------------------------------------------------
+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 {
+
+  // Note that derivative of constructors below is not identity for velocity, but
+  // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
+  NavState state_i(pose_i, vel_i);
+  NavState state_j(pose_j, vel_j);
+
+  /// Predict state at time j
+  Matrix99 D_predict_state_i;
+  Matrix96 D_predict_bias_i;
+  NavState predictedState_j = predict(state_i, bias_i,
+      H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0);
+
+  Matrix9 D_error_state_j, D_error_predict;
+  Vector9 error = state_j.localCoordinates(predictedState_j,
+      H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
+
+  // Separate out derivatives in terms of 5 arguments
+  // Note that doing so requires special treatment of velocities, as when treated as
+  // separate variables the retract applied will not be the semi-direct product in NavState
+  // Instead, the velocities in nav are updated using a straight addition
+  // This is difference is accounted for by the R().transpose calls below
+  if (H1)
+    *H1 << D_error_predict * D_predict_state_i.leftCols<6>();
+  if (H2)
+    *H2
+        << D_error_predict * D_predict_state_i.rightCols<3>()
+            * state_i.R().transpose();
+  if (H3)
+    *H3 << D_error_state_j.leftCols<6>();
+  if (H4)
+    *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();
+  if (H5)
+    *H5 << D_error_predict * D_predict_bias_i;
+
+  return error;
+}
+
+//------------------------------------------------------------------------------
+PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
+    const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+    const Vector3& n_gravity, const Vector3& omegaCoriolis,
+    const bool use2ndOrderCoriolis) {
+  // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
+  boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
+  q->n_gravity = n_gravity;
+  q->omegaCoriolis = omegaCoriolis;
+  q->use2ndOrderCoriolis = use2ndOrderCoriolis;
+  p_ = q;
+  return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
+}
+
+//------------------------------------------------------------------------------
+
+}/// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -22,25 +22,26 @@
 #pragma once
 
 #include <gtsam/navigation/PreintegratedRotation.h>
+#include <gtsam/navigation/NavState.h>
 #include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
+#include <boost/make_shared.hpp>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/Vector.h>
 
 namespace gtsam {
 
-/**
+/// @deprecated
- * Struct to hold all state variables of returned by Predict function
- */
 struct PoseVelocityBias {
   Pose3 pose;
   Vector3 velocity;
   imuBias::ConstantBias bias;
-
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
+      const imuBias::ConstantBias _bias) :
-      : pose(_pose),
+      pose(_pose), velocity(_velocity), bias(_bias) {
-        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);
   }
 };
 
@@ -50,51 +51,126 @@ struct PoseVelocityBias {
  * It includes the definitions of the preintegrated variables and the methods
  * to access, print, and compare them.
  */
-class PreintegrationBase : public PreintegratedRotation {
+class PreintegrationBase {
 
-  imuBias::ConstantBias biasHat_;  ///< Acceleration and angular rate bias values used during preintegration
+public:
-  bool use2ndOrderIntegration_;  ///< Controls the order of integration
 
-  Vector3 deltaPij_;  ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+  /// Parameters for pre-integration:
-  Vector3 deltaVij_;  ///< Preintegrated relative velocity (in global frame)
+  /// Usage: Create just a single Params and pass a shared pointer to the constructor
+  struct Params: PreintegratedRotation::Params {
+    Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
+    Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
+    bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
+    Vector3 n_gravity; ///< Gravity vector in nav frame
 
+    /// The Params constructor insists on getting the navigation frame gravity vector
+    /// For convenience, two commonly used conventions are provided by named constructors below
+    Params(const Vector3& n_gravity) :
+        accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
+            false), n_gravity(n_gravity) {
+    }
+
+    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
+    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, g));
+    }
+
+    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, -g));
+    }
+
+    void print(const std::string& s) const;
+
+  protected:
+    /// Default constructor for serialization only: uninitialized!
+    Params();
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class ARCHIVE>
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
+      ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
+      ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
+      ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
+      ar & BOOST_SERIALIZATION_NVP(n_gravity);
+    }
+  };
+
+protected:
+
+  double deltaTij_;   ///< Time interval from i to j
+
+  /**
+   * Preintegrated navigation state, from frame i to frame j
+   * Note: relative position does not take into account velocity at time i, see deltap+, in [2]
+   * Note: velocity is now also in frame i, as opposed to deltaVij in [2]
+   */
+  NavState deltaXij_;
+
+  /// Parameters
+  boost::shared_ptr<Params> p_;
+
+  /// Acceleration and gyro bias used for preintegration
+  imuBias::ConstantBias biasHat_;
+
+  Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
   Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
   Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
   Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
   Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 
-  const Matrix3 accelerometerCovariance_;  ///< continuous-time "Covariance" of accelerometer measurements
+  /// Default constructor for serialization
-  const Matrix3 integrationCovariance_;  ///< continuous-time "Covariance" describing integration uncertainty
+  PreintegrationBase() {
-  /// (to compensate errors in Euler integration)
+  }
 
- public:
+public:
 
   /**
-   *  Default constructor, initializes the variables in the base class
+   *  Constructor, initializes the variables in the base class
    *  @param bias Current estimate of acceleration and rotation rate biases
-   *  @param use2ndOrderIntegration     Controls the order of integration
+   *  @param p    Parameters, typically fixed in a single application
-   *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
    */
-  PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+  PreintegrationBase(const boost::shared_ptr<Params>& p,
-                     const Matrix3& measuredOmegaCovariance,
+      const imuBias::ConstantBias& biasHat) :
-                     const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration);
+      p_(p), biasHat_(biasHat) {
+    resetIntegration();
+  }
 
-  /// methods to access class variables
+  /// Re-initialize PreintegratedMeasurements
-  bool use2ndOrderIntegration() const {
+  void resetIntegration();
-    return use2ndOrderIntegration_;
+
+  const Params& p() const {
+    return *boost::static_pointer_cast<Params>(p_);
   }
-  const Vector3& deltaPij() const {
+
-    return deltaPij_;
+  void set_body_P_sensor(const Pose3& body_P_sensor) {
+    p_->body_P_sensor = body_P_sensor;
   }
-  const Vector3& deltaVij() const {
+
-    return deltaVij_;
+  /// getters
+  const NavState& deltaXij() const {
+    return deltaXij_;
+  }
+  const double& deltaTij() const {
+    return deltaTij_;
+  }
+  const Rot3& deltaRij() const {
+    return deltaXij_.attitude();
+  }
+  Vector3 deltaPij() const {
+    return deltaXij_.position().vector();
+  }
+  Vector3 deltaVij() const {
+    return deltaXij_.velocity();
   }
   const imuBias::ConstantBias& biasHat() const {
     return biasHat_;
   }
-  Vector6 biasHatVector() const {
+  const Matrix3& delRdelBiasOmega() const {
-    return biasHat_.vector();
+    return delRdelBiasOmega_;
-  }  // expensive
+  }
   const Matrix3& delPdelBiasAcc() const {
     return delPdelBiasAcc_;
   }
@@ -108,11 +184,9 @@ class PreintegrationBase : public PreintegratedRotation {
     return delVdelBiasOmega_;
   }
 
-  const Matrix3& accelerometerCovariance() const {
+  // Exposed for MATLAB
-    return accelerometerCovariance_;
+  Vector6 biasHatVector() const {
-  }
+    return biasHat_.vector();
-  const Matrix3& integrationCovariance() const {
-    return integrationCovariance_;
   }
 
   /// print
@@ -121,50 +195,62 @@ class PreintegrationBase : public PreintegratedRotation {
   /// check equality
   bool equals(const PreintegrationBase& other, double tol) const;
 
-  /// Re-initialize PreintegratedMeasurements
+  /// Subtract estimate and correct for sensor pose
-  void resetIntegration();
+  /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
+  /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
+  std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
+      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+      OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
+      OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
+      OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
 
-  /// Update preintegrated measurements
+  /// Calculate the updated preintegrated measurement, does not modify
-  void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
+  /// It takes measured quantities in the j frame
-                                       const double deltaT, OptionalJacobian<9, 9> F);
+  NavState updatedDeltaXij(const Vector3& j_measuredAcc,
+      const Vector3& j_measuredOmega, const double dt,
+      OptionalJacobian<9, 9> D_updated_current = boost::none,
+      OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
+      OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
 
-  /// Update Jacobians to be used during preintegration
+  /// Update preintegrated measurements and get derivatives
-  void updatePreintegratedJacobians(const Vector3& correctedAcc,
+  /// It takes measured quantities in the j frame
-                                    const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
+  void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
-                                    double deltaT);
+      const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
+      Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
 
-  void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
+  /// Given the estimate of the bias, return a NavState tangent vector
-                                              const Vector3& measuredOmega, Vector3& correctedAcc,
+  /// summarizing the preintegrated IMU measurements so far
-                                              Vector3& correctedOmega,
+  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-                                              boost::optional<const Pose3&> body_P_sensor);
+      OptionalJacobian<9, 6> H = boost::none) const;
 
   /// Predict state at time j
-  PoseVelocityBias predict(
+  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
-      const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+          boost::none) const;
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const;
 
   /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
-  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
+  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
-                                   const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
+      const Pose3& pose_j, const Vector3& vel_j,
-                                   const Vector3& gravity, const Vector3& omegaCoriolis,
+      const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
-                                   const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 =
+          boost::none, OptionalJacobian<9, 3> H2 = boost::none,
-                                       boost::none,
+      OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
-                                   OptionalJacobian<9, 3> H2 = boost::none,
+          boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
-                                   OptionalJacobian<9, 6> H3 = boost::none,
-                                   OptionalJacobian<9, 3> H4 = boost::none,
-                                   OptionalJacobian<9, 6> H5 = boost::none) const;
 
- private:
+  /// @deprecated predict
+  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
+      const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
+
+private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
+    ar & BOOST_SERIALIZATION_NVP(p_);
+    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaXij_);
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
-    ar & BOOST_SERIALIZATION_NVP(deltaPij_);
+    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
-    ar & BOOST_SERIALIZATION_NVP(deltaVij_);
     ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
     ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
     ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
@@ -172,32 +258,4 @@ class PreintegrationBase : public PreintegratedRotation {
   }
 };
 
-class ImuBase {
-
- protected:
-
-  Vector3 gravity_;
-  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
-  bool use2ndOrderCoriolis_;  ///< Controls whether higher order terms are included when calculating the Coriolis Effect
-
- public:
-
-  /// Default constructor, with decent gravity and zero coriolis
-  ImuBase();
-
-  /// Fully fledge constructor
-  ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
-          boost::optional<const Pose3&> body_P_sensor = boost::none,
-          const bool use2ndOrderCoriolis = false);
-
-  const Vector3& gravity() const {
-    return gravity_;
-  }
-  const Vector3& omegaCoriolis() const {
-    return omegaCoriolis_;
-  }
-
-};
-
 } /// namespace gtsam

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -35,6 +35,12 @@ using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
+Vector3 kZeroOmegaCoriolis(0,0,0);
+
+// Define covariance matrices
+double accNoiseVar = 0.01;
+const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
+
 //******************************************************************************
 namespace {
 Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
@@ -50,8 +56,8 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
 AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
     const Vector3& bias, const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3()) {
+    const Vector3& initialRotationRate = Vector3::Zero()) {
-  AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
+  AHRSFactor::PreintegratedMeasurements result(bias, I_3x3);
 
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
   list<double>::const_iterator itDeltaT = deltaTs.begin();
@@ -95,7 +101,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
   double expectedDeltaT1(0.5);
 
   // Actual preintegrated values
-  AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero());
+  AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3);
   actual1.integrateMeasurement(measuredOmega, deltaT);
 
   EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
@@ -121,18 +127,14 @@ TEST(AHRSFactor, Error) {
   Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
 
   // Measurements
-  Vector3 gravity;
-  gravity << 0, 0, 9.81;
-  Vector3 omegaCoriolis;
-  omegaCoriolis << 0, 0, 0;
   Vector3 measuredOmega;
   measuredOmega << M_PI / 100, 0, 0;
   double deltaT = 1.0;
-  AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
+  AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3);
-  pre_int_data.integrateMeasurement(measuredOmega, deltaT);
+  pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none);
+  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none);
 
   Vector3 errorActual = factor.evaluateError(x1, x2, bias);
 
@@ -182,18 +184,16 @@ TEST(AHRSFactor, ErrorWithBiases) {
   Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
 
   // Measurements
-  Vector3 omegaCoriolis;
-  omegaCoriolis << 0, 0.0, 0.0;
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   double deltaT = 1.0;
 
-  AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0),
+  AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0),
-      Matrix3::Zero());
+      Z_3x3);
-  pre_int_data.integrateMeasurement(measuredOmega, deltaT);
+  pim.integrateMeasurement(measuredOmega, deltaT);
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
+  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
 
   Vector errorActual = factor.evaluateError(x1, x2, bias);
 
@@ -269,7 +269,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
   const Vector3 x = thetahat; // parametrization of so(3)
   const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
   double normx = norm_2(x);
-  const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X
+  const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
       + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
           * X;
 
@@ -359,8 +359,6 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
   Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
 
   // Measurements
-  Vector3 gravity;
-  gravity << 0, 0, 9.81;
   Vector3 omegaCoriolis;
   omegaCoriolis << 0, 0.1, 0.1;
   Vector3 measuredOmega;
@@ -370,13 +368,15 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
   const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
       Point3(1, 0, 0));
 
-  AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(),
+  AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
-      Matrix3::Zero());
 
-  pre_int_data.integrateMeasurement(measuredOmega, deltaT);
+  pim.integrateMeasurement(measuredOmega, deltaT);
+
+  // Check preintegrated covariance
+  EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));
 
   // Create factor
-  AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
+  AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
 
   // Expected Jacobians
   Matrix H1e = numericalDerivative11<Vector, Rot3>(
@@ -407,34 +407,35 @@ TEST (AHRSFactor, predictTest) {
   Vector3 bias(0,0,0);
 
   // Measurements
-  Vector3 gravity;
-  gravity << 0, 0, 9.81;
-  Vector3 omegaCoriolis;
-  omegaCoriolis << 0, 0.0, 0.0;
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0;
-  double deltaT = 0.001;
+  double deltaT = 0.2;
-  AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
+  AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance);
   for (int i = 0; i < 1000; ++i) {
-    pre_int_data.integrateMeasurement(measuredOmega, deltaT);
+    pim.integrateMeasurement(measuredOmega, deltaT);
   }
-  AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
+  // Check preintegrated covariance
+  Matrix expectedMeasCov(3,3);
+  expectedMeasCov = 200*kMeasuredAccCovariance;
+  EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
+
+  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
 
   // Predict
   Rot3 x;
-  Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0);
+  Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0);
-  Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis);
+  Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
   EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
 
   // AHRSFactor::PreintegratedMeasurements::predict
   Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
       boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
-          &pre_int_data, _1, boost::none), bias);
+          &pim, _1, boost::none), bias);
 
   // Actual Jacobians
   Matrix H;
-  (void) pre_int_data.predict(bias,H);
+  (void) pim.predict(bias,H);
-  EXPECT(assert_equal(expectedH, H));
+  EXPECT(assert_equal(expectedH, H, 1e-8));
 }
 //******************************************************************************
 #include <gtsam/linear/GaussianFactorGraph.h>
@@ -448,12 +449,7 @@ TEST (AHRSFactor, graphTest) {
 
   // PreIntegrator
   Vector3 biasHat(0, 0, 0);
-  Vector3 gravity;
+  AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance);
-  gravity << 0, 0, 9.81;
-  Vector3 omegaCoriolis;
-  omegaCoriolis << 0, 0, 0;
-  AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat,
-      Matrix3::Identity());
 
   // Pre-integrate measurements
   Vector3 measuredOmega(0, M_PI / 20, 0);
@@ -461,15 +457,15 @@ TEST (AHRSFactor, graphTest) {
 
   // Create Factor
   noiseModel::Base::shared_ptr model = //
-      noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov());
+      noiseModel::Gaussian::Covariance(pim.preintMeasCov());
   NonlinearFactorGraph graph;
   Values values;
   for (size_t i = 0; i < 5; ++i) {
-    pre_int_data.integrateMeasurement(measuredOmega, deltaT);
+    pim.integrateMeasurement(measuredOmega, deltaT);
   }
 
-  // pre_int_data.print("Pre integrated measurementes");
+  // pim.print("Pre integrated measurementes");
-  AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
+  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
   values.insert(X(1), x1);
   values.insert(X(2), x2);
   values.insert(B(1), bias);

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -40,46 +40,6 @@ using symbol_shorthand::V;
 using symbol_shorthand::B;
 
 namespace {
-/* ************************************************************************* */
-// Auxiliary functions to test Jacobians F and G used for
-// covariance propagation during preintegration
-/* ************************************************************************* */
-Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
-    const Vector3& deltaVij_old, const Rot3& deltaRij_old,
-    const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
-    const Vector3& correctedOmega, const double deltaT,
-    const bool use2ndOrderIntegration) {
-
-  Matrix3 dRij = deltaRij_old.matrix();
-  Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
-  Vector3 deltaPij_new;
-  if (!use2ndOrderIntegration) {
-    deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
-  } else {
-    deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
-  }
-  Vector3 deltaVij_new = deltaVij_old + temp;
-  Rot3 deltaRij_new = deltaRij_old
-      * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
-  Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
-  imuBias::ConstantBias bias_new(bias_old);
-  Vector result(15);
-  result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
-  return result;
-}
-
-Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
-    const Vector3& deltaVij_old, const Rot3& deltaRij_old,
-    const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
-    const Vector3& correctedOmega, const double deltaT,
-    const bool use2ndOrderIntegration) {
-
-  Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
-      deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
-      deltaT, use2ndOrderIntegration);
-
-  return Rot3::Expmap(result.segment<3>(6));
-}
 
 // Auxiliary functions to test preintegrated Jacobians
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
@@ -87,9 +47,8 @@ Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
 CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-  CombinedImuFactor::CombinedPreintegratedMeasurements result(bias,
+  CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3,
-      Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
+      I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
-      Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false);
 
   list<Vector3>::const_iterator itAcc = measuredAccs.begin();
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@@ -136,25 +95,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
   double tol = 1e-6;
 
   // Actual preintegrated values
-  ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
+  ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3);
-      Matrix3::Zero(), Matrix3::Zero());
   expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
+  CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3,
-      Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
+      Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6);
-      Matrix3::Zero(), Matrix::Zero(6, 6));
 
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  EXPECT(
+  EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
-      assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()),
+  EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
-          tol));
+  EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
-  EXPECT(
-      assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()),
-          tol));
-  EXPECT(
-      assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()),
-          tol));
   DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
 }
 
@@ -180,46 +131,39 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
   double deltaT = 1.0;
   double tol = 1e-6;
 
-  Matrix I6x6(6, 6);
+  ImuFactor::PreintegratedMeasurements pim(
-  I6x6 = Matrix::Identity(6, 6);
-
-  ImuFactor::PreintegratedMeasurements pre_int_data(
       imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
-      Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
+      I_3x3, I_3x3, I_3x3);
 
-  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
+  CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
       imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
-      Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
+      I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6);
-      Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6);
 
-  Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
+  combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-      deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity,
+  ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity,
       omegaCoriolis);
 
   noiseModel::Gaussian::shared_ptr Combinedmodel =
-      noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
+      noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
-  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
+  CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
-      Combined_pre_int_data, gravity, omegaCoriolis);
+      combined_pim, gravity, omegaCoriolis);
 
-  Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
+  Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
-
+  Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
-  Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias,
       bias2);
-
   EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
 
   // Expected Jacobians
   Matrix H1e, H2e, H3e, H4e, H5e;
-  (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
+  (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a, H4a, H5a, H6a;
-  (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
+  (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
       H3a, H4a, H5a, H6a);
 
   EXPECT(assert_equal(H1e, H1a.topRows(9)));
@@ -253,7 +197,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
   }
 
   // Actual preintegrated values
-  CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
+  CombinedImuFactor::CombinedPreintegratedMeasurements pim =
       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
           deltaTs);
 
@@ -280,16 +224,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
+  EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc()));
-  EXPECT(
+  EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega()));
-      assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
+  EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc()));
-  EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
+  EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega()));
-  EXPECT(
-      assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
   EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
-  EXPECT(
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
-      assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
-          1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
 /* ************************************************************************* */
@@ -307,28 +247,23 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
   measuredAcc << 0, 1.1, -9.81;
   double deltaT = 0.001;
 
-  Matrix I6x6(6, 6);
+  CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
-  I6x6 = Matrix::Identity(6, 6);
+      I_3x3, I_3x3, 2 * I_3x3, I_6x6);
-
-  CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
-      bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
-      Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
 
   for (int i = 0; i < 1000; ++i)
-    Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
+    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-        deltaT);
 
   // Create factor
-  noiseModel::Gaussian::shared_ptr Combinedmodel =
+  noiseModel::Gaussian::shared_ptr combinedmodel =
-      noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
+      noiseModel::Gaussian::Covariance(pim.preintMeasCov());
-  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
+  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
-      Combined_pre_int_data, gravity, omegaCoriolis);
+      gravity, omegaCoriolis);
 
   // Predict
   Pose3 x1;
   Vector3 v1(0, 0.0, 0.0);
-  PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1,
+  PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity,
-      bias, gravity, omegaCoriolis);
+      omegaCoriolis);
   Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
   Vector3 expectedVelocity;
   expectedVelocity << 0, 1, 0;
@@ -340,10 +275,8 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
 /* ************************************************************************* */
 TEST(CombinedImuFactor, PredictRotation) {
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
-  Matrix I6x6(6, 6);
+  CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
-  CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
+      I_3x3, I_3x3, 2 * I_3x3, I_6x6);
-      bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
-      Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
   Vector3 measuredAcc;
   measuredAcc << 0, 0, -9.81;
   Vector3 gravity;
@@ -355,175 +288,18 @@ TEST(CombinedImuFactor, PredictRotation) {
   double deltaT = 0.001;
   double tol = 1e-4;
   for (int i = 0; i < 1000; ++i)
-    Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
+    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-        deltaT);
+  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
-  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
+      gravity, omegaCoriolis);
-      Combined_pre_int_data, gravity, omegaCoriolis);
 
   // Predict
   Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
   Vector3 v(0, 0, 0), v2;
-  CombinedImuFactor::Predict(x, v, x2, v2, bias,
+  CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
-      Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
   Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
   EXPECT(assert_equal(expectedPose, x2, tol));
 }
 
-/* ************************************************************************* */
-TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
-  // Linearization point
-  imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
-  Pose3 body_P_sensor = Pose3();
-
-  // Measurements
-  list<Vector3> measuredAccs, measuredOmegas;
-  list<double> deltaTs;
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
-  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
-  deltaTs.push_back(0.01);
-  for (int i = 1; i < 100; i++) {
-    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
-    measuredOmegas.push_back(
-        Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
-    deltaTs.push_back(0.01);
-  }
-  // Actual preintegrated values
-  CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
-      evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas,
-          deltaTs);
-
-  // so far we only created a nontrivial linearization point for the preintegrated measurements
-  // Now we add a new measurement and ask for Jacobians
-  const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
-  const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
-  const double newDeltaT = 0.01;
-  const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
-  const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
-  const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
-
-  Matrix oldPreintCovariance = preintegrated.preintMeasCov();
-
-  Matrix Factual, Gactual;
-  preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
-      newDeltaT, body_P_sensor, Factual, Gactual);
-
-  bool use2ndOrderIntegration = false;
-
-  //////////////////////////////////////////////////////////////////////////////////////////////
-  // COMPUTE NUMERICAL DERIVATIVES FOR F
-  //////////////////////////////////////////////////////////////////////////////////////////////
-  // Compute expected F wrt positions (15,3)
-  Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), deltaPij_old);
-  // rotation part has to be done properly, on manifold
-  df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), deltaPij_old);
-
-  // Compute expected F wrt velocities (15,3)
-  Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), deltaVij_old);
-  // rotation part has to be done properly, on manifold
-  df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), deltaVij_old);
-
-  // Compute expected F wrt angles (15,3)
-  Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), deltaRij_old);
-  // rotation part has to be done properly, on manifold
-  df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
-      boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), deltaRij_old);
-
-  // Compute expected F wrt biases (15,6)
-  Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
-  // rotation part has to be done properly, on manifold
-  df_dbias.block<3, 6>(6, 0) =
-      numericalDerivative11<Rot3, imuBias::ConstantBias>(
-          boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-              deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-              newDeltaT, use2ndOrderIntegration), bias_old);
-
-  Matrix Fexpected(15, 15);
-  Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
-  EXPECT(assert_equal(Fexpected, Factual));
-
-  //////////////////////////////////////////////////////////////////////////////////////////////
-  // COMPUTE NUMERICAL DERIVATIVES FOR G
-  //////////////////////////////////////////////////////////////////////////////////////////////
-  // Compute expected G wrt integration noise
-  Matrix df_dintNoise(15, 3);
-  df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
-
-  // Compute expected G wrt acc noise (15,3)
-  Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), newMeasuredAcc);
-  // rotation part has to be done properly, on manifold
-  df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
-          use2ndOrderIntegration), newMeasuredAcc);
-
-  // Compute expected G wrt gyro noise (15,3)
-  Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
-          use2ndOrderIntegration), newMeasuredOmega);
-  // rotation part has to be done properly, on manifold
-  df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
-          use2ndOrderIntegration), newMeasuredOmega);
-
-  // Compute expected G wrt bias random walk noise (15,6)
-  Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
-  df_rwBias.setZero();
-  df_rwBias.block<6, 6>(9, 0) = eye(6);
-
-  // Compute expected G wrt gyro noise (15,6)
-  Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
-      boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
-  // rotation part has to be done properly, on manifold
-  df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
-      imuBias::ConstantBias>(
-      boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
-  df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows
-
-  Matrix Gexpected(15, 21);
-  Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
-
-  EXPECT(assert_equal(Gexpected, Gactual));
-
-  // Check covariance propagation
-  Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
-      * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose();
-
-  Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
-  EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/tests/testNavState.cpp

@@ -0,0 +1,272 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testNavState.cpp
+ * @brief   Unit tests for NavState
+ * @author  Frank Dellaert
+ * @date    July 2015
+ */
+
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
+static const Point3 kPosition(1.0, 2.0, 3.0);
+static const Pose3 kPose(kAttitude, kPosition);
+static const Velocity3 kVelocity(0.4, 0.5, 0.6);
+static const NavState kIdentity;
+static const NavState kState1(kAttitude, kPosition, kVelocity);
+static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
+static const Vector3 kGravity(0, 0, 9.81);
+static const Vector9 kZeroXi = Vector9::Zero();
+
+/* ************************************************************************* */
+TEST(NavState, Constructor) {
+  boost::function<NavState(const Pose3&, const Vector3&)> construct =
+      boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
+          boost::none);
+  Matrix aH1, aH2;
+  EXPECT(
+      assert_equal(kState1,
+          NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
+  EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
+  EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
+}
+
+/* ************************************************************************* */
+TEST( NavState, Attitude) {
+  Matrix39 aH, eH;
+  Rot3 actual = kState1.attitude(aH);
+  EXPECT(assert_equal(actual, kAttitude));
+  eH = numericalDerivative11<Rot3, NavState>(
+      boost::bind(&NavState::attitude, _1, boost::none), kState1);
+  EXPECT(assert_equal((Matrix )eH, aH));
+}
+
+/* ************************************************************************* */
+TEST( NavState, Position) {
+  Matrix39 aH, eH;
+  Point3 actual = kState1.position(aH);
+  EXPECT(assert_equal(actual, kPosition));
+  eH = numericalDerivative11<Point3, NavState>(
+      boost::bind(&NavState::position, _1, boost::none), kState1);
+  EXPECT(assert_equal((Matrix )eH, aH));
+}
+
+/* ************************************************************************* */
+TEST( NavState, Velocity) {
+  Matrix39 aH, eH;
+  Velocity3 actual = kState1.velocity(aH);
+  EXPECT(assert_equal(actual, kVelocity));
+  eH = numericalDerivative11<Velocity3, NavState>(
+      boost::bind(&NavState::velocity, _1, boost::none), kState1);
+  EXPECT(assert_equal((Matrix )eH, aH));
+}
+
+/* ************************************************************************* */
+TEST( NavState, BodyVelocity) {
+  Matrix39 aH, eH;
+  Velocity3 actual = kState1.bodyVelocity(aH);
+  EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity)));
+  eH = numericalDerivative11<Velocity3, NavState>(
+      boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
+  EXPECT(assert_equal((Matrix )eH, aH));
+}
+
+/* ************************************************************************* */
+TEST( NavState, MatrixGroup ) {
+  // check roundtrip conversion to 7*7 matrix representation
+  Matrix7 T = kState1.matrix();
+  EXPECT(assert_equal(kState1, NavState(T)));
+
+  // check group product agrees with matrix product
+  NavState state2 = kState1 * kState1;
+  Matrix T2 = T * T;
+  EXPECT(assert_equal(state2, NavState(T2)));
+  EXPECT(assert_equal(T2, state2.matrix()));
+}
+
+/* ************************************************************************* */
+TEST( NavState, Manifold ) {
+  // Check zero xi
+  EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi)));
+  EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity)));
+  EXPECT(assert_equal(kState1, kState1.retract(kZeroXi)));
+  EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1)));
+
+  // Check definition of retract as operating on components separately
+  Vector9 xi;
+  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
+  Rot3 drot = Rot3::Expmap(xi.head<3>());
+  Point3 dt = Point3(xi.segment<3>(3));
+  Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
+  NavState state2 = kState1 * NavState(drot, dt, dvel);
+  EXPECT(assert_equal(state2, kState1.retract(xi)));
+  EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
+
+  // roundtrip from state2 to state3 and back
+  NavState state3 = state2.retract(xi);
+  EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
+
+  // Check derivatives for ChartAtOrigin::Retract
+  Matrix9 aH;
+  //  For zero xi
+  boost::function<NavState(const Vector9&)> Retract = boost::bind(
+      NavState::ChartAtOrigin::Retract, _1, boost::none);
+  NavState::ChartAtOrigin::Retract(kZeroXi, aH);
+  EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
+  //  For non-zero xi
+  NavState::ChartAtOrigin::Retract(xi, aH);
+  EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
+
+  // Check derivatives for ChartAtOrigin::Local
+  //  For zero xi
+  boost::function<Vector9(const NavState&)> Local = boost::bind(
+      NavState::ChartAtOrigin::Local, _1, boost::none);
+  NavState::ChartAtOrigin::Local(kIdentity, aH);
+  EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
+  //  For non-zero xi
+  NavState::ChartAtOrigin::Local(kState1, aH);
+  EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
+
+  // Check retract derivatives
+  Matrix9 aH1, aH2;
+  kState1.retract(xi, aH1, aH2);
+  boost::function<NavState(const NavState&, const Vector9&)> retract =
+      boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
+  EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
+  EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
+
+  // Check localCoordinates derivatives
+  boost::function<Vector9(const NavState&, const NavState&)> local =
+      boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
+          boost::none);
+  // from state1 to state2
+  kState1.localCoordinates(state2, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
+  EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
+  // from identity to state2
+  kIdentity.localCoordinates(state2, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
+  EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
+  // from state2 to identity
+  state2.localCoordinates(kIdentity, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
+  EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
+}
+
+/* ************************************************************************* */
+TEST( NavState, Lie ) {
+  // Check zero xi
+  EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
+  EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
+  EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
+  EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
+
+  // Expmap/Logmap roundtrip
+  Vector xi(9);
+  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
+  NavState state2 = NavState::Expmap(xi);
+  EXPECT(assert_equal(xi, NavState::Logmap(state2)));
+
+  // roundtrip from state2 to state3 and back
+  NavState state3 = state2.expmap(xi);
+  EXPECT(assert_equal(xi, state2.logmap(state3)));
+
+  // For the expmap/logmap (not necessarily expmap/local) -xi goes other way
+  EXPECT(assert_equal(state2, state3.expmap(-xi)));
+  EXPECT(assert_equal(xi, -state3.logmap(state2)));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Update) {
+  Vector3 omega(M_PI / 100.0, 0.0, 0.0);
+  Vector3 acc(0.1, 0.0, 0.0);
+  double dt = 10;
+  Matrix9 aF;
+  Matrix93 aG1, aG2;
+  boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
+      boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
+          boost::none, boost::none);
+  Vector3 b_acc = kAttitude * acc;
+  NavState expected(kAttitude.expmap(dt * omega),
+      kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),
+      kVelocity + b_acc * dt);
+  NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2);
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
+  EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
+  EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
+
+  // Try different values
+  omega = Vector3(0.1, 0.2, 0.3);
+  acc = Vector3(0.4, 0.5, 0.6);
+  kState1.update(acc, omega, dt, aF, aG1, aG2);
+  EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
+  EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
+  EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
+}
+
+/* ************************************************************************* */
+static const double dt = 2.0;
+boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
+    &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
+
+TEST(NavState, Coriolis) {
+  Matrix9 aH;
+
+  // first-order
+  kState1.coriolis(dt, kOmegaCoriolis, false, aH);
+  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
+  // second-order
+  kState1.coriolis(dt, kOmegaCoriolis, true, aH);
+  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
+}
+
+TEST(NavState, Coriolis2) {
+  Matrix9 aH;
+  NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
+      Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
+
+  // first-order
+  state2.coriolis(dt, kOmegaCoriolis, false, aH);
+  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
+  // second-order
+  state2.coriolis(dt, kOmegaCoriolis, true, aH);
+  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
+}
+
+/* ************************************************************************* */
+TEST(NavState, CorrectPIM) {
+  Vector9 xi;
+  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
+  double dt = 0.5;
+  Matrix9 aH1, aH2;
+  boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
+      boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
+          false, boost::none, boost::none);
+  kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
+  EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/navigation/tests/testPoseVelocityBias.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testPoseVelocityBias.cpp
+ * @brief   Unit test for PoseVelocityBias
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
+Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
+  Matrix3 R1 = pvb1.pose.rotation().matrix();
+  // Ri.transpose() translate the error from the global frame into pose1's frame
+  const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector();
+  const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity);
+  const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation());
+  const Vector3 fR = Rot3::Logmap(R1BetweenR2);
+  Vector9 r;
+  r << fp, fv, fR;
+  return r;
+}
+
+/* ************************************************************************************************/
+TEST(PoseVelocityBias, error) {
+  Point3 i1(0, 1, 0), j1(-1, 0, 0), k1(0, 0, 1);
+  Pose3 x1(Rot3(i1, j1, k1), Point3(5.0, 1.0, 0.0));
+  Vector3 v1(Vector3(0.5, 0.0, 0.0));
+  imuBias::ConstantBias bias1(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
+
+  Pose3 x2(Rot3(i1, j1, k1).expmap(Vector3(0.1, 0.2, 0.3)), Point3(5.5, 1.0, 6.0));
+  Vector3 v2(Vector3(0.5, 4.0, 3.0));
+  imuBias::ConstantBias bias2(Vector3(0.1, 0.2, -0.3), Vector3(0.2, 0.3, 0.1));
+
+  PoseVelocityBias pvb1(x1, v1, bias1), pvb2(x2, v2, bias2);
+
+  Vector9 expected, actual = error(pvb1, pvb2);
+  expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
+  EXPECT(assert_equal(expected, actual, 1e-9));
+}
+
+/* ************************************************************************************************/
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************************************/

gtsam/nonlinear/factorTesting.h

@@ -85,9 +85,23 @@ bool testFactorJacobians(TestResult& result_, const std::string& name_,
   // Create actual value by linearize
   boost::shared_ptr<JacobianFactor> actual = //
       boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
+  if (!actual) return false;
 
   // Check cast result and then equality
-  return actual && assert_equal(expected, *actual, tolerance);
+  bool equal = assert_equal(expected, *actual, tolerance);
+
+  // if not equal, test individual jacobians:
+  if (!equal) {
+    for (size_t i = 0; i < actual->size(); i++) {
+      bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
+          (Matrix) (actual->getA(actual->begin() + i)), tolerance);
+      if (!i_good) {
+        std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
+      }
+    }
+  }
+
+  return equal;
 }
 }
 

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -38,13 +38,14 @@ Vector PoseRTV::vector() const {
   Vector rtv(9);
   rtv.head(3) = rotation().xyz();
   rtv.segment(3,3) = translation().vector();
-  rtv.tail(3) = velocity().vector();
+  rtv.tail(3) = velocity();
   return rtv;
 }
 
 /* ************************************************************************* */
 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
-  return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
+  return pose().equals(other.pose(), tol)
+      && equal_with_abs_tol(velocity(), other.velocity(), tol);
 }
 
 /* ************************************************************************* */
@@ -52,7 +53,7 @@ void PoseRTV::print(const string& s) const {
   cout << s << ":" << endl;
   gtsam::print((Vector)R().xyz(), "  R:rpy");
   t().print("  T");
-  velocity().print("  V");
+  gtsam::print((Vector)velocity(), "  V");
 }
 
 /* ************************************************************************* */
@@ -137,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
   Vector6 imu;
 
   // acceleration
-  Vector3 accel = (v2-v1).vector() / dt;
+  Vector3 accel = (v2-v1) / dt;
   imu.head<3>() = r2.transpose() * (accel - kGravity);
 
   // rotation rates
@@ -160,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
   // predict point for constraint
   // NOTE: uses simple Euler approach for prediction
-  Point3 pred_t2 = t() + v2 * dt;
+  Point3 pred_t2 = t().retract(v2 * dt);
   return pred_t2;
 }
 
@@ -187,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
 
   // Note that we rotate the velocity
   Matrix3 D_newvel_R, D_newvel_v;
-  Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
+  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
 
   if (Dglobal) {
     Dglobal->setZero();
@@ -204,10 +205,10 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
 }
 
 /* ************************************************************************* */
-Matrix PoseRTV::RRTMbn(const Vector& euler) {
+Matrix PoseRTV::RRTMbn(const Vector3& euler) {
   assert(euler.size() == 3);
-  const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
+  const double s1 = sin(euler.x()), c1 = cos(euler.x());
-  const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1));
+  const double t2 = tan(euler.y()), c2 = cos(euler.y());
   Matrix Ebn(3,3);
   Ebn << 1.0, s1 * t2, c1 * t2,
          0.0,      c1,     -s1,
@@ -221,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) {
 }
 
 /* ************************************************************************* */
-Matrix PoseRTV::RRTMnb(const Vector& euler) {
+Matrix PoseRTV::RRTMnb(const Vector3& euler) {
-  assert(euler.size() == 3);
   Matrix Enb(3,3);
-  const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
+  const double s1 = sin(euler.x()), c1 = cos(euler.x());
-  const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1));
+  const double s2 = sin(euler.y()), c2 = cos(euler.y());
   Enb << 1.0, 0.0,   -s2,
          0.0,  c1, s1*c2,
          0.0, -s1, c1*c2;

gtsam_unstable/dynamics/PoseRTV.h

@@ -13,11 +13,12 @@
 namespace gtsam {
 
 /// Syntactic sugar to clarify components
-typedef Point3 Velocity3;
+typedef Vector3 Velocity3;
 
 /**
  * Robot state for use with IMU measurements
  * - contains translation, translational velocity and rotation
+ * TODO(frank): Alex should deprecate/move to project
  */
 class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
 protected:
@@ -34,11 +35,11 @@ public:
   PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
   : Base(Pose3(rot, t), vel) {}
   explicit PoseRTV(const Point3& t)
-  : Base(Pose3(Rot3(), t),Velocity3()) {}
+  : Base(Pose3(Rot3(), t),Vector3::Zero()) {}
   PoseRTV(const Pose3& pose, const Velocity3& vel)
   : Base(pose, vel) {}
   explicit PoseRTV(const Pose3& pose)
-  : Base(pose,Velocity3()) {}
+  : Base(pose,Vector3::Zero()) {}
 
   // Construct from Base
   PoseRTV(const Base& base)
@@ -66,7 +67,7 @@ public:
   // and avoidance of Point3
   Vector vector() const;
   Vector translationVec() const { return pose().translation().vector(); }
-  Vector velocityVec() const { return velocity().vector(); }
+  const Velocity3& velocityVec() const { return velocity(); }
 
   // testable
   bool equals(const PoseRTV& other, double tol=1e-6) const;
@@ -145,14 +146,12 @@ public:
 
   /// RRTMbn - Function computes the rotation rate transformation matrix from
   /// body axis rates to euler angle (global) rates
-  static Matrix RRTMbn(const Vector& euler);
+  static Matrix RRTMbn(const Vector3& euler);
-
   static Matrix RRTMbn(const Rot3& att);
 
   /// RRTMnb - Function computes the rotation rate transformation matrix from
   /// euler angle rates to body axis rates
-  static Matrix RRTMnb(const Vector& euler);
+  static Matrix RRTMnb(const Vector3& euler);
-
   static Matrix RRTMnb(const Rot3& att);
   /// @}
 
@@ -173,14 +172,7 @@ struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
 // Define Range functor specializations that are used in RangeFactor
 template <typename A1, typename A2> struct Range;
 
-template <>
+template<>
-struct Range<PoseRTV, PoseRTV> {
+struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};
-  typedef double result_type;
-  double operator()(const PoseRTV& pose1, const PoseRTV& pose2,
-                    OptionalJacobian<1, 9> H1 = boost::none,
-                    OptionalJacobian<1, 9> H2 = boost::none) {
-    return pose1.range(pose2, H1, H2);
-  }
-};
 
 } // \namespace gtsam

gtsam_unstable/dynamics/VelocityConstraint.h

@@ -106,12 +106,13 @@ private:
   static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
       double dt, const dynamics::IntegrationMode& mode) {
 
-    const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();
+    const Velocity3& v1 = x1.v(), v2 = x2.v();
-    Velocity3 hx;
+    const Point3& p1 = x1.t(), p2 = x2.t();
+    Point3 hx;
     switch(mode) {
-    case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break;
+    case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
-    case dynamics::EULER_START: hx = p1 + v1 * dt; break;
+    case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
-    case dynamics::EULER_END  : hx = p1 + v2 * dt; break;
+    case dynamics::EULER_END  : hx = p1.retract(v2 * dt); break;
     default: assert(false); break;
     }
     return (p2 - hx).vector();

gtsam_unstable/dynamics/tests/testIMUSystem.cpp

@@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)),
+          pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
-          pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)),
+          pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0));
+          pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Unit::Create(6);
@@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
+          pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
-          pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
+          pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0));
+          pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -15,44 +15,43 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(PoseRTV)
 GTSAM_CONCEPT_LIE_INST(PoseRTV)
 
-const double tol=1e-5;
+static const Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
-
+static const Point3 pt(1.0, 2.0, 3.0);
-Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
+static const Velocity3 vel(0.4, 0.5, 0.6);
-Point3 pt(1.0, 2.0, 3.0);
+static const Vector3 kZero3 = Vector3::Zero();
-Velocity3 vel(0.4, 0.5, 0.6);
 
 /* ************************************************************************* */
 TEST( testPoseRTV, constructors ) {
   PoseRTV state1(pt, rot, vel);
-  EXPECT(assert_equal(pt, state1.t(), tol));
+  EXPECT(assert_equal(pt, state1.t()));
-  EXPECT(assert_equal(rot, state1.R(), tol));
+  EXPECT(assert_equal(rot, state1.R()));
-  EXPECT(assert_equal(vel, state1.v(), tol));
+  EXPECT(assert_equal(vel, state1.v()));
-  EXPECT(assert_equal(Pose3(rot, pt), state1.pose(), tol));
+  EXPECT(assert_equal(Pose3(rot, pt), state1.pose()));
 
   PoseRTV state2;
-  EXPECT(assert_equal(Point3(),  state2.t(), tol));
+  EXPECT(assert_equal(Point3(),  state2.t()));
-  EXPECT(assert_equal(Rot3(), state2.R(), tol));
+  EXPECT(assert_equal(Rot3(), state2.R()));
-  EXPECT(assert_equal(Velocity3(), state2.v(), tol));
+  EXPECT(assert_equal(kZero3, state2.v()));
-  EXPECT(assert_equal(Pose3(), state2.pose(), tol));
+  EXPECT(assert_equal(Pose3(), state2.pose()));
 
   PoseRTV state3(Pose3(rot, pt), vel);
-  EXPECT(assert_equal(pt,  state3.t(), tol));
+  EXPECT(assert_equal(pt,  state3.t()));
-  EXPECT(assert_equal(rot, state3.R(), tol));
+  EXPECT(assert_equal(rot, state3.R()));
-  EXPECT(assert_equal(vel, state3.v(), tol));
+  EXPECT(assert_equal(vel, state3.v()));
-  EXPECT(assert_equal(Pose3(rot, pt), state3.pose(), tol));
+  EXPECT(assert_equal(Pose3(rot, pt), state3.pose()));
 
   PoseRTV state4(Pose3(rot, pt));
-  EXPECT(assert_equal(pt,  state4.t(), tol));
+  EXPECT(assert_equal(pt,  state4.t()));
-  EXPECT(assert_equal(rot, state4.R(), tol));
+  EXPECT(assert_equal(rot, state4.R()));
-  EXPECT(assert_equal(Velocity3(), state4.v(), tol));
+  EXPECT(assert_equal(kZero3, state4.v()));
-  EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol));
+  EXPECT(assert_equal(Pose3(rot, pt), state4.pose()));
 
   Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3,  1.0, 2.0, 3.0,  0.4, 0.5, 0.6).finished();
   PoseRTV state5(vec_init);
-  EXPECT(assert_equal(pt,  state5.t(), tol));
+  EXPECT(assert_equal(pt,  state5.t()));
-  EXPECT(assert_equal(rot, state5.R(), tol));
+  EXPECT(assert_equal(rot, state5.R()));
-  EXPECT(assert_equal(vel, state5.v(), tol));
+  EXPECT(assert_equal(vel, state5.v()));
-  EXPECT(assert_equal(vec_init, state5.vector(), tol));
+  EXPECT(assert_equal(vec_init, state5.vector()));
 }
 
 /* ************************************************************************* */
@@ -65,33 +64,44 @@ TEST( testPoseRTV, dim ) {
 /* ************************************************************************* */
 TEST( testPoseRTV, equals ) {
   PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt));
-  EXPECT(assert_equal(state1, state1, tol));
+  EXPECT(assert_equal(state1, state1));
-  EXPECT(assert_equal(state2, state3, tol));
+  EXPECT(assert_equal(state2, state3));
-  EXPECT(assert_equal(state3, state2, tol));
+  EXPECT(assert_equal(state3, state2));
-  EXPECT(assert_inequal(state1, state2, tol));
+  EXPECT(assert_inequal(state1, state2));
-  EXPECT(assert_inequal(state2, state1, tol));
+  EXPECT(assert_inequal(state2, state1));
-  EXPECT(assert_inequal(state2, state4, tol));
+  EXPECT(assert_inequal(state2, state4));
 }
 
 /* ************************************************************************* */
 TEST( testPoseRTV, Lie ) {
   // origin and zero deltas
   PoseRTV identity;
-  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol));
+  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9))));
-  EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
+  EXPECT(assert_equal(zero(9), identity.localCoordinates(identity)));
 
   PoseRTV state1(pt, rot, vel);
-  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol));
+  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9))));
-  EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
+  EXPECT(assert_equal(zero(9), state1.localCoordinates(state1)));
 
-  Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished();
+  Vector delta(9);
-  Rot3 rot2 = rot.retract(repeat(3, 0.1));
+  delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
-  Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
+  Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>());
-  Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
+  Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
-  PoseRTV state2(pt2, rot2, vel2);
+  PoseRTV state2(pose2.translation(), pose2.rotation(), vel2);
-  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
+  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta)));
-  EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
+  EXPECT(assert_equal(delta, state1.localCoordinates(state2)));
-  EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
+
+  // roundtrip from state2 to state3 and back
+  PoseRTV state3 = state2.retract(delta);
+  EXPECT(assert_equal(delta, state2.localCoordinates(state3)));
+
+  // roundtrip from state3 to state4 and back, with expmap.
+  PoseRTV state4 = state3.expmap(delta);
+  EXPECT(assert_equal(delta, state3.logmap(state4)));
+
+  // For the expmap/logmap (not necessarily retract/local) -delta goes other way
+  EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta)));
+  EXPECT(assert_equal(delta, -state4.logmap(state3)));
 }
 
 /* ************************************************************************* */
@@ -108,15 +118,15 @@ TEST( testPoseRTV, dynamics_identities ) {
   x3 = x2.generalDynamics(accel, gyro, dt);
   x4 = x3.generalDynamics(accel, gyro, dt);
 
-//  EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first, tol));
+//  EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first));
-//  EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first, tol));
+//  EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first));
-//  EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first, tol));
+//  EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first));
-//  EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first, tol));
+//  EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first));
 //
-//  EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second, tol));
+//  EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second));
-//  EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second, tol));
+//  EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second));
-//  EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second, tol));
+//  EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second));
-//  EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol));
+//  EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second));
 }
 
 
@@ -129,8 +139,8 @@ TEST( testPoseRTV, compose ) {
   state1.compose(state2, actH1, actH2);
   Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
   Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
-  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH1, actH1));
-  EXPECT(assert_equal(numericH2, actH2, tol));
+  EXPECT(assert_equal(numericH2, actH2));
 }
 
 /* ************************************************************************* */
@@ -142,8 +152,8 @@ TEST( testPoseRTV, between ) {
   state1.between(state2, actH1, actH2);
   Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
   Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
-  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH1, actH1));
-  EXPECT(assert_equal(numericH2, actH2, tol));
+  EXPECT(assert_equal(numericH2, actH2));
 }
 
 /* ************************************************************************* */
@@ -154,7 +164,7 @@ TEST( testPoseRTV, inverse ) {
   Matrix actH1;
   state1.inverse(actH1);
   Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
-  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH1, actH1));
 }
 
 /* ************************************************************************* */
@@ -162,16 +172,16 @@ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
 TEST( testPoseRTV, range ) {
   Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0);
   PoseRTV rtvA(tA), rtvB(tB);
-  EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), tol);
+  EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), 1e-9);
-  EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), tol);
+  EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), 1e-9);
-  EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), tol);
+  EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), 1e-9);
 
   Matrix actH1, actH2;
   rtvA.range(rtvB, actH1, actH2);
   Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB);
   Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB);
-  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH1, actH1));
-  EXPECT(assert_equal(numericH2, actH2, tol));
+  EXPECT(assert_equal(numericH2, actH2));
 }
 
 /* ************************************************************************* */
@@ -187,13 +197,13 @@ TEST( testPoseRTV, transformed_from_1 ) {
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
-  PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
+  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
-  EXPECT(assert_equal(expected, actual, tol));
+  EXPECT(assert_equal(expected, actual));
 
   Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
   Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
-  EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
+  EXPECT(assert_equal(numDGlobal, actDGlobal));
-  EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative
+  EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
 }
 
 /* ************************************************************************* */
@@ -206,27 +216,27 @@ TEST( testPoseRTV, transformed_from_2 ) {
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
-  PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
+  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
-  EXPECT(assert_equal(expected, actual, tol));
+  EXPECT(assert_equal(expected, actual));
 
   Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
   Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
-  EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
+  EXPECT(assert_equal(numDGlobal, actDGlobal));
-  EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative
+  EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
 }
 
 /* ************************************************************************* */
 TEST(testPoseRTV, RRTMbn) {
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol));
+  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol));
+  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
-  EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
+  EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3))));
 }
 
 /* ************************************************************************* */
 TEST(testPoseRTV, RRTMnb) {
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol));
+  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol));
+  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
-  EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
+  EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3))));
 }
 
 /* ************************************************************************* */

gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp

@@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
 const double dt = 1.0;
 
 PoseRTV origin,
-        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)),
+        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
         pose1a(Point3(0.5, 0.0, 0.0)),
-        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0));
+        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
 
 /* ************************************************************************* */
 TEST( testVelocityConstraint, trapezoidal ) {

gtsam_unstable/gtsam_unstable.h

@@ -53,9 +53,9 @@ class Dummy {
 class PoseRTV {
   PoseRTV();
   PoseRTV(Vector rtv);
-  PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
+  PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel);
-  PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel);
+  PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel);
-  PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel);
+  PoseRTV(const gtsam::Pose3& pose, const Vector& vel);
   PoseRTV(const gtsam::Pose3& pose);
   PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
 
@@ -66,7 +66,7 @@ class PoseRTV {
   // access
   gtsam::Point3 translation() const;
   gtsam::Rot3 rotation() const;
-  gtsam::Point3 velocity() const;
+  Vector velocity() const;
   gtsam::Pose3 pose() const;
 
   // Vector interfaces

gtsam_unstable/slam/Mechanization_bRn2.cpp

@@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u,
 
   // convert to navigation frame
   Rot3 nRb = bRn_.inverse();
-  Vector3 n_omega_bn = (nRb*b_omega_bn).vector();
+  Vector3 n_omega_bn = nRb * b_omega_bn;
 
   // integrate bRn using exponential map, assuming constant over dt
   Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt);

gtsam_unstable/slam/Mechanization_bRn2.h

@@ -42,7 +42,7 @@ public:
   /// gravity in the body frame
   Vector3 b_g(double g_e) const {
     Vector3 n_g(0, 0, g_e);
-    return (bRn_ * n_g).vector();
+    return bRn_ * n_g;
   }
 
   const Rot3& bRn() const {return bRn_; }

tests/testLie.cpp

@@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) {
   Vector5 d;
   d << 1, 2, 0.1, 0.2, 0.3;
   Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
-  Product pair2 = pair1.retract(d);
+  Product pair2 = pair1.expmap(d);
   EXPECT(assert_equal(expected, pair2, 1e-9));
-  EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
+  EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9));
 }
 
 /* ************************************************************************* */