Merge remote-tracking branch 'origin/develop' into feature/concurrent-calibration

examples/Pose2SLAMwSPCG.cpp

@@ -104,7 +104,7 @@ int main(int argc, char** argv) {
   LevenbergMarquardtParams parameters;
   parameters.verbosity = NonlinearOptimizerParams::ERROR;
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
-  parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
+  parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
 
   {
     parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

gtsam.h

@@ -1481,9 +1481,7 @@ class GaussianISAM {
 
 #include <gtsam/linear/IterativeSolver.h>
 virtual class IterativeOptimizationParameters {
-  string getKernel() const ;
   string getVerbosity() const;
-  void setKernel(string s) ;
   void setVerbosity(string s) ;
   void print() const;
 };
@@ -1516,7 +1514,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   void print() const;
 };
 
-class SubgraphSolver  {
+virtual class SubgraphSolver  {
   SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   gtsam::VectorValues optimize() const;
@@ -1855,12 +1853,12 @@ virtual class NonlinearOptimizerParams {
   
   void setLinearSolverType(string solver);
   void setOrdering(const gtsam::Ordering& ordering);
-  void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
+  void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
 
   bool isMultifrontal() const;
   bool isSequential() const;
   bool isCholmod() const;
-  bool isCG() const;
+  bool isIterative() const;
 };
 
 bool checkConvergence(double relativeErrorTreshold,
@@ -2220,6 +2218,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+template<POSE, LANDMARK, CALIBRATION>
+virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+
+  SmartProjectionPoseFactor(double rankTol, double linThreshold,
+      bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
+
+  SmartProjectionPoseFactor(double rankTol);
+  SmartProjectionPoseFactor();
+
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i,
+      const CALIBRATION* K_i);
+
+  // enabling serialization functionality
+  //void serialize() const;
+};
+
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
+
 
 #include <gtsam/slam/StereoFactor.h>
 template<POSE, LANDMARK>
@@ -2322,7 +2339,8 @@ virtual class ConstantBias : gtsam::Value {
 #include <gtsam/navigation/ImuFactor.h>
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
+  ImuFactorPreintegratedMeasurements(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);
   ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
 
   // Testable
@@ -2360,6 +2378,15 @@ class CombinedImuFactorPreintegratedMeasurements {
       Matrix biasAccCovariance,
       Matrix biasOmegaCovariance,
       Matrix biasAccOmegaInit);
+  CombinedImuFactorPreintegratedMeasurements(
+      const gtsam::imuBias::ConstantBias& bias,
+      Matrix measuredAccCovariance,
+      Matrix measuredOmegaCovariance,
+      Matrix integrationErrorCovariance,
+      Matrix biasAccCovariance,
+      Matrix biasOmegaCovariance,
+      Matrix biasAccOmegaInit,
+      bool use2ndOrderIntegration);
   CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
 
   // Testable
@@ -2373,8 +2400,7 @@ class CombinedImuFactorPreintegratedMeasurements {
 
 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::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
-      const gtsam::noiseModel::Base* model);
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;

gtsam/geometry/Cal3DS2.cpp

@@ -25,7 +25,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Cal3DS2::Cal3DS2(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
+    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
 
 /* ************************************************************************* */
 Matrix Cal3DS2::K() const {
@@ -34,32 +34,64 @@ Matrix Cal3DS2::K() const {
 
 /* ************************************************************************* */
 Vector Cal3DS2::vector() const {
-  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
+  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
 }
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s) const {
+void Cal3DS2::print(const std::string& s_) const {
-  gtsam::print(K(), s + ".K");
+  gtsam::print(K(), s_ + ".K");
-  gtsam::print(Vector(k()), s + ".k");
+  gtsam::print(Vector(k()), s_ + ".k");
 }
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
   if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
       fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
+      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
     return false;
   return true;
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2::uncalibrate(const Point2& p,
+static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
-       boost::optional<Matrix&> H1,
+    double yy, double xy, double rr, double r4, double pnx, double pny,
-       boost::optional<Matrix&> H2) const {
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  Eigen::Matrix<double, 2, 5> DR1;
+  DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 4> DR2;
+  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
+         y * rr, y * r4, rr + 2 * yy, 2 * xy;
+  Eigen::Matrix<double, 2, 9> D;
+  D << DR1, DK * DR2;
+  return D;
+}
 
-  // parameters
+/* ************************************************************************* */
-  const double fx = fx_, fy = fy_, s = s_;
+static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
-  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
+    double g, double k1, double k2, double p1, double p2,
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  const double drdx = 2. * x;
+  const double drdy = 2. * y;
+  const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
+  const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
+
+  // Dx = 2*p1*xy + p2*(rr+2*xx);
+  // Dy = 2*p2*xy + p1*(rr+2*yy);
+  const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
+  const double dDxdy = 2. * p1 * x + p2 * drdy;
+  const double dDydx = 2. * p2 * y + p1 * drdx;
+  const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
+
+  Eigen::Matrix<double, 2, 2> DR;
+  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
+        y * dgdx + dDydx, g + y * dgdy + dDydy;
+
+  return DK * DR;
+}
+
+/* ************************************************************************* */
+Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
 
   //  rr = x^2 + y^2;
   //  g = (1 + k(1)*rr + k(2)*rr^2);
@@ -68,40 +100,29 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
-  const double g = 1. + k1 * rr + k2 * r4;
+  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
-  const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
-  const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
 
-  const double pnx = g*x + dx;
+  // tangential component
-  const double pny = g*y + dy;
+  const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
+  const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
 
-  // Inlined derivative for calibration
+  // Radial and tangential distortion applied
-  if (H1) {
+  const double pnx = g * x + dx;
-    *H1 = (Matrix(2, 9) <<  pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
+  const double pny = g * y + dy;
-        fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
-        fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
-        fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
-  }
-  // Inlined derivative for points
-  if (H2) {
-    const double dr_dx = 2. * x;
-    const double dr_dy = 2. * y;
-    const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
-    const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
 
-    const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
+  Eigen::Matrix<double, 2, 2> DK;
-    const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
+  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
-    const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
-    const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
 
-    Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
+  // Derivative for calibration
-    Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
+  if (H1)
-        y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
+    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 
-    *H2 = DK * DR;
+  // Derivative for points
-  }
+  if (H2)
+    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
 
-  return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
+  // Regular uncalibrate after distortion
+  return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
@@ -118,14 +139,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
-  for ( iteration = 0; iteration < maxIterations; ++iteration ) {
+  for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if ( uncalibrate(pn).distance(pi) <= tol ) break;
+    if (uncalibrate(pn).distance(pi) <= tol) break;
-    const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
+    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
     const double rr = xx + yy;
-    const double g = (1+k1_*rr+k2_*rr*rr);
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
-    const double dx = 2*k3_*xy + k4_*(rr+2*xx);
+    const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-    const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+    const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-    pn = (invKPi - Point2(dx,dy))/g;
+    pn = (invKPi - Point2(dx, dy)) / g;
   }
 
   if ( iteration >= maxIterations )
@@ -136,47 +157,28 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
 
 /* ************************************************************************* */
 Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
-  //const double fx = fx_, fy = fy_, s = s_;
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
-  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
-  //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
-  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
   const double rr = xx + yy;
-  const double dr_dx = 2*x;
+  const double r4 = rr * rr;
-  const double dr_dy = 2*y;
+  const double g = (1 + k1_ * rr + k2_ * r4);
-  const double r4 = rr*rr;
+  Eigen::Matrix<double, 2, 2> DK;
-  const double g = 1 + k1*rr + k2*r4;
+  DK << fx_, s_, 0.0, fy_;
-  const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
+  return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
-  const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
-
-  // Dx = 2*k3*xy + k4*(rr+2*xx);
-  // Dy = 2*k4*xy + k3*(rr+2*yy);
-  const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
-  const double dDx_dy = 2*k3*x + k4*dr_dy;
-  const double dDy_dx = 2*k4*y + k3*dr_dx;
-  const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
-
-  Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
-  Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx,     x*dg_dy + dDx_dy,
-             y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
-
-  return DK * DR;
 }
 
 /* ************************************************************************* */
 Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
-  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
   const double rr = xx + yy;
-  const double r4 = rr*rr;
+  const double r4 = rr * rr;
-  const double fx = fx_, fy = fy_, s = s_;
+  const double g = (1 + k1_ * rr + k2_ * r4);
-  const double g = (1+k1_*rr+k2_*r4);
+  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-  const double dx = 2*k3_*xy + k4_*(rr+2*xx);
+  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-  const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+  const double pnx = g * x + dx;
-  const double pnx = g*x + dx;
+  const double pny = g * y + dy;
-  const double pny = g*y + dy;
+  Eigen::Matrix<double, 2, 2> DK;
-
+  DK << fx_, s_, 0.0, fy_;
-  return (Matrix(2, 9) <<
+  return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
-  pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
-  0.0, pny, 0.0, 0.0, 1.0, fy*y*rr      , fy*y*r4         , fy*(rr+2*yy)         , fy*(2*xy)  );
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3DS2.h

@@ -27,6 +27,15 @@ namespace gtsam {
  * @brief Calibration of a camera with radial distortion
  * @addtogroup geometry
  * \nosubgrouping
+ *
+ * Uses same distortionmodel as OpenCV, with
+ * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+ * but using only k1,k2,p1, and p2 coefficients.
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * pi = K*pn
  */
 class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
 
@@ -34,28 +43,22 @@ protected:
 
   double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
   double k1_, k2_ ; // radial 2nd-order and 4th-order
-  double k3_, k4_ ; // tangential distortion
+  double p1_, p2_ ; // tangential distortion
-
-  // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
-  // rr = Pn.x^2 + Pn.y^2
-  // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
-  //                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
-  // pi = K*pn
 
 public:
   Matrix K() const ;
-  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
+  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
   Vector vector() const ;
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
+  Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
 
   Cal3DS2(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
+      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
+  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
 
   /// @}
   /// @name Advanced Constructors
@@ -92,18 +95,30 @@ public:
   /// image center in y
   inline double py() const { return v0_;}
 
+  /// First distortion coefficient
+  inline double k1() const { return k1_;}
+
+  /// Second distortion coefficient
+  inline double k2() const { return k2_;}
+
+  /// First tangential distortion coefficient
+  inline double p1() const { return p1_;}
+
+  /// Second tangential distortion coefficient
+  inline double p2() const { return p2_;}
+
   /**
-   * convert intrinsic coordinates xy to image coordinates uv
+   * convert intrinsic coordinates xy to (distorted) image coordinates uv
    * @param p point in intrinsic coordinates
    * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
+   * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p,
       boost::optional<Matrix&> Dcal = boost::none,
       boost::optional<Matrix&> Dp = boost::none) const ;
 
-  /// Conver a pixel coordinate to ideal coordinate
+  /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
@@ -148,8 +163,8 @@ private:
     ar & BOOST_SERIALIZATION_NVP(v0_);
     ar & BOOST_SERIALIZATION_NVP(k1_);
     ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(k3_);
+    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(k4_);
+    ar & BOOST_SERIALIZATION_NVP(p2_);
   }
 
 

gtsam/geometry/Cal3Unified.cpp

@@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const {
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
   if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
       fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
+      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
       fabs(xi_ - K.xi_) > tol)
     return false;
   return true;

gtsam/geometry/Cal3Unified.h

@@ -31,6 +31,14 @@ namespace gtsam {
  * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
  * @addtogroup geometry
  * \nosubgrouping
+ *
+ * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
 
@@ -41,13 +49,6 @@ private:
 
   double xi_;  // mirror parameter
 
-  // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
-  // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
-  // rr = Pn.x^2 + Pn.y^2
-  // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
-  //                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
-  // pi = K*pn
-
 public:
   //Matrix K() const ;
   //Eigen::Vector4d k() const { return Base::k(); }
@@ -60,8 +61,8 @@ public:
   Cal3Unified() : Base(), xi_(0) {}
 
   Cal3Unified(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
+      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
-        Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
+        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
   /// @}
   /// @name Advanced Constructors

gtsam/geometry/tests/testCal3DS2.cpp

@@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1)
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical,computed,1e-5));
+  Matrix separate = K.D2d_calibration(p);
+  CHECK(assert_equal(numerical,separate,1e-5));
 }
 
 /* ************************************************************************* */
@@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2)
   Matrix computed; K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical,computed,1e-5));
+  Matrix separate = K.D2d_intrinsic(p);
+  CHECK(assert_equal(numerical,separate,1e-5));
 }
 
 /* ************************************************************************* */

gtsam/inference/Ordering.h

@@ -17,9 +17,11 @@
 
 #pragma once
 
+#include <algorithm>
 #include <vector>
 #include <boost/assign/list_inserter.hpp>
 
+#include <gtsam/base/FastSet.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/FactorGraph.h>
@@ -135,6 +137,15 @@ namespace gtsam {
     static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
       const FastMap<Key, int>& groups);
 
+    /// Return a natural Ordering. Typically used by iterative solvers
+    template <class FACTOR>
+    static Ordering Natural(const FactorGraph<FACTOR> &fg) {
+      FastSet<Key> src = fg.keys();
+      std::vector<Key> keys(src.begin(), src.end());
+      std::stable_sort(keys.begin(), keys.end());
+      return Ordering(keys);
+    }
+
     /// @}
 
     /// @name Testable @{

gtsam/linear/ConjugateGradientSolver.cpp

@@ -0,0 +1,49 @@
+/*
+ * ConjugateGradientSolver.cpp
+ *
+ *  Created on: Jun 4, 2014
+ *      Author: ydjian
+ */
+
+#include <gtsam/linear/ConjugateGradientSolver.h>
+#include <boost/algorithm/string.hpp>
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+
+/*****************************************************************************/
+void ConjugateGradientParameters::print(ostream &os) const {
+   Base::print(os);
+   cout << "ConjugateGradientParameters" << endl
+        << "minIter:       " << minIterations_ << endl
+        << "maxIter:       " << maxIterations_ << endl
+        << "resetIter:     " << reset_ << endl
+        << "eps_rel:       " << epsilon_rel_ << endl
+        << "eps_abs:       " << epsilon_abs_ << endl;
+}
+
+/*****************************************************************************/
+std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) {
+  std::string s;
+  switch (value) {
+  case ConjugateGradientParameters::GTSAM:      s = "GTSAM" ;      break;
+  default:                                      s = "UNDEFINED" ;  break;
+  }
+  return s;
+}
+
+/*****************************************************************************/
+ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "GTSAM")  return ConjugateGradientParameters::GTSAM;
+
+  /* default is SBM */
+  return ConjugateGradientParameters::GTSAM;
+}
+
+
+}
+
+

gtsam/linear/ConjugateGradientSolver.h

@@ -12,14 +12,15 @@
 #pragma once
 
 #include <gtsam/linear/IterativeSolver.h>
+#include <iosfwd>
 
 namespace gtsam {
 
 /**
  * parameters for the conjugate gradient method
  */
+class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {
 
-class ConjugateGradientParameters : public IterativeOptimizationParameters {
 public:
   typedef IterativeOptimizationParameters Base;
   typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
@@ -30,14 +31,23 @@ public:
   double epsilon_rel_;    ///< threshold for relative error decrease
   double epsilon_abs_;    ///< threshold for absolute error decrease
 
-  ConjugateGradientParameters()
+  /* Matrix Operation Kernel */
-  : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3){}
+  enum BLASKernel {
+    GTSAM = 0,        ///< Jacobian Factor Graph of GTSAM
+  } blas_kernel_ ;
 
-  ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, double epsilon_rel, double epsilon_abs)
+  ConjugateGradientParameters()
-    : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs){}
+    : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
+      epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
+
+  ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
+    double epsilon_rel, double epsilon_abs, BLASKernel blas)
+    : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
+      epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
 
   ConjugateGradientParameters(const ConjugateGradientParameters &p)
-    : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_) {}
+    : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
+               epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
 
   /* general interface */
   inline size_t minIterations() const { return minIterations_; }
@@ -61,15 +71,81 @@ public:
   inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
   inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
 
-  virtual void print() const {
+
-    Base::print();
+  void print() const { Base::print(); }
-    std::cout << "ConjugateGradientParameters" << std::endl
+  virtual void print(std::ostream &os) const;
-              << "minIter:       " << minIterations_ << std::endl
+
-              << "maxIter:       " << maxIterations_ << std::endl
+  static std::string blasTranslator(const BLASKernel k) ;
-              << "resetIter:     " << reset_ << std::endl
+  static BLASKernel blasTranslator(const std::string &s) ;
-              << "eps_rel:       " << epsilon_rel_ << std::endl
-              << "eps_abs:       " << epsilon_abs_ << std::endl;
-  }
 };
 
+/*********************************************************************************************/
+/*
+ * A template of linear preconditioned conjugate gradient method.
+ * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v,
+ * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
+ * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
+ */
+template <class S, class V>
+V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
+
+  V estimate, residual, direction, q1, q2;
+  estimate = residual = direction = q1 = q2 = initial;
+
+  system.residual(estimate, q1);                /* q1 = b-Ax */
+  system.leftPrecondition(q1, residual);        /* r = S^{-T} (b-Ax) */
+  system.rightPrecondition(residual, direction);/* d = S^{-1} r */
+
+  double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
+
+  const size_t iMaxIterations = parameters.maxIterations(),
+               iMinIterations = parameters.minIterations(),
+               iReset = parameters.reset() ;
+  const double threshold = std::max(parameters.epsilon_abs(),
+                                    parameters.epsilon() * parameters.epsilon() * currentGamma);
+
+  if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
+    std::cout << "[PCG] epsilon = " << parameters.epsilon()
+             << ", max = " << parameters.maxIterations()
+             << ", reset = " << parameters.reset()
+             << ", ||r0||^2 = " << currentGamma
+             << ", threshold = " << threshold << std::endl;
+
+  size_t k;
+  for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
+
+    if ( k % iReset == 0 ) {
+      system.residual(estimate, q1);                      /* q1 = b-Ax */
+      system.leftPrecondition(q1, residual);              /* r = S^{-T} (b-Ax) */
+      system.rightPrecondition(residual, direction);      /* d = S^{-1} r */
+      currentGamma = system.dot(residual, residual);
+    }
+    system.multiply(direction, q1);                       /* q1 = A d */
+    alpha = currentGamma / system.dot(direction, q1);     /* alpha = gamma / (d' A d) */
+    system.axpy(alpha, direction, estimate);              /* estimate += alpha * direction */
+    system.leftPrecondition(q1, q2);                      /* q2 = S^{-T} * q1 */
+    system.axpy(-alpha, q2, residual);                    /* residual -= alpha * q2 */
+    prevGamma = currentGamma;
+    currentGamma = system.dot(residual, residual);        /* gamma = |residual|^2 */
+    beta = currentGamma / prevGamma;
+    system.rightPrecondition(residual, q1);               /* q1 = S^{-1} residual */
+    system.scal(beta, direction);
+    system.axpy(1.0, q1, direction);                      /* direction = q1 + beta * direction */
+
+    if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
+       std::cout << "[PCG] k = " << k
+                 << ", alpha = " << alpha
+                 << ", beta = " << beta
+                 << ", ||r||^2 = " << currentGamma
+                 << std::endl;
+  }
+  if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
+     std::cout << "[PCG] iterations = " << k
+               << ", ||r||^2 = " << currentGamma
+               << std::endl;
+
+  return estimate;
+}
+
+
 }

gtsam/linear/GaussianFactorGraph.cpp

@@ -54,6 +54,20 @@ namespace gtsam {
     return keys;
   }
 
+  /* ************************************************************************* */
+  std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
+    map<Key, size_t> spec;
+    BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) {
+      for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) {
+        map<Key,size_t>::iterator it2 = spec.find(*it);
+        if ( it2 == spec.end() ) {
+          spec.insert(make_pair(*it, gf->getDim(it)));
+        }
+      }
+    }
+    return spec;
+  }
+
   /* ************************************************************************* */
     vector<size_t> GaussianFactorGraph::getkeydim() const {
       // First find dimensions of each variable

gtsam/linear/GaussianFactorGraph.h

@@ -138,6 +138,9 @@ namespace gtsam {
     typedef FastSet<Key> Keys;
     Keys keys() const;
 
+    /* return a map of (Key, dimension) */
+    std::map<Key, size_t> getKeyDimMap() const;
+
     std::vector<size_t> getkeydim() const;
 
     /** unnormalized error */

gtsam/linear/IterativeSolver.cpp

@@ -6,25 +6,42 @@
  */
 
 #include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
 #include <boost/algorithm/string.hpp>
+#include <iostream>
 #include <string>
 
+using namespace std;
+
 namespace gtsam {
 
-std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); }
+/*****************************************************************************/
-std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
+string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
-void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); }
-void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
 
-IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) {
+/*****************************************************************************/
-  std::string s = src;  boost::algorithm::to_upper(s);
+void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); }
-  if (s == "CG") return IterativeOptimizationParameters::CG;
+
-  /* default is cg */
+/*****************************************************************************/
-  else return IterativeOptimizationParameters::CG;
+void IterativeOptimizationParameters::print() const {
+  print(cout);
 }
 
-IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src)  {
+/*****************************************************************************/
-  std::string s = src;  boost::algorithm::to_upper(s);
+void IterativeOptimizationParameters::print(ostream &os) const {
+  os << "IterativeOptimizationParameters:" <<  endl
+     << "verbosity:     " << verbosityTranslator(verbosity_) <<  endl;
+}
+
+/*****************************************************************************/
+ ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
+  p.print(os);
+  return os;
+}
+
+/*****************************************************************************/
+IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const  string &src)  {
+   string s = src;  boost::algorithm::to_upper(s);
   if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
   else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
   else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
@@ -32,18 +49,85 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb
   else return IterativeOptimizationParameters::SILENT;
 }
 
-std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k)  {
+/*****************************************************************************/
-  if ( k == CG ) return "CG";
+ string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity)  {
-  else return "UNKNOWN";
-}
-
-std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity)  {
   if (verbosity == SILENT) return "SILENT";
   else if (verbosity == COMPLEXITY) return "COMPLEXITY";
   else if (verbosity == ERROR) return "ERROR";
   else return "UNKNOWN";
 }
 
+/*****************************************************************************/
+VectorValues IterativeSolver::optimize(
+    const GaussianFactorGraph &gfg,
+    boost::optional<const KeyInfo&> keyInfo,
+    boost::optional<const std::map<Key, Vector>&> lambda)
+{
+  return optimize(
+           gfg,
+           keyInfo ? *keyInfo : KeyInfo(gfg),
+           lambda ? *lambda : std::map<Key,Vector>());
+}
+
+/*****************************************************************************/
+VectorValues IterativeSolver::optimize (
+  const GaussianFactorGraph &gfg,
+  const KeyInfo &keyInfo,
+  const std::map<Key, Vector> &lambda)
+{
+  return optimize(gfg, keyInfo, lambda, keyInfo.x0());
+}
+
+/****************************************************************************/
+KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
+  : ordering_(ordering) {
+  initialize(fg);
+}
+
+/****************************************************************************/
+KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
+  : ordering_(Ordering::Natural(fg)) {
+  initialize(fg);
+}
+
+/****************************************************************************/
+void KeyInfo::initialize(const GaussianFactorGraph &fg){
+  const map<Key, size_t> colspec = fg.getKeyDimMap();
+  const size_t n = ordering_.size();
+  size_t start = 0;
+
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t key = ordering_[i];
+    const size_t dim = colspec.find(key)->second;
+    insert(make_pair(key, KeyInfoEntry(i, dim, start)));
+    start += dim ;
+  }
+  numCols_ = start;
+}
+
+/****************************************************************************/
+vector<size_t> KeyInfo::colSpec() const {
+  std::vector<size_t> result(size(), 0);
+  BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
+    result[item.second.index()] = item.second.dim();
+  }
+  return result;
+}
+
+/****************************************************************************/
+VectorValues KeyInfo::x0() const {
+  VectorValues result;
+  BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
+    result.insert(item.first, Vector::Zero(item.second.dim()));
+  }
+  return result;
+}
+
+/****************************************************************************/
+Vector KeyInfo::x0vector() const {
+  return Vector::Zero(numCols_);
+}
+
 
 }
 

gtsam/linear/IterativeSolver.h

@@ -11,17 +11,27 @@
 
 #pragma once
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/global_includes.h>
-
+#include <gtsam/inference/Ordering.h>
+#include <boost/tuple/tuple.hpp>
+#include <boost/optional/optional.hpp>
+#include <boost/none.hpp>
+#include <iosfwd>
+#include <map>
 #include <string>
-#include <iostream>
+#include <vector>
 
 namespace gtsam {
 
   // Forward declarations
-  class VectorValues;
+  class KeyInfo;
+  class KeyInfoEntry;
   class GaussianFactorGraph;
+  class Values;
+  class VectorValues;
 
+  /************************************************************************************/
   /**
    * parameters for iterative linear solvers
    */
@@ -30,55 +40,99 @@ namespace gtsam {
   public:
 
     typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
-    enum Kernel { CG = 0 } kernel_ ;                                          ///< Iterative Method Kernel
     enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_;
 
   public:
 
-    IterativeOptimizationParameters(const IterativeOptimizationParameters &p)
+    IterativeOptimizationParameters(Verbosity v = SILENT)
-      : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
+      : verbosity_(v) {}
-
-    IterativeOptimizationParameters(const Kernel kernel = CG, const Verbosity verbosity = SILENT)
-      : kernel_(kernel), verbosity_(verbosity) {}
 
     virtual ~IterativeOptimizationParameters() {}
 
-    /* general interface */
+    /* utility */
-    inline Kernel kernel() const { return kernel_; }
     inline Verbosity verbosity() const { return verbosity_; }
-
-    /* matlab interface */
-    std::string getKernel() const ;
     std::string getVerbosity() const;
-    void setKernel(const std::string &s) ;
     void setVerbosity(const std::string &s) ;
 
-    virtual void print() const {
+    /* matlab interface */
-      std::cout << "IterativeOptimizationParameters" << std::endl
+    void print() const ;
-                << "kernel:        " << kernelTranslator(kernel_) << std::endl
+
-                << "verbosity:     " << verbosityTranslator(verbosity_) << std::endl;
+    /* virtual print function */
-    }
+    virtual void print(std::ostream &os) const ;
+
+    /* for serialization */
+    friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p);
 
-    static Kernel kernelTranslator(const std::string &s);
     static Verbosity verbosityTranslator(const std::string &s);
-    static std::string kernelTranslator(Kernel k);
     static std::string verbosityTranslator(Verbosity v);
   };
 
+  /************************************************************************************/
   class GTSAM_EXPORT IterativeSolver {
   public:
     typedef boost::shared_ptr<IterativeSolver> shared_ptr;
     IterativeSolver() {}
     virtual ~IterativeSolver() {}
 
-    /* interface to the nonlinear optimizer  */
+    /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
-    virtual VectorValues optimize () = 0;
+    VectorValues optimize (
+      const GaussianFactorGraph &gfg,
+      boost::optional<const KeyInfo&> = boost::none,
+      boost::optional<const std::map<Key, Vector>&> lambda = boost::none
+    );
 
-    /* interface to the nonlinear optimizer  */
+    /* interface to the nonlinear optimizer, without initial estimate */
-    virtual VectorValues optimize (const VectorValues &initial) = 0;
+    VectorValues optimize (
+      const GaussianFactorGraph &gfg,
+      const KeyInfo &keyInfo,
+      const std::map<Key, Vector> &lambda
+    );
+
+    /* interface to the nonlinear optimizer that the subclasses have to implement */
+    virtual VectorValues optimize (
+      const GaussianFactorGraph &gfg,
+      const KeyInfo &keyInfo,
+      const std::map<Key, Vector> &lambda,
+      const VectorValues &initial
+    ) = 0;
 
-    /* update interface to the nonlinear optimizer  */
-    virtual void replaceFactors(const boost::shared_ptr<GaussianFactorGraph> &factorGraph, const double lambda) {}
   };
 
+  /************************************************************************************/
+  /* Handy data structure for iterative solvers
+   * key to (index, dimension, colstart) */
+  class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
+  public:
+    typedef boost::tuple<Key,size_t,Key> Base;
+    KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
+    const size_t index() const { return this->get<0>(); }
+    const size_t dim() const { return this->get<1>(); }
+    const size_t colstart() const { return this->get<2>(); }
+  };
+
+  /************************************************************************************/
+  class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> {
+  public:
+    typedef std::map<Key, KeyInfoEntry> Base;
+    KeyInfo() : numCols_(0) {}
+    KeyInfo(const GaussianFactorGraph &fg);
+    KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
+
+    std::vector<size_t> colSpec() const ;
+    VectorValues x0() const;
+    Vector x0vector() const;
+
+    inline size_t numCols() const { return numCols_; }
+    inline const Ordering & ordering() const { return ordering_; }
+
+  protected:
+
+    void initialize(const GaussianFactorGraph &fg);
+
+    Ordering ordering_;
+    size_t numCols_;
+
+  };
+
+
 }

gtsam/linear/PCGSolver.cpp

@@ -0,0 +1,201 @@
+/*
+ * PCGSolver.cpp
+ *
+ *  Created on: Feb 14, 2012
+ *      Author: ydjian
+ */
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+//#include <gtsam/inference/FactorGraph-inst.h>
+//#include <gtsam/linear/FactorGraphUtil-inl.h>
+//#include <gtsam/linear/JacobianFactorGraph.h>
+//#include <gtsam/linear/LSPCGSolver.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/Preconditioner.h>
+//#include <gtsam/linear/SuiteSparseUtil.h>
+//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
+//#include <gsp2/gtsam-interface-sbm.h>
+//#include <ydjian/tool/ThreadSafeTimer.h>
+#include <boost/algorithm/string.hpp>
+#include <iostream>
+#include <stdexcept>
+
+using namespace std;
+
+namespace gtsam {
+
+/*****************************************************************************/
+void PCGSolverParameters::print(ostream &os) const {
+  Base::print(os);
+  os << "PCGSolverParameters:" <<  endl;
+  preconditioner_->print(os);
+}
+
+/*****************************************************************************/
+PCGSolver::PCGSolver(const PCGSolverParameters &p) {
+  preconditioner_ = createPreconditioner(p.preconditioner_);
+}
+
+/*****************************************************************************/
+VectorValues PCGSolver::optimize (
+  const GaussianFactorGraph &gfg,
+  const KeyInfo &keyInfo,
+  const std::map<Key, Vector> &lambda,
+  const VectorValues &initial)
+{
+  /* build preconditioner */
+  preconditioner_->build(gfg, keyInfo, lambda);
+
+  /* apply pcg */
+  const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
+        GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
+        initial.vector(keyInfo.ordering()), parameters_);
+
+  return buildVectorValues(sol, keyInfo);
+}
+
+/*****************************************************************************/
+GaussianFactorGraphSystem::GaussianFactorGraphSystem(
+    const GaussianFactorGraph &gfg,
+    const Preconditioner &preconditioner,
+    const KeyInfo &keyInfo,
+    const std::map<Key, Vector> &lambda)
+  : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}
+
+/*****************************************************************************/
+void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
+  /* implement b-Ax, assume x and r are pre-allocated */
+
+  /* reset r to b */
+  getb(r);
+
+  /* substract A*x */
+  Vector Ax = Vector::Zero(r.rows(), 1);
+  multiply(x, Ax);
+  r -= Ax ;
+}
+
+/*****************************************************************************/
+void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
+  /* implement Ax, assume x and Ax are pre-allocated */
+
+  /* reset y */
+  Ax.setZero();
+
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
+    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+      /* accumulate At A x*/
+      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+        const Matrix Ai = jf->getA(it);
+        /* this map lookup should be replaced */
+        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
+        Ax.segment(entry.colstart(), entry.dim())
+            += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
+      }
+    }
+    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+      /* accumulate H x */
+
+      /* use buffer to avoid excessive table lookups */
+      const size_t sz = hf->size();
+      vector<Vector> y;
+      y.reserve(sz);
+      for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
+        /* initialize y to zeros */
+        y.push_back(zero(hf->getDim(it)));
+      }
+
+      for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
+        /* retrieve the key mapping */
+        const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
+        // xj is the input vector
+        const Vector xj = x.segment(entry.colstart(), entry.dim());
+        size_t idx = 0;
+        for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
+          if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
+          else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
+        }
+      }
+
+      /* accumulate to r */
+      for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
+        /* retrieve the key mapping */
+        const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
+        Ax.segment(entry.colstart(), entry.dim()) += y[i];
+      }
+    }
+    else {
+      throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+  }
+}
+
+/*****************************************************************************/
+void GaussianFactorGraphSystem::getb(Vector &b) const {
+  /* compute rhs, assume b pre-allocated */
+
+  /* reset */
+  b.setZero();
+
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
+    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+      const Vector rhs = jf->getb();
+      /* accumulate At rhs */
+      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+        /* this map lookup should be replaced */
+        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
+        b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
+      }
+    }
+    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+      /* accumulate g */
+      for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
+        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
+        b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
+      }
+    }
+    else {
+      throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+  }
+}
+
+/**********************************************************************************/
+void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
+{ preconditioner_.transposeSolve(x, y); }
+
+/**********************************************************************************/
+void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
+{ preconditioner_.solve(x, y); }
+
+/**********************************************************************************/
+VectorValues buildVectorValues(const Vector &v,
+                               const Ordering &ordering,
+                               const map<Key, size_t>  & dimensions) {
+  VectorValues result;
+
+  DenseIndex offset = 0;
+  for ( size_t i = 0 ; i < ordering.size() ; ++i ) {
+    const Key &key = ordering[i];
+    map<Key, size_t>::const_iterator it = dimensions.find(key);
+    if ( it == dimensions.end() ) {
+      throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
+    }
+    const size_t dim = it->second;
+    result.insert(key, v.segment(offset, dim));
+    offset += dim;
+  }
+
+  return result;
+}
+
+/**********************************************************************************/
+VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
+  VectorValues result;
+  BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
+    result.insert(item.first, v.segment(item.second.colstart(), item.second.dim()));
+  }
+  return result;
+}
+
+}

gtsam/linear/PCGSolver.h

@@ -0,0 +1,109 @@
+/*
+ * PCGSolver.h
+ *
+ *  Created on: Jan 31, 2012
+ *  Author: Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/ConjugateGradientSolver.h>
+#include <gtsam/linear/VectorValues.h>
+#include <boost/shared_ptr.hpp>
+
+#include <algorithm>
+#include <iosfwd>
+#include <map>
+#include <string>
+
+namespace gtsam {
+
+class GaussianFactorGraph;
+class KeyInfo;
+class Preconditioner;
+struct PreconditionerParameters;
+
+/*****************************************************************************/
+struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
+public:
+  typedef ConjugateGradientParameters Base;
+  typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
+
+  PCGSolverParameters() {}
+
+  virtual void print(std::ostream &os) const;
+
+  /* interface to preconditioner parameters */
+  inline const PreconditionerParameters& preconditioner() const {
+    return *preconditioner_;
+  }
+
+  boost::shared_ptr<PreconditionerParameters> preconditioner_;
+};
+
+/*****************************************************************************/
+/* A virtual base class for the preconditioned conjugate gradient solver */
+class GTSAM_EXPORT PCGSolver: public IterativeSolver {
+public:
+  typedef IterativeSolver Base;
+  typedef boost::shared_ptr<PCGSolver> shared_ptr;
+
+protected:
+
+  PCGSolverParameters parameters_;
+  boost::shared_ptr<Preconditioner> preconditioner_;
+
+public:
+  /* Interface to initialize a solver without a problem */
+  PCGSolver(const PCGSolverParameters &p);
+  virtual ~PCGSolver() {}
+
+  using IterativeSolver::optimize;
+
+  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
+      const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
+      const VectorValues &initial);
+
+};
+
+/*****************************************************************************/
+class GTSAM_EXPORT GaussianFactorGraphSystem {
+public:
+
+  GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
+      const Preconditioner &preconditioner, const KeyInfo &info,
+      const std::map<Key, Vector> &lambda);
+
+  const GaussianFactorGraph &gfg_;
+  const Preconditioner &preconditioner_;
+  const KeyInfo &keyInfo_;
+  const std::map<Key, Vector> &lambda_;
+
+  void residual(const Vector &x, Vector &r) const;
+  void multiply(const Vector &x, Vector& y) const;
+  void leftPrecondition(const Vector &x, Vector &y) const;
+  void rightPrecondition(const Vector &x, Vector &y) const;
+  inline void scal(const double alpha, Vector &x) const {
+    x *= alpha;
+  }
+  inline double dot(const Vector &x, const Vector &y) const {
+    return x.dot(y);
+  }
+  inline void axpy(const double alpha, const Vector &x, Vector &y) const {
+    y += alpha * x;
+  }
+
+  void getb(Vector &b) const;
+};
+
+/* utility functions */
+/**********************************************************************************/
+VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
+    const std::map<Key, size_t> & dimensions);
+
+/**********************************************************************************/
+VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
+
+}
+

gtsam/linear/Preconditioner.cpp

@@ -0,0 +1,214 @@
+/*
+ * Preconditioner.cpp
+ *
+ *  Created on: Jun 2, 2014
+ *      Author: ydjian
+ */
+
+#include <gtsam/inference/FactorGraph-inst.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <boost/shared_ptr.hpp>
+#include <boost/algorithm/string.hpp>
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+namespace gtsam {
+
+/*****************************************************************************/
+void PreconditionerParameters::print() const {
+  print(cout);
+}
+
+/***************************************************************************************/
+void PreconditionerParameters::print(ostream &os) const {
+  os << "PreconditionerParameters" << endl
+     << "kernel:        " << kernelTranslator(kernel_) << endl
+     << "verbosity:     " << verbosityTranslator(verbosity_) << endl;
+}
+
+/*****************************************************************************/
+ ostream& operator<<(ostream &os, const PreconditionerParameters &p) {
+  p.print(os);
+  return os;
+}
+
+/***************************************************************************************/
+PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "GTSAM") return PreconditionerParameters::GTSAM;
+  else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD;
+  /* default is cholmod */
+  else return PreconditionerParameters::CHOLMOD;
+}
+
+/***************************************************************************************/
+PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src)  {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "SILENT") return PreconditionerParameters::SILENT;
+  else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY;
+  else if (s == "ERROR") return PreconditionerParameters::ERROR;
+  /* default is default */
+  else return PreconditionerParameters::SILENT;
+}
+
+/***************************************************************************************/
+std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k)  {
+  if ( k == GTSAM ) return "GTSAM";
+  if ( k == CHOLMOD ) return "CHOLMOD";
+  else return "UNKNOWN";
+}
+
+/***************************************************************************************/
+std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity)  {
+  if (verbosity == SILENT)          return "SILENT";
+  else if (verbosity == COMPLEXITY) return "COMPLEXITY";
+  else if (verbosity == ERROR)      return "ERROR";
+  else return "UNKNOWN";
+}
+
+/***************************************************************************************/
+BlockJacobiPreconditioner::BlockJacobiPreconditioner()
+  : Base(), buffer_(0), bufferSize_(0), nnz_(0) {}
+
+/***************************************************************************************/
+BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); }
+
+/***************************************************************************************/
+void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
+
+  const size_t n = dims_.size();
+  double *ptr = buffer_, *dst = x.data();
+
+  std::copy(y.data(), y.data() + y.rows(), x.data());
+
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t d = dims_[i];
+    const size_t sz = d*d;
+
+    const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
+    Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
+    R.triangularView<Eigen::Upper>().solveInPlace(b);
+
+    dst += d;
+    ptr += sz;
+  }
+}
+
+/***************************************************************************************/
+void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const {
+  const size_t n = dims_.size();
+  double *ptr = buffer_, *dst = x.data();
+
+  std::copy(y.data(), y.data() + y.rows(), x.data());
+
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t d = dims_[i];
+    const size_t sz = d*d;
+
+    const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
+    Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
+    R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);
+
+    dst += d;
+    ptr += sz;
+  }
+}
+
+/***************************************************************************************/
+void BlockJacobiPreconditioner::build(
+  const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
+{
+  const size_t n = keyInfo.size();
+  dims_ = keyInfo.colSpec();
+
+  /* prepare the buffer of block diagonals */
+  std::vector<Matrix> blocks; blocks.reserve(n);
+
+  /* allocate memory for the factorization of block diagonals */
+  size_t nnz = 0;
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t dim = dims_[i];
+    blocks.push_back(Matrix::Zero(dim, dim));
+    // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
+    nnz += dim*dim;
+  }
+
+  /* compute the block diagonal by scanning over the factors */
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Ai = jf->getA(it);
+        blocks[entry.index()] += (Ai.transpose() * Ai);
+      }
+    }
+    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Hii = hf->info(it, it).selfadjointView();
+        blocks[entry.index()] += Hii;
+      }
+    }
+    else {
+      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+  }
+
+  /* if necessary, allocating the memory for cacheing the factorization results */
+  if ( nnz > bufferSize_ ) {
+    clean();
+    buffer_ = new double [nnz];
+    bufferSize_ = nnz;
+  }
+  nnz_ = nnz;
+
+  /* factorizing the blocks respectively */
+  double *ptr = buffer_;
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    /* use eigen to decompose Di */
+    const Matrix R = blocks[i].llt().matrixL().transpose();
+
+    /* store the data in the buffer */
+    size_t sz = dims_[i]*dims_[i] ;
+    std::copy(R.data(), R.data() + sz, ptr);
+
+    /* advance the pointer */
+    ptr += sz;
+  }
+}
+
+/*****************************************************************************/
+void BlockJacobiPreconditioner::clean() {
+  if ( buffer_ ) {
+    delete [] buffer_;
+    buffer_ = 0;
+    bufferSize_ = 0;
+    nnz_ = 0;
+  }
+}
+
+/***************************************************************************************/
+boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
+
+  if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
+    return boost::make_shared<DummyPreconditioner>();
+  }
+  else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
+    return boost::make_shared<BlockJacobiPreconditioner>();
+  }
+  else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
+    return boost::make_shared<SubgraphPreconditioner>(*subgraph);
+  }
+
+  throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
+}
+
+}
+
+

gtsam/linear/Preconditioner.h

@@ -0,0 +1,170 @@
+/*
+ * Preconditioner.h
+ *
+ *  Created on: Jun 2, 2014
+ *      Author: ydjian
+ */
+
+#pragma once
+
+#include <gtsam/base/Vector.h>
+#include <boost/shared_ptr.hpp>
+#include <iosfwd>
+#include <map>
+#include <string>
+
+namespace gtsam {
+
+class GaussianFactorGraph;
+class KeyInfo;
+class VectorValues;
+
+/* parameters for the preconditioner */
+struct GTSAM_EXPORT PreconditionerParameters {
+
+   typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
+
+   enum Kernel { /* Preconditioner Kernel */
+     GTSAM = 0,
+     CHOLMOD     /* experimental */
+   } kernel_ ;
+
+   enum Verbosity {
+     SILENT = 0,
+     COMPLEXITY = 1,
+     ERROR = 2
+   } verbosity_ ;
+
+   PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {}
+   PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
+   virtual ~PreconditionerParameters() {}
+
+   /* general interface */
+   inline Kernel kernel() const { return kernel_; }
+   inline Verbosity verbosity() const { return verbosity_; }
+
+   void print() const ;
+
+   virtual void print(std::ostream &os) const ;
+
+   static Kernel kernelTranslator(const std::string &s);
+   static Verbosity verbosityTranslator(const std::string &s);
+   static std::string kernelTranslator(Kernel k);
+   static std::string verbosityTranslator(Verbosity v);
+
+   /* for serialization */
+   friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
+ };
+
+/* PCG aims to solve the problem: A x = b by reparametrizing it as
+ * S^t A S y = S^t b   or   M A x = M b, where A \approx S S, or A \approx M
+ * The goal of this class is to provide a general interface to all preconditioners */
+class GTSAM_EXPORT Preconditioner {
+public:
+  typedef boost::shared_ptr<Preconditioner> shared_ptr;
+  typedef std::vector<size_t> Dimensions;
+
+  /* Generic Constructor and Destructor */
+  Preconditioner() {}
+  virtual ~Preconditioner() {}
+
+  /* Computation Interfaces */
+
+  /* implement x = S^{-1} y */
+  virtual void solve(const Vector& y, Vector &x) const = 0;
+//  virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
+
+  /* implement x = S^{-T} y */
+  virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
+//  virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
+
+//  /* implement x = S^{-1} S^{-T} y */
+//  virtual void fullSolve(const Vector& y, Vector &x) const = 0;
+//  virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
+
+  /* build/factorize the preconditioner */
+  virtual void build(
+    const GaussianFactorGraph &gfg,
+    const KeyInfo &info,
+    const std::map<Key,Vector> &lambda
+    ) = 0;
+};
+
+/*******************************************************************************************/
+struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
+  typedef PreconditionerParameters Base;
+  typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
+  DummyPreconditionerParameters() : Base() {}
+  virtual ~DummyPreconditionerParameters() {}
+};
+
+/*******************************************************************************************/
+class GTSAM_EXPORT DummyPreconditioner : public Preconditioner {
+public:
+  typedef Preconditioner Base;
+  typedef boost::shared_ptr<DummyPreconditioner> shared_ptr;
+
+public:
+
+  DummyPreconditioner() : Base() {}
+  virtual ~DummyPreconditioner() {}
+
+  /* Computation Interfaces for raw vector */
+  virtual void solve(const Vector& y, Vector &x) const { x = y; }
+//  virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
+
+  virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
+//  virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
+
+//  virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
+//  virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
+
+  virtual void build(
+    const GaussianFactorGraph &gfg,
+    const KeyInfo &info,
+    const std::map<Key,Vector> &lambda
+    )  {}
+};
+
+/*******************************************************************************************/
+struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters {
+  typedef PreconditionerParameters Base;
+  BlockJacobiPreconditionerParameters() : Base() {}
+  virtual ~BlockJacobiPreconditionerParameters() {}
+};
+
+/*******************************************************************************************/
+class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
+public:
+  typedef Preconditioner Base;
+  BlockJacobiPreconditioner() ;
+  virtual ~BlockJacobiPreconditioner() ;
+
+  /* Computation Interfaces for raw vector */
+  virtual void solve(const Vector& y, Vector &x) const;
+  virtual void transposeSolve(const Vector& y, Vector& x) const ;
+//  virtual void fullSolve(const Vector& y, Vector &x) const ;
+
+  virtual void build(
+    const GaussianFactorGraph &gfg,
+    const KeyInfo &info,
+    const std::map<Key,Vector> &lambda
+    ) ;
+
+protected:
+
+  void clean() ;
+
+  std::vector<size_t> dims_;
+  double *buffer_;
+  size_t bufferSize_;
+  size_t nnz_;
+};
+
+/*********************************************************************************************/
+/* factory method to create preconditioners */
+boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
+
+}
+
+

gtsam/linear/SubgraphPreconditioner.cpp

@@ -15,126 +15,646 @@
  * @author: Frank Dellaert
  */
 
+#include <gtsam/base/DSFVector.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <boost/foreach.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <fstream>
+#include <iostream>
+#include <numeric>
+#include <queue>
+#include <set>
+#include <utility>
+#include <vector>
 
 using namespace std;
 
 namespace gtsam {
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
+static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
-    GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
-    BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
+  BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
-      JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+    JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      if( !jf ) {
+    if( !jf ) {
-        jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+      jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+    }
+    result->push_back(jf);
+  }
+  return result;
+}
+
+/*****************************************************************************/
+static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
+
+  /* compute the sum of the weights */
+  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
+
+  /* make a normalized and accumulated version of the weight vector */
+  const size_t m = weight.size();
+  vector<double> w; w.reserve(m);
+  for ( size_t i = 0 ; i < m ; ++i ) {
+    w.push_back(weight[i]/sum);
+  }
+
+  vector<double> acc(m);
+  std::partial_sum(w.begin(),w.end(),acc.begin());
+
+  /* iid sample n times */
+  vector<size_t> result; result.reserve(n);
+  const double denominator = (double)RAND_MAX;
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const double value = rand() / denominator;
+    /* binary search the interval containing "value" */
+    vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
+    size_t idx = it - acc.begin();
+    result.push_back(idx);
+  }
+  return result;
+}
+
+/*****************************************************************************/
+vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
+
+  const size_t m = weight.size();
+  if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
+
+  vector<size_t> result;
+
+  size_t count = 0;
+  std::vector<bool> touched(m, false);
+  while ( count < n ) {
+    std::vector<size_t> localIndices; localIndices.reserve(n-count);
+    std::vector<double> localWeights; localWeights.reserve(n-count);
+
+    /* collect data */
+    for ( size_t i = 0 ; i < m ; ++i ) {
+      if ( !touched[i] ) {
+        localIndices.push_back(i);
+        localWeights.push_back(weight[i]);
       }
-      result->push_back(jf);
-    }
-    return result;
-  }
-
-  /* ************************************************************************* */
-  SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
-      const sharedBayesNet& Rc1, const sharedValues& xbar) :
-    Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) {
-  }
-
-  /* ************************************************************************* */
-  // x = xbar + inv(R1)*y
-  VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
-    return *xbar_ + Rc1_->backSubstitute(y);
-  }
-
-  /* ************************************************************************* */
-  double SubgraphPreconditioner::error(const VectorValues& y) const {
-    Errors e(y);
-    VectorValues x = this->x(y);
-    Errors e2 = Ab2()->gaussianErrors(x);
-    return 0.5 * (dot(e, e) + dot(e2,e2));
-  }
-
-  /* ************************************************************************* */
-  // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
-  VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
-    VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
-    Errors e = (*Ab2()*x - *b2bar());               /* (A2*inv(R1)*y-b2bar) */
-    VectorValues v = VectorValues::Zero(x);
-    Ab2()->transposeMultiplyAdd(1.0, e, v);           /* A2'*(A2*inv(R1)*y-b2bar) */
-    return y + Rc1()->backSubstituteTranspose(v);
-  }
-
-  /* ************************************************************************* */
-  // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
-  Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
-
-    Errors e(y);
-    VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
-    Errors e2 = *Ab2() * x;                              /* A2*x */
-    e.splice(e.end(), e2);
-    return e;
-  }
-
-  /* ************************************************************************* */
-  // In-place version that overwrites e
-  void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
-
-    Errors::iterator ei = e.begin();
-    for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
-      *ei = y[i];
     }
 
-    // Add A2 contribution
+    /* sampling and cache results */
-    VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
+    vector<size_t> samples = iidSampler(localWeights, n-count);
-    Ab2()->multiplyInPlace(x, ei);                  // use iterator version
+    BOOST_FOREACH ( const size_t &id, samples ) {
-  }
+      if ( touched[id] == false ) {
-
+        touched[id] = true ;
-  /* ************************************************************************* */
+        result.push_back(id);
-  // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
+        if ( ++count >= n ) break;
-  VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
+      }
-
-    Errors::const_iterator it = e.begin();
-    VectorValues y = zero();
-    for ( Key i = 0 ; i < y.size() ; ++i, ++it )
-      y[i] = *it ;
-    transposeMultiplyAdd2(1.0,it,e.end(),y);
-    return y;
-  }
-
-  /* ************************************************************************* */
-  // y += alpha*A'*e
-  void SubgraphPreconditioner::transposeMultiplyAdd
-    (double alpha, const Errors& e, VectorValues& y) const {
-
-    Errors::const_iterator it = e.begin();
-    for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
-      const Vector& ei = *it;
-      axpy(alpha, ei, y[i]);
     }
-    transposeMultiplyAdd2(alpha, it, e.end(), y);
+  }
+  return result;
+}
+
+/****************************************************************************/
+Subgraph::Subgraph(const std::vector<size_t> &indices) {
+  edges_.reserve(indices.size());
+  BOOST_FOREACH ( const size_t &idx, indices ) {
+    edges_.push_back(SubgraphEdge(idx, 1.0));
+  }
+}
+
+/****************************************************************************/
+std::vector<size_t> Subgraph::edgeIndices() const {
+  std::vector<size_t> eid; eid.reserve(size());
+  BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
+    eid.push_back(edge.index_);
+  }
+  return eid;
+}
+
+/****************************************************************************/
+void Subgraph::save(const std::string &fn) const {
+  std::ofstream os(fn.c_str());
+  boost::archive::text_oarchive oa(os);
+  oa << *this;
+  os.close();
+}
+
+/****************************************************************************/
+Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
+  std::ifstream is(fn.c_str());
+  boost::archive::text_iarchive ia(is);
+  Subgraph::shared_ptr subgraph(new Subgraph());
+  ia >> *subgraph;
+  is.close();
+  return subgraph;
+}
+
+/****************************************************************************/
+std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
+  if ( edge.weight() != 1.0 )
+    os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
+  else
+    os << edge.index() ;
+  return os;
+}
+
+/****************************************************************************/
+std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
+  os << "Subgraph" << endl;
+  BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
+    os << e << ", " ;
+  }
+  return os;
+}
+
+/*****************************************************************************/
+void SubgraphBuilderParameters::print() const {
+  print(cout);
+}
+
+/***************************************************************************************/
+void SubgraphBuilderParameters::print(ostream &os) const {
+  os << "SubgraphBuilderParameters" << endl
+      << "skeleton:            " << skeletonTranslator(skeleton_) << endl
+      << "skeleton weight:     " << skeletonWeightTranslator(skeletonWeight_) << endl
+      << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
+      ;
+}
+
+/*****************************************************************************/
+ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
+  p.print(os);
+  return os;
+}
+
+/*****************************************************************************/
+SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "NATURALCHAIN")    return NATURALCHAIN;
+  else if (s == "BFS")        return BFS;
+  else if (s == "KRUSKAL")    return KRUSKAL;
+  throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
+  return KRUSKAL;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
+  if ( w == NATURALCHAIN )return "NATURALCHAIN";
+  else if ( w == BFS )    return "BFS";
+  else if ( w == KRUSKAL )return "KRUSKAL";
+  else                    return "UNKNOWN";
+}
+
+/****************************************************************/
+SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "EQUAL")       return EQUAL;
+  else if (s == "RHS")    return RHS_2NORM;
+  else if (s == "LHS")    return LHS_FNORM;
+  else if (s == "RANDOM") return RANDOM;
+  throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
+  return EQUAL;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
+  if ( w == EQUAL )           return "EQUAL";
+  else if ( w == RHS_2NORM )  return "RHS";
+  else if ( w == LHS_FNORM )  return "LHS";
+  else if ( w == RANDOM )     return "RANDOM";
+  else                        return "UNKNOWN";
+}
+
+/****************************************************************/
+SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "SKELETON")      return SKELETON;
+//  else if (s == "STRETCH")  return STRETCH;
+//  else if (s == "GENERALIZED_STRETCH")  return GENERALIZED_STRETCH;
+  throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
+  return SKELETON;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
+  if ( w == SKELETON )                  return "SKELETON";
+//  else if ( w == STRETCH )              return "STRETCH";
+//  else if ( w == GENERALIZED_STRETCH )  return "GENERALIZED_STRETCH";
+  else                                  return "UNKNOWN";
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
+  const SubgraphBuilderParameters &p = parameters_;
+  switch (p.skeleton_) {
+  case SubgraphBuilderParameters::NATURALCHAIN:
+    return natural_chain(gfg);
+    break;
+  case SubgraphBuilderParameters::BFS:
+    return bfs(gfg);
+    break;
+  case SubgraphBuilderParameters::KRUSKAL:
+    return kruskal(gfg, ordering, w);
+    break;
+  default:
+    cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
+    break;
+  }
+  return vector<size_t>();
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
+  std::vector<size_t> result ;
+  size_t idx = 0;
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+    if ( gf->size() == 1 ) {
+      result.push_back(idx);
+    }
+    idx++;
+  }
+  return result;
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
+  std::vector<size_t> result ;
+  size_t idx = 0;
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+    if ( gf->size() == 2 ) {
+      const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
+      if ( (k1-k0) == 1 || (k0-k1) == 1 )
+        result.push_back(idx);
+    }
+    idx++;
+  }
+  return result;
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
+  const VariableIndex variableIndex(gfg);
+  /* start from the first key of the first factor */
+  Key seed = gfg[0]->keys()[0];
+
+  const size_t n = variableIndex.size();
+
+  /* each vertex has self as the predecessor */
+  std::vector<size_t> result;
+  result.reserve(n-1);
+
+  /* Initialize */
+  std::queue<size_t> q;
+  q.push(seed);
+
+  std::set<size_t> flags;
+  flags.insert(seed);
+
+  /* traversal */
+  while ( !q.empty() ) {
+    const size_t head = q.front(); q.pop();
+    BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
+      const GaussianFactor &gf = *gfg[id];
+      BOOST_FOREACH ( const size_t key, gf.keys() ) {
+        if ( flags.count(key) == 0 ) {
+          q.push(key);
+          flags.insert(key);
+          result.push_back(id);
+        }
+      }
+    }
+  }
+  return result;
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
+  const VariableIndex variableIndex(gfg);
+  const size_t n = variableIndex.size();
+  const vector<size_t> idx = sort_idx(w) ;
+
+  /* initialize buffer */
+  vector<size_t> result;
+  result.reserve(n-1);
+
+  // container for acsendingly sorted edges
+  DSFVector D(n) ;
+
+  size_t count = 0 ; double sum = 0.0 ;
+  BOOST_FOREACH (const size_t id, idx) {
+    const GaussianFactor &gf = *gfg[id];
+    if ( gf.keys().size() != 2 ) continue;
+    const size_t u = ordering.find(gf.keys()[0])->second,
+                 u_root = D.find(u),
+                 v = ordering.find(gf.keys()[1])->second,
+                 v_root = D.find(v) ;
+    if ( u_root != v_root ) {
+      D.merge(u_root, v_root) ;
+      result.push_back(id) ;
+      sum += w[id] ;
+      if ( ++count == n-1 ) break ;
+    }
+  }
+  return result;
+}
+
+/****************************************************************/
+std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
+  return uniqueSampler(weights, t);
+}
+
+/****************************************************************/
+Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
+
+  const SubgraphBuilderParameters &p = parameters_;
+  const Ordering inverse_ordering = Ordering::Natural(gfg);
+  const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
+  const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
+
+  vector<double> w = weights(gfg);
+  const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
+
+  /* sanity check */
+  if ( tree.size() != n-1 ) {
+    throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
   }
 
-  /* ************************************************************************* */
+  /* down weight the tree edges to zero */
-  // y += alpha*inv(R1')*A2'*e2
+  BOOST_FOREACH ( const size_t id, tree ) {
-  void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
+    w[id] = 0.0;
+  }
+
+  /* decide how many edges to augment */
+  std::vector<size_t> offTree = sample(w, t);
+
+  vector<size_t> subgraph = unary(gfg);
+  subgraph.insert(subgraph.end(), tree.begin(), tree.end());
+  subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
+
+  return boost::make_shared<Subgraph>(subgraph);
+}
+
+/****************************************************************/
+SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
+{
+  const size_t m = gfg.size() ;
+  Weights weight; weight.reserve(m);
+
+  BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
+    switch ( parameters_.skeletonWeight_ ) {
+    case SubgraphBuilderParameters::EQUAL:
+      weight.push_back(1.0);
+      break;
+    case SubgraphBuilderParameters::RHS_2NORM:
+    {
+      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+        weight.push_back(jf->getb().norm());
+      }
+      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+        weight.push_back(hf->linearTerm().norm());
+      }
+    }
+      break;
+    case SubgraphBuilderParameters::LHS_FNORM:
+    {
+      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+        weight.push_back(std::sqrt(jf->getA().squaredNorm()));
+      }
+      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+        weight.push_back(std::sqrt(hf->information().squaredNorm()));
+      }
+    }
+      break;
+
+    case SubgraphBuilderParameters::RANDOM:
+      weight.push_back(std::rand()%100 + 1.0);
+      break;
+
+    default:
+      throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
+      break;
+    }
+  }
+  return weight;
+}
+
+/* ************************************************************************* */
+SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
+         parameters_(p) {}
+
+/* ************************************************************************* */
+SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
+    const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
+        Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
+        b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
+}
+
+/* ************************************************************************* */
+// x = xbar + inv(R1)*y
+VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
+  return *xbar_ + Rc1_->backSubstitute(y);
+}
+
+/* ************************************************************************* */
+double SubgraphPreconditioner::error(const VectorValues& y) const {
+  Errors e(y);
+  VectorValues x = this->x(y);
+  Errors e2 = Ab2()->gaussianErrors(x);
+  return 0.5 * (dot(e, e) + dot(e2,e2));
+}
+
+/* ************************************************************************* */
+// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
+VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
+  VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
+  Errors e = (*Ab2()*x - *b2bar());               /* (A2*inv(R1)*y-b2bar) */
+  VectorValues v = VectorValues::Zero(x);
+  Ab2()->transposeMultiplyAdd(1.0, e, v);           /* A2'*(A2*inv(R1)*y-b2bar) */
+  return y + Rc1()->backSubstituteTranspose(v);
+}
+
+/* ************************************************************************* */
+// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
+Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
+
+  Errors e(y);
+  VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
+  Errors e2 = *Ab2() * x;                              /* A2*x */
+  e.splice(e.end(), e2);
+  return e;
+}
+
+/* ************************************************************************* */
+// In-place version that overwrites e
+void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
+
+  Errors::iterator ei = e.begin();
+  for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
+    *ei = y[i];
+  }
+
+  // Add A2 contribution
+  VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
+  Ab2()->multiplyInPlace(x, ei);                  // use iterator version
+}
+
+/* ************************************************************************* */
+// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
+VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
+
+  Errors::const_iterator it = e.begin();
+  VectorValues y = zero();
+  for ( Key i = 0 ; i < y.size() ; ++i, ++it )
+    y[i] = *it ;
+  transposeMultiplyAdd2(1.0,it,e.end(),y);
+  return y;
+}
+
+/* ************************************************************************* */
+// y += alpha*A'*e
+void SubgraphPreconditioner::transposeMultiplyAdd
+(double alpha, const Errors& e, VectorValues& y) const {
+
+  Errors::const_iterator it = e.begin();
+  for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
+    const Vector& ei = *it;
+    axpy(alpha, ei, y[i]);
+  }
+  transposeMultiplyAdd2(alpha, it, e.end(), y);
+}
+
+/* ************************************************************************* */
+// y += alpha*inv(R1')*A2'*e2
+void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
     Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
 
-    // create e2 with what's left of e
+  // create e2 with what's left of e
-    // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
+  // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
-    Errors e2;
+  Errors e2;
-    while (it != end) e2.push_back(*(it++));
+  while (it != end) e2.push_back(*(it++));
 
-    VectorValues x = VectorValues::Zero(y); // x = 0
+  VectorValues x = VectorValues::Zero(y); // x = 0
-    Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
+  Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
-    axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
+  axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
+}
+
+/* ************************************************************************* */
+void SubgraphPreconditioner::print(const std::string& s) const {
+  cout << s << endl;
+  Ab2_->print();
+}
+
+/*****************************************************************************/
+void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
+{
+  /* copy first */
+  std::copy(y.data(), y.data() + y.rows(), x.data());
+
+  /* in place back substitute */
+  BOOST_REVERSE_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
+    /* collect a subvector of x that consists of the parents of cg (S) */
+    const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+
+    /* compute the solution for the current pivot */
+    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
+
+    /* assign subvector of sol to the frontal variables */
+    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+  }
+}
+
+/*****************************************************************************/
+void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
+{
+  /* copy first */
+  std::copy(y.data(), y.data() + y.rows(), x.data());
+
+  /* in place back substitute */
+  BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+//    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
+    const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
+
+    // Check for indeterminant solution
+    if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
+
+    /* assign subvector of sol to the frontal variables */
+    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+
+    /* substract from parent variables */
+    for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
+      KeyInfo::const_iterator it2 = keyInfo_.find(*it);
+      Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
+      rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
+    }
+  }
+}
+
+/*****************************************************************************/
+void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
+{
+  /* identify the subgraph structure */
+  const SubgraphBuilder builder(parameters_.builderParams_);
+  Subgraph::shared_ptr subgraph = builder(gfg);
+
+  keyInfo_ = keyInfo;
+
+  /* build factor subgraph */
+  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);
+
+  /* factorize and cache BayesNet */
+  Rc1_ = gfg_subgraph->eliminateSequential();
+}
+
+/*****************************************************************************/
+Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
+
+  /* a cache of starting index and dim */
+  typedef vector<pair<size_t, size_t> > Cache;
+  Cache cache;
+
+  /* figure out dimension by traversing the keys */
+  size_t d = 0;
+  BOOST_FOREACH ( const Key &key, keys ) {
+    const KeyInfoEntry &entry = keyInfo.find(key)->second;
+    cache.push_back(make_pair(entry.colstart(), entry.dim()));
+    d += entry.dim();
   }
 
-  /* ************************************************************************* */
+  /* use the cache to fill the result */
-  void SubgraphPreconditioner::print(const std::string& s) const {
+  Vector result = Vector::Zero(d, 1);
-    cout << s << endl;
+  size_t idx = 0;
-    Ab2_->print();
+  BOOST_FOREACH ( const Cache::value_type &p, cache ) {
+    result.segment(idx, p.second) = src.segment(p.first, p.second) ;
+    idx += p.second;
   }
+
+  return result;
+}
+
+/*****************************************************************************/
+void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
+  /* use the cache  */
+  size_t idx = 0;
+  BOOST_FOREACH ( const Key &key, keys ) {
+    const KeyInfoEntry &entry = keyInfo.find(key)->second;
+    dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
+    idx += entry.dim();
+  }
+}
+
+/*****************************************************************************/
+boost::shared_ptr<GaussianFactorGraph>
+buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
+
+  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+  result->reserve(subgraph.size());
+  BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
+    const size_t idx = e.index();
+    if ( clone ) result->push_back(gfg[idx]->clone());
+    else result->push_back(gfg[idx]);
+  }
+  return result;
+}
+
 } // nsamespace gtsam

gtsam/linear/SubgraphPreconditioner.h

@@ -12,15 +12,16 @@
 /**
  * @file SubgraphPreconditioner.h
  * @date Dec 31, 2009
- * @author Frank Dellaert
+ * @author Frank Dellaert, Yong-Dian Jian
  */
 
 #pragma once
 
 #include <gtsam/global_includes.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/Errors.h>
-
+#include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/linear/VectorValues.h>
 #include <boost/shared_ptr.hpp>
 
 namespace gtsam {
@@ -30,6 +31,146 @@ namespace gtsam {
   class GaussianFactorGraph;
   class VectorValues;
 
+  struct GTSAM_EXPORT SubgraphEdge {
+    size_t index_;   /* edge id */
+    double weight_; /* edge weight */
+    SubgraphEdge() : index_(0), weight_(1.0) {}
+    SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
+    SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
+    inline size_t index() const { return index_; }
+    inline double weight() const { return weight_; }
+    inline bool isUnitWeight() const { return (weight_ == 1.0); }
+    friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
+  private:
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive & ar, const unsigned int version) {
+      ar & BOOST_SERIALIZATION_NVP(index_);
+      ar & BOOST_SERIALIZATION_NVP(weight_);
+    }
+  };
+
+  /**************************************************************************/
+  class GTSAM_EXPORT Subgraph {
+  public:
+    typedef boost::shared_ptr<Subgraph> shared_ptr;
+    typedef std::vector<shared_ptr> vector_shared_ptr;
+    typedef std::vector<SubgraphEdge> Edges;
+    typedef std::vector<size_t> EdgeIndices;
+    typedef Edges::iterator iterator;
+    typedef Edges::const_iterator const_iterator;
+
+  protected:
+    Edges edges_;                 /* index to the factors */
+
+  public:
+    Subgraph() {}
+    Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
+    Subgraph(const Edges &edges) : edges_(edges) {}
+    Subgraph(const std::vector<size_t> &indices) ;
+
+    inline const Edges& edges() const { return edges_; }
+    inline const size_t size() const { return edges_.size(); }
+    EdgeIndices edgeIndices() const;
+
+    iterator begin() { return edges_.begin(); }
+    const_iterator begin() const { return edges_.begin(); }
+    iterator end() { return edges_.end(); }
+    const_iterator end() const { return edges_.end(); }
+
+    void save(const std::string &fn) const;
+    static shared_ptr load(const std::string &fn);
+    friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
+
+  private:
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive & ar, const unsigned int version) {
+      ar & BOOST_SERIALIZATION_NVP(edges_);
+    }
+  };
+
+  /****************************************************************************/
+  struct GTSAM_EXPORT SubgraphBuilderParameters {
+  public:
+    typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
+
+    enum Skeleton {
+      /* augmented tree */
+      NATURALCHAIN = 0,  /* natural ordering of the graph */
+      BFS,      /* breadth-first search tree */
+      KRUSKAL,  /* maximum weighted spanning tree */
+    } skeleton_ ;
+
+    enum SkeletonWeight {   /* how to weigh the graph edges */
+      EQUAL = 0,    /* every block edge has equal weight */
+      RHS_2NORM,    /* use the 2-norm of the rhs */
+      LHS_FNORM,    /* use the frobenius norm of the lhs */
+      RANDOM,       /* bounded random edge weight */
+    } skeletonWeight_ ;
+
+    enum AugmentationWeight {   /* how to weigh the graph edges */
+      SKELETON = 0,             /* use the same weights in building the skeleton */
+//      STRETCH,                  /* stretch in the laplacian sense */
+//      GENERALIZED_STRETCH       /* the generalized stretch defined in jian2013iros */
+    } augmentationWeight_ ;
+
+    double complexity_;
+
+    SubgraphBuilderParameters()
+      : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
+    virtual ~SubgraphBuilderParameters() {}
+
+    /* for serialization */
+    void print() const ;
+    virtual void print(std::ostream &os) const ;
+    friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
+
+    static Skeleton skeletonTranslator(const std::string &s);
+    static std::string skeletonTranslator(Skeleton w);
+    static SkeletonWeight skeletonWeightTranslator(const std::string &s);
+    static std::string skeletonWeightTranslator(SkeletonWeight w);
+    static AugmentationWeight augmentationWeightTranslator(const std::string &s);
+    static std::string augmentationWeightTranslator(AugmentationWeight w);
+  };
+
+  /*****************************************************************************/
+  class GTSAM_EXPORT SubgraphBuilder {
+
+  public:
+    typedef SubgraphBuilder Base;
+    typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
+    typedef std::vector<double> Weights;
+
+    SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
+      : parameters_(p) {}
+    virtual ~SubgraphBuilder() {}
+    virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
+
+  private:
+    std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
+    std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
+    std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
+    std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
+    std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
+    std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
+    Weights weights(const GaussianFactorGraph &gfg) const;
+    SubgraphBuilderParameters parameters_;
+
+  private:
+    SubgraphBuilder() {}
+  };
+
+  /*******************************************************************************************/
+  struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
+    typedef PreconditionerParameters Base;
+    typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
+    SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
+      : Base(), builderParams_(p) {}
+    virtual ~SubgraphPreconditionerParameters() {}
+    SubgraphBuilderParameters builderParams_;
+  };
+
   /**
    * Subgraph conditioner class, as explained in the RSS 2010 submission.
    * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
@@ -37,7 +178,7 @@ namespace gtsam {
    * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2.
    * Then solve for yhat using CG, and solve for xhat = system.x(yhat).
    */
-  class GTSAM_EXPORT SubgraphPreconditioner {
+  class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
 
   public:
     typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
@@ -52,9 +193,12 @@ namespace gtsam {
     sharedValues xbar_;  ///< A1 \ b1
     sharedErrors b2bar_; ///< A2*xbar - b2
 
+    KeyInfo keyInfo_;
+    SubgraphPreconditionerParameters parameters_;
+
   public:
 
-    SubgraphPreconditioner();
+    SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
 
     /**
      * Constructor
@@ -62,7 +206,10 @@ namespace gtsam {
      * @param Rc1: the Bayes Net R1*x=c1
      * @param xbar: the solution to R1*x=c1
      */
-    SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
+    SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
+                           const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
+
+    virtual ~SubgraphPreconditioner() {}
 
     /** print the object */
     void print(const std::string& s = "SubgraphPreconditioner") const;
@@ -80,7 +227,6 @@ namespace gtsam {
      * Add zero-mean i.i.d. Gaussian prior terms to each variable
      * @param sigma Standard deviation of Gaussian
      */
-//  SubgraphPreconditioner add_priors(double sigma) const;
 
     /* x = xbar + inv(R1)*y */
     VectorValues x(const VectorValues& y) const;
@@ -119,6 +265,54 @@ namespace gtsam {
     *  y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
     */
     void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
+
+    /*****************************************************************************/
+    /* implement virtual functions of Preconditioner */
+
+    /* Computation Interfaces for Vector */
+    virtual void solve(const Vector& y, Vector &x) const;
+    virtual void transposeSolve(const Vector& y, Vector& x) const ;
+
+    virtual void build(
+      const GaussianFactorGraph &gfg,
+      const KeyInfo &info,
+      const std::map<Key,Vector> &lambda
+      ) ;
+    /*****************************************************************************/
   };
 
+  /* get subvectors */
+  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
+
+  /* set subvectors */
+  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
+
+
+  /* build a factor subgraph, which is defined as a set of weighted edges (factors) */
+  boost::shared_ptr<GaussianFactorGraph>
+  buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
+
+
+  /* sort the container and return permutation index with default comparator */
+   template <typename Container>
+   std::vector<size_t> sort_idx(const Container &src)
+   {
+     typedef typename Container::value_type T;
+     const size_t n = src.size() ;
+     std::vector<std::pair<size_t,T> > tmp;
+     tmp.reserve(n);
+     for ( size_t i = 0 ; i < n ; i++ )
+       tmp.push_back(std::make_pair(i, src[i]));
+
+     /* sort */
+     std::stable_sort(tmp.begin(), tmp.end()) ;
+
+     /* copy back */
+     std::vector<size_t> idx; idx.reserve(n);
+     for ( size_t i = 0 ; i < n ; i++ ) {
+       idx.push_back(tmp[i].first) ;
+     }
+     return idx;
+   }
+
 } // namespace gtsam

gtsam/linear/SubgraphSolver.cpp

@@ -14,6 +14,7 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/graph-inl.h>
@@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
   return boost::tie(At, Ac);
 }
 
+/****************************************************************************/
+VectorValues SubgraphSolver::optimize (
+  const GaussianFactorGraph &gfg,
+  const KeyInfo &keyInfo,
+  const std::map<Key, Vector> &lambda,
+  const VectorValues &initial
+) { return VectorValues(); }
 } // \namespace gtsam

gtsam/linear/SubgraphSolver.h

@@ -13,22 +13,23 @@
 
 
 #include <gtsam/linear/ConjugateGradientSolver.h>
-#include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/inference/Ordering.h>
-
 #include <boost/tuple/tuple.hpp>
+#include <iosfwd>
 
 namespace gtsam {
 
   // Forward declarations
   class GaussianFactorGraph;
   class GaussianBayesNet;
+  class SubgraphPreconditioner;
 
 class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
 public:
   typedef ConjugateGradientParameters Base;
   SubgraphSolverParameters() : Base() {}
-  virtual void print() const { Base::print(); }
+  void print() const { Base::print(); }
+  virtual void print(std::ostream &os) const { Base::print(os); }
 };
 
 /**
@@ -61,7 +62,7 @@ public:
 protected:
   Parameters parameters_;
   Ordering ordering_;
-  SubgraphPreconditioner::shared_ptr pc_;  ///< preconditioner object
+  boost::shared_ptr<SubgraphPreconditioner> pc_;  ///< preconditioner object
 
 public:
   /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
@@ -77,8 +78,17 @@ public:
   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
 
   virtual ~SubgraphSolver() {}
-  virtual VectorValues optimize () ;
+
-  virtual VectorValues optimize (const VectorValues &initial) ;
+  VectorValues optimize () ;
+  VectorValues optimize (const VectorValues &initial) ;
+
+  /* interface to the nonlinear optimizer that the subclasses have to implement */
+  virtual VectorValues optimize (
+    const GaussianFactorGraph &gfg,
+    const KeyInfo &keyInfo,
+    const std::map<Key, Vector> &lambda,
+    const VectorValues &initial
+  ) ;
 
 protected:
 

gtsam/navigation/CombinedImuFactor.h

@@ -11,7 +11,7 @@
 
 /**
  *  @file  CombinedImuFactor.h
- *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
+ *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
  **/
 
 #pragma once
@@ -71,8 +71,9 @@ namespace gtsam {
       Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
       Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
       Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-
       Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
+      bool use2ndOrderIntegration_; ///< Controls the order of integration
+
       ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
       ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
       /** Default constructor, initialize with no IMU measurements */
@@ -83,12 +84,14 @@ namespace gtsam {
           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
-          const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
+          const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements
+          const bool use2ndOrderIntegration = false ///< Controls the order of integration
           ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
       ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
       delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
       delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
-      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
+      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)),
+      use2ndOrderIntegration_(use2ndOrderIntegration)
       {
           // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
         measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),      Matrix3::Zero(),
@@ -136,6 +139,19 @@ namespace gtsam {
             && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
       }
 
+      void resetIntegration(){
+        deltaPij = Vector3::Zero();
+        deltaVij = Vector3::Zero();
+        deltaRij = Rot3();
+        deltaTij = 0.0;
+        delPdelBiasAcc = Matrix3::Zero();
+        delPdelBiasOmega = Matrix3::Zero();
+        delVdelBiasAcc = Matrix3::Zero();
+        delVdelBiasOmega = Matrix3::Zero();
+        delRdelBiasOmega = Matrix3::Zero();
+        PreintMeasCov = Matrix::Zero(15,15);
+      }
+
       /** Add a single IMU measurement to the preintegration. */
       void integrateMeasurement(
           const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
@@ -163,11 +179,14 @@ namespace gtsam {
 
         // Update Jacobians
         /* ----------------------------------------------------------------------------------------------------------------------- */
-        //        delPdelBiasAcc += delVdelBiasAcc * deltaT;
+        if(!use2ndOrderIntegration_){
-        //        delPdelBiasOmega += delVdelBiasOmega * deltaT;
+          delPdelBiasAcc += delVdelBiasAcc * deltaT;
-        delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
+          delPdelBiasOmega += delVdelBiasOmega * deltaT;
-        delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
+        }else{
-                                    * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
+          delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
+          delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
+                                        * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
+        }
 
         delVdelBiasAcc += -deltaRij.matrix() * deltaT;
         delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
@@ -222,32 +241,31 @@ namespace gtsam {
         G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
 
         G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc)  *
-            (measurementCovariance.block(3,3,3,3) +  measurementCovariance.block(9,9,3,3) +  measurementCovariance.block(15,15,3,3) ) *
+            (measurementCovariance.block(3,3,3,3)  +  measurementCovariance.block(15,15,3,3) ) *
-                                                   (H_vel_biasacc.transpose());
+            (H_vel_biasacc.transpose());
 
         G_measCov_Gt.block(6,6,3,3) = (1/deltaT) *  (H_angles_biasomega) *
-            (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) +  measurementCovariance.block(18,18,3,3) ) *
+            (measurementCovariance.block(6,6,3,3)  +  measurementCovariance.block(18,18,3,3) ) *
-                                                    (H_angles_biasomega.transpose());
+            (H_angles_biasomega.transpose());
 
         G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3);
 
         G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3);
 
-        // OFF BLOCK DIAGONAL TERMS
+        // NEW OFF BLOCK DIAGONAL TERMS
-        Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3);
+        Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) *  H_angles_biasomega.transpose();
-        G_measCov_Gt.block(3,9,3,3) = block24;
+        G_measCov_Gt.block(3,6,3,3) = block23;
-        G_measCov_Gt.block(9,3,3,3) = block24.transpose();
+        G_measCov_Gt.block(6,3,3,3) = block23.transpose();
-
-        Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3);
-        G_measCov_Gt.block(6,12,3,3) = block35;
-        G_measCov_Gt.block(12,6,3,3) = block35.transpose();
 
         PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt;
 
         // Update preintegrated measurements
         /* ----------------------------------------------------------------------------------------------------------------------- */
-        // deltaPij += deltaVij * deltaT;
+        if(!use2ndOrderIntegration_){
-        deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+          deltaPij += deltaVij * deltaT;
+        }else{
+          deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+        }
         deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
         deltaRij = deltaRij * Rincr;
         deltaTij += deltaT;
@@ -311,23 +329,40 @@ namespace gtsam {
     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:
 
     /** Shorthand for a smart pointer to a factor */
-    typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
+    typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#else
+      typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#endif
 
     /** Default constructor - only use for serialization */
     CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
 
     /** Constructor */
-    CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+    CombinedImuFactor(
-        const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
+    	Key pose_i, ///< previous pose key
-        const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
+    	Key vel_i, ///< previous velocity key
-      Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
+    	Key pose_j, ///< current pose key
+    	Key vel_j, ///< current velocity key
+    	Key bias_i, ///< previous bias key
+    	Key bias_j, ///< current bias key
+        const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
+        const Vector3& gravity, ///< gravity vector
+        const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
+        boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
+        const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
+    ) :
+      Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
       preintegratedMeasurements_(preintegratedMeasurements),
       gravity_(gravity),
       omegaCoriolis_(omegaCoriolis),
-      body_P_sensor_(body_P_sensor) {
+      body_P_sensor_(body_P_sensor),
+      use2ndOrderCoriolis_(use2ndOrderCoriolis){
     }
 
     virtual ~CombinedImuFactor() {}
@@ -360,7 +395,7 @@ namespace gtsam {
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != NULL && Base::equals(*e, tol)
-          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
+          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
           && equal_with_abs_tol(gravity_, e->gravity_, 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_)));
@@ -370,6 +405,10 @@ namespace gtsam {
     const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
       return preintegratedMeasurements_; }
 
+    const Vector3& gravity() const { return gravity_; }
+
+    const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
@@ -414,19 +453,18 @@ namespace gtsam {
 
       const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
 
-      if(H1) {
+      /*
-        H1->resize(15,6);
         (*H1) <<
             // dfP/dRi
             Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
                 + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
                 // dfP/dPi
-                - Rot_i.matrix(),
+                - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
                 // dfV/dRi
                 Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
                     + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
                     // dfV/dPi
-                    Matrix3::Zero(),
+                    skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
                     // dfR/dRi
                     Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
                     // dfR/dPi
@@ -435,6 +473,40 @@ namespace gtsam {
                     Matrix3::Zero(), Matrix3::Zero(),
                     //dBiasOmega/dPi
                     Matrix3::Zero(), Matrix3::Zero();
+          */
+      if(H1) {
+        H1->resize(15,6);
+
+        Matrix3 dfPdPi;
+        Matrix3 dfVdPi;
+        if(use2ndOrderCoriolis_){
+        	dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+        	dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+        }
+        else{
+        	dfPdPi = - Rot_i.matrix();
+        	dfVdPi = Matrix3::Zero();
+        }
+
+		(*H1) <<
+			// dfP/dRi
+			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+				+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
+			// dfP/dPi
+			dfPdPi,
+			// dfV/dRi
+			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+				+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
+			// dfV/dPi
+			dfVdPi,
+			// dfR/dRi
+			Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+			// dfR/dPi
+			Matrix3::Zero(),
+			//dBiasAcc/dPi
+			Matrix3::Zero(), Matrix3::Zero(),
+			//dBiasOmega/dPi
+			Matrix3::Zero(), Matrix3::Zero();
       }
 
       if(H2) {
@@ -445,14 +517,13 @@ namespace gtsam {
             + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
             // dfV/dVi
             - Matrix3::Identity()
-        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+			+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-        // dfR/dVi
+			// dfR/dVi
-        Matrix3::Zero(),
+			Matrix3::Zero(),
-        //dBiasAcc/dVi
+			//dBiasAcc/dVi
-        Matrix3::Zero(),
+			Matrix3::Zero(),
-        //dBiasOmega/dVi
+			//dBiasOmega/dVi
-        Matrix3::Zero();
+			Matrix3::Zero();
-
       }
 
       if(H3) {
@@ -524,7 +595,6 @@ namespace gtsam {
                   Matrix3::Zero(), Matrix3::Identity();
       }
 
-
       // Evaluate residual error, according to [3]
       /* ---------------------------------------------------------------------------------------------------- */
       const Vector3 fp =
@@ -559,7 +629,8 @@ namespace gtsam {
     static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
         const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
         const CombinedPreintegratedMeasurements& preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
+        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
+        const bool use2ndOrderCoriolis = false)
     {
 
       const double& deltaTij = preintegratedMeasurements.deltaTij;
@@ -571,18 +642,23 @@ namespace gtsam {
 
       // Predict state at time j
       /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+       Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-          + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+		  + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+		  + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-          + vel_i * deltaTij
+		  + vel_i * deltaTij
-          - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          + 0.5 * gravity * deltaTij*deltaTij;
+		  + 0.5 * gravity * deltaTij*deltaTij;
 
-      vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+	  vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-          + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
+		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-          - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-          + gravity * deltaTij);
+		  + gravity * deltaTij);
+
+      if(use2ndOrderCoriolis){
+    	  pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;	// 2nd order coriolis term for position
+    	  vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+      }
 
       const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
       // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)

gtsam/navigation/ImuFactor.h

@@ -11,7 +11,7 @@
 
 /**
  *  @file  ImuFactor.h
- *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
+ *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
  **/
 
 #pragma once
@@ -60,7 +60,7 @@ namespace gtsam {
       imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
       Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
-      Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i)
+      Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
       Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
       Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
       double deltaTij; ///< Time interval from i to j
@@ -70,19 +70,20 @@ namespace gtsam {
       Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
       Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
       Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-
       Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
+      bool use2ndOrderIntegration_; ///< Controls the order of integration
 
       /** Default constructor, initialize with no IMU measurements */
       PreintegratedMeasurements(
           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc
+          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
+          const bool use2ndOrderIntegration = false ///< Controls the order of integration
       ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
       delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
       delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
-      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
+      delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
       {
         measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
                                        Matrix3::Zero(), measuredAccCovariance,  Matrix3::Zero(),
@@ -127,6 +128,19 @@ namespace gtsam {
             && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
       }
 
+      void resetIntegration(){
+        deltaPij = Vector3::Zero();
+        deltaVij = Vector3::Zero();
+        deltaRij = Rot3();
+        deltaTij = 0.0;
+        delPdelBiasAcc = Matrix3::Zero();
+        delPdelBiasOmega = Matrix3::Zero();
+        delVdelBiasAcc = Matrix3::Zero();
+        delVdelBiasOmega = Matrix3::Zero();
+        delRdelBiasOmega = Matrix3::Zero();
+        PreintMeasCov = Matrix::Zero(9,9);
+      }
+
       /** Add a single IMU measurement to the preintegration. */
       void integrateMeasurement(
           const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
@@ -143,11 +157,8 @@ namespace gtsam {
         // 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();
-
           correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-
           Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-
           correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
           // linear acceleration vector in the body frame
         }
@@ -159,16 +170,19 @@ namespace gtsam {
 
         // Update Jacobians
         /* ----------------------------------------------------------------------------------------------------------------------- */
-        //        delPdelBiasAcc += delVdelBiasAcc * deltaT;
+        if(!use2ndOrderIntegration_){
-        //        delPdelBiasOmega += delVdelBiasOmega * deltaT;
+          delPdelBiasAcc += delVdelBiasAcc * deltaT;
-        delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
+          delPdelBiasOmega += delVdelBiasOmega * deltaT;
-        delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
+        }else{
-                            * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
+          delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
+          delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
+                                    * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
+        }
         delVdelBiasAcc += -deltaRij.matrix() * deltaT;
         delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
         delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr  * deltaT;
 
-        // Update preintegrated mesurements covariance
+        // Update preintegrated measurements covariance
         /* ----------------------------------------------------------------------------------------------------------------------- */
         Matrix3 Z_3x3 = Matrix3::Zero();
         Matrix3 I_3x3 = Matrix3::Identity();
@@ -210,7 +224,11 @@ namespace gtsam {
 
         // Update preintegrated measurements
         /* ----------------------------------------------------------------------------------------------------------------------- */
-        deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+        if(!use2ndOrderIntegration_){
+          deltaPij += deltaVij * deltaT;
+        }else{
+          deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+        }
         deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
         deltaRij = deltaRij * Rincr;
         deltaTij += deltaT;
@@ -274,24 +292,39 @@ namespace gtsam {
     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:
 
     /** Shorthand for a smart pointer to a factor */
+#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
+    typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
+#else
     typedef boost::shared_ptr<ImuFactor> shared_ptr;
-
+#endif
 
     /** Default constructor - only use for serialization */
     ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
 
     /** Constructor */
-    ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
+    ImuFactor(
-        const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
+    	Key pose_i, ///< previous pose key
-        boost::optional<const Pose3&> body_P_sensor = boost::none) :
+    	Key vel_i, ///< previous velocity key
+    	Key pose_j, ///< current pose key
+    	Key vel_j, ///< current velocity key
+    	Key bias, ///< previous bias key
+        const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
+        const Vector3& gravity, ///< gravity vector
+        const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
+        boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
+        const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
+    ) :
       Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
       preintegratedMeasurements_(preintegratedMeasurements),
       gravity_(gravity),
       omegaCoriolis_(omegaCoriolis),
-      body_P_sensor_(body_P_sensor) {
+      body_P_sensor_(body_P_sensor),
+      use2ndOrderCoriolis_(use2ndOrderCoriolis){
     }
 
     virtual ~ImuFactor() {}
@@ -323,7 +356,7 @@ namespace gtsam {
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != NULL && Base::equals(*e, tol)
-          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
+          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
           && equal_with_abs_tol(gravity_, e->gravity_, 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_)));
@@ -333,6 +366,10 @@ namespace gtsam {
     const PreintegratedMeasurements& preintegratedMeasurements() const {
       return preintegratedMeasurements_; }
 
+    const Vector3& gravity() const { return gravity_; }
+
+    const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
@@ -378,22 +415,35 @@ namespace gtsam {
 
       if(H1) {
         H1->resize(9,6);
-        (*H1) <<
+
-            // dfP/dRi
+        Matrix3 dfPdPi;
-            Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+        Matrix3 dfVdPi;
-                + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
+        if(use2ndOrderCoriolis_){
-                // dfP/dPi
+        	dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-                - Rot_i.matrix(),
+        	dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-                // dfV/dRi
+        }
-                Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+        else{
-                    + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
+        	dfPdPi = - Rot_i.matrix();
-                    // dfV/dPi
+        	dfVdPi = Matrix3::Zero();
-                    Matrix3::Zero(),
+        }
-                    // dfR/dRi
+
-                    Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+		(*H1) <<
-                    // dfR/dPi
+			// dfP/dRi
-                    Matrix3::Zero();
+			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+				+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
+			// dfP/dPi
+			dfPdPi,
+			// dfV/dRi
+			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+				+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
+			// dfV/dPi
+			dfVdPi,
+			// dfR/dRi
+			Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+			// dfR/dPi
+			Matrix3::Zero();
       }
+
       if(H2) {
         H2->resize(9,3);
         (*H2) <<
@@ -402,11 +452,11 @@ namespace gtsam {
             + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
             // dfV/dVi
             - Matrix3::Identity()
-        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+            + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-        // dfR/dVi
+            // dfR/dVi
-        Matrix3::Zero();
+            Matrix3::Zero();
-
       }
+
       if(H3) {
 
         H3->resize(9,6);
@@ -418,6 +468,7 @@ namespace gtsam {
             // dfR/dPosej
             Jrinv_fRhat *  ( Matrix3::Identity() ), Matrix3::Zero();
       }
+
       if(H4) {
         H4->resize(9,3);
         (*H4) <<
@@ -428,6 +479,7 @@ namespace gtsam {
             // dfR/dVj
             Matrix3::Zero();
       }
+
       if(H5) {
 
         const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
@@ -475,7 +527,8 @@ namespace gtsam {
     /** predicted states from IMU */
     static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
         const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
+        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
+        const bool use2ndOrderCoriolis = false)
     {
 
       const double& deltaTij = preintegratedMeasurements.deltaTij;
@@ -487,18 +540,23 @@ namespace gtsam {
 
       // Predict state at time j
       /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+	  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-          + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+		  + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+		  + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-          + vel_i * deltaTij
+		  + vel_i * deltaTij
-          - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          + 0.5 * gravity * deltaTij*deltaTij;
+		  + 0.5 * gravity * deltaTij*deltaTij;
 
-      vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+	  vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-          + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
+		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-          - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-          + gravity * deltaTij);
+		  + gravity * deltaTij);
+
+      if(use2ndOrderCoriolis){
+    	  pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;	// 2nd order coriolis term for position
+    	  vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+      }
 
       const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
       // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -183,7 +183,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
 
     noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov);
-    CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel);
+    CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
 
 
     Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
@@ -260,7 +260,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
   EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
   EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
 #include <gtsam/linear/GaussianFactorGraph.h>

gtsam/navigation/tests/testImuFactor.cpp

@@ -136,8 +136,9 @@ TEST( ImuFactor, PreintegratedMeasurements )
   Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0);
   double expectedDeltaT1(0.5);
 
+  bool use2ndOrderIntegration = true;
   // Actual preintegrated values
-  ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+  ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6));
@@ -177,7 +178,8 @@ TEST( ImuFactor, Error )
   Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
   double deltaT = 1.0;
-  ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+  bool use2ndOrderIntegration = true;
+  ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
@@ -217,7 +219,7 @@ TEST( ImuFactor, Error )
   Matrix H1atop6 =  H1a.topRows(6);
   EXPECT(assert_equal(H1etop6, H1atop6));
   // rotations
-  EXPECT(assert_equal(RH1e, H1a.bottomRows(3)));  // evaluate only the rotation residue
+  EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H2e, H2a));
 
@@ -226,7 +228,7 @@ TEST( ImuFactor, Error )
   Matrix H3atop6 =  H3a.topRows(6);
   EXPECT(assert_equal(H3etop6, H3atop6));
   // rotations
-  EXPECT(assert_equal(RH3e, H3a.bottomRows(3)));  // evaluate only the rotation residue
+  EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H4e, H4a));
 //  EXPECT(assert_equal(H5e, H5a));
@@ -324,7 +326,7 @@ TEST( ImuFactor, PartialDerivativeExpmap )
    Matrix3  actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
 
   // Compare Jacobians
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega));
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));  // 1e-3 needs to be added only when using quaternions for rotations
 
 }
 
@@ -436,7 +438,7 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
   EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
   EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
+  EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
 //#include <gtsam/linear/GaussianFactorGraph.h>

gtsam/nonlinear/DoglegOptimizer.cpp

@@ -76,7 +76,7 @@ void DoglegOptimizer::iterate(void) {
     result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
       dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose);
   }
-  else if ( params_.isCG() ) {
+  else if ( params_.isIterative() ) {
     throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver");
   }
   else {

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/linear/VectorValues.h>
 #include <boost/date_time/posix_time/posix_time.hpp>
 
 class NonlinearOptimizerMoreOptimizationTest;

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/SubgraphSolver.h>
+#include <gtsam/linear/PCGSolver.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 
@@ -106,23 +107,21 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
     delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.eliminateSequential(*params.ordering,
+    delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
-        params.getEliminationFunction())->optimize();
+  } else if (params.isIterative()) {
-  } else if (params.isCG()) {
+
     // Conjugate Gradient -> needs params.iterativeParams
     if (!params.iterativeParams)
-      throw std::runtime_error(
+      throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
-          "NonlinearOptimizer::solve: cg parameter has to be assigned ...");
+
-    // the type of params.iterativeParams decides the type of CG solver
+    if (boost::shared_ptr<PCGSolverParameters> pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams) ) {
-    if (boost::dynamic_pointer_cast<SubgraphSolverParameters>(
+      delta = PCGSolver(*pcg).optimize(gfg);
-        params.iterativeParams)) {
+    }
-      SubgraphSolver solver(gfg,
+    else if (boost::shared_ptr<SubgraphSolverParameters> spcg = boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
-          dynamic_cast<const SubgraphSolverParameters&>(*params.iterativeParams),
+      delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();
-          *params.ordering);
+    }
-      delta = solver.optimize();
+    else {
-    } else {
+      throw std::runtime_error("NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
-      throw std::runtime_error(
-          "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
     }
   } else {
     throw std::runtime_error(

gtsam/nonlinear/NonlinearOptimizerParams.cpp

@@ -66,8 +66,8 @@ std::string NonlinearOptimizerParams::verbosityTranslator(
 
 /* ************************************************************************* */
 void NonlinearOptimizerParams::setIterativeParams(
-    const SubgraphSolverParameters &params) {
+    const boost::shared_ptr<IterativeOptimizationParameters> params) {
-  iterativeParams = boost::make_shared<SubgraphSolverParameters>(params);
+  iterativeParams = params;
 }
 
 /* ************************************************************************* */
@@ -99,8 +99,8 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
   case CHOLMOD:
     std::cout << "         linear solver type: CHOLMOD\n";
     break;
-  case CONJUGATE_GRADIENT:
+  case Iterative:
-    std::cout << "         linear solver type: CONJUGATE GRADIENT\n";
+    std::cout << "         linear solver type: ITERATIVE\n";
     break;
   default:
     std::cout << "         linear solver type: (invalid)\n";
@@ -127,8 +127,8 @@ std::string NonlinearOptimizerParams::linearSolverTranslator(
     return "SEQUENTIAL_CHOLESKY";
   case SEQUENTIAL_QR:
     return "SEQUENTIAL_QR";
-  case CONJUGATE_GRADIENT:
+  case Iterative:
-    return "CONJUGATE_GRADIENT";
+    return "ITERATIVE";
   case CHOLMOD:
     return "CHOLMOD";
   default:
@@ -148,8 +148,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
     return SEQUENTIAL_CHOLESKY;
   if (linearSolverType == "SEQUENTIAL_QR")
     return SEQUENTIAL_QR;
-  if (linearSolverType == "CONJUGATE_GRADIENT")
+  if (linearSolverType == "ITERATIVE")
-    return CONJUGATE_GRADIENT;
+    return Iterative;
   if (linearSolverType == "CHOLMOD")
     return CHOLMOD;
   throw std::invalid_argument(

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -99,7 +99,7 @@ public:
     MULTIFRONTAL_QR,
     SEQUENTIAL_CHOLESKY,
     SEQUENTIAL_QR,
-    CONJUGATE_GRADIENT, /* Experimental Flag */
+    Iterative, /* Experimental Flag */
     CHOLMOD, /* Experimental Flag */
   };
 
@@ -121,8 +121,8 @@ public:
     return (linearSolverType == CHOLMOD);
   }
 
-  inline bool isCG() const {
+  inline bool isIterative() const {
-    return (linearSolverType == CONJUGATE_GRADIENT);
+    return (linearSolverType == Iterative);
   }
 
   GaussianFactorGraph::Eliminate getEliminationFunction() const {
@@ -148,7 +148,9 @@ public:
   void setLinearSolverType(const std::string& solver) {
     linearSolverType = linearSolverTranslator(solver);
   }
-  void setIterativeParams(const SubgraphSolverParameters& params);
+
+  void setIterativeParams(const boost::shared_ptr<IterativeOptimizationParameters> params);
+
   void setOrdering(const Ordering& ordering) {
     this->ordering = ordering;
   }

gtsam_unstable/gtsam_unstable.h

@@ -23,6 +23,7 @@ virtual class gtsam::NoiseModelFactor4;
 virtual class gtsam::GaussianFactor;
 virtual class gtsam::HessianFactor;
 virtual class gtsam::JacobianFactor;
+virtual class gtsam::Cal3_S2;
 class gtsam::GaussianFactorGraph;
 class gtsam::NonlinearFactorGraph;
 class gtsam::Ordering;
@@ -379,7 +380,6 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
 
 };
 
-
 #include <gtsam/slam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NonlinearFactor {

matlab/unstable_examples/+imuSimulator/+lib/antisim.m

@@ -0,0 +1,7 @@
+function S = antisim(v)
+
+% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v
+% Uso: S = antisim(v)
+
+S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
+

matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m

@@ -0,0 +1,140 @@
+function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio)
+
+% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 
+%     Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead
+%     pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem)
+%     deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos')
+%     colorCode - Color parameters as per the 'surf' command.  For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow
+%     stemRatio - The ratio of the length of the stem in proportion to the arrowhead.  For example, a call of:
+%                 arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance
+%                 of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18.  
+%     rhoRatio - The ratio of the cylinder radius (0.05 is the default)
+%     value)
+% 
+%     Example:
+%       arrow3D([0,0,0], [4,3,7]);  %---- arrow with default parameters
+%       axis equal;
+% 
+%    Author: Shawn Arseneau
+%    Created: September 14, 2006
+%    Updated: September 18, 2006
+% 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    if nargin<2 || nargin>5     
+        error('Incorrect number of inputs to arrow3D');     
+    end
+    if numel(pos)~=3 || numel(deltaValues)~=3
+        error('pos and/or deltaValues is incorrect dimensions (should be three)');
+    end
+    if nargin<3                 
+        colorCode = 'interp';                             
+    end
+    if nargin<4                 
+        stemRatio = 0.75;                                   
+    end    
+    if nargin<5                 
+        rhoRatio = 0.05;                                   
+    end    
+
+    X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector
+    Y = pos(2);
+    Z = pos(3);
+    
+    [sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3));  
+    
+    %******************************************* CYLINDER == STEM *********************************************
+    cylinderRadius = srho*rhoRatio;
+    cylinderLength = srho*stemRatio;
+    [CX,CY,CZ] = cylinder(cylinderRadius);      
+    CZ = CZ.*cylinderLength;    %---- lengthen
+    
+    %----- ROTATE CYLINDER
+    [row, col] = size(CX);      %---- initial rotation to coincide with X-axis
+    
+    newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]);
+    CX = reshape(newEll(:,1), row, col);
+    CY = reshape(newEll(:,2), row, col);
+    CZ = reshape(newEll(:,3), row, col);
+    
+    [row, col] = size(CX);    
+    newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]);
+    stemX = reshape(newEll(:,1), row, col);
+    stemY = reshape(newEll(:,2), row, col);
+    stemZ = reshape(newEll(:,3), row, col);
+
+    %----- TRANSLATE CYLINDER
+    stemX = stemX + X;
+    stemY = stemY + Y;
+    stemZ = stemZ + Z;
+    
+    %******************************************* CONE == ARROWHEAD *********************************************
+    coneLength = srho*(1-stemRatio);
+    coneRadius = cylinderRadius*1.5;
+    incr = 4;  %---- Steps of cone increments
+    coneincr = coneRadius/incr;    
+    [coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0);  %---------- CONE 
+    coneZ = coneZ.*coneLength;
+    
+    %----- ROTATE CONE 
+    [row, col] = size(coneX);    
+    newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]);
+    coneX = reshape(newEll(:,1), row, col);
+    coneY = reshape(newEll(:,2), row, col);
+    coneZ = reshape(newEll(:,3), row, col);
+    
+    newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]);
+    headX = reshape(newEll(:,1), row, col);
+    headY = reshape(newEll(:,2), row, col);
+    headZ = reshape(newEll(:,3), row, col);
+    
+    %---- TRANSLATE CONE
+    V = [0, 0, srho*stemRatio];    %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder
+    Vp = rotatePoints([0 0 -1], V);
+    Vp = rotatePoints(deltaValues, Vp);
+    headX = headX + Vp(1) + X;
+    headY = headY + Vp(2) + Y;
+    headZ = headZ + Vp(3) + Z;
+    %************************************************************************************************************    
+    hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); 
+    hold on;  
+    hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none');
+    
+    if nargout==1   
+        arrowHandle = [hStem, hHead]; 
+    end
+    
+    
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

matlab/unstable_examples/+imuSimulator/+lib/getxyz.m

@@ -0,0 +1,14 @@
+function [c] = getxyz(poses, j)
+% The function extract the Cartesian variables from pose (pose.p = positions, 
+% pose.R = rotations). In particular, if there are T poses, 
+% - getxyz(pose, 1) estracts the vector x \in R^T, 
+% - getxyz(pose, 2) estracts the vector y \in R^T,
+% - getxyz(pose, 3) estracts the vector z \in R^T.
+
+L = length(poses);
+c = [];
+for i=1:L % for each pose
+    c = [c poses(i).p(j)];
+end
+
+c = c(:); % column vector

matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m

@@ -0,0 +1,19 @@
+function [] = plot_trajectory(pose, step, color, plot_name,a,b,c)
+% Plot a collection of poses: positions are connected by a solid line 
+% of color "color" and it is shown a ref frame for each pose. 
+% Plot_name defines the name of the created figure.
+
+x = getxyz(pose,1); 
+y = getxyz(pose,2); 
+z = getxyz(pose,3); 
+plot3(x,y,z,color)
+n = length(x)-1;
+hold on
+for i=1:step:n+1
+    ref_frame_plot(pose(i).R,pose(i).p,a,b,c) 
+end
+title(plot_name);
+xlabel('x')
+ylabel('y')
+zlabel('z')
+view(3)  

matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m

@@ -0,0 +1,54 @@
+function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize)
+% ref_frame plot a 3D representation of a reference frame 
+% given by three unit vectors
+% 
+% ref_frame(Ref,DimSpace,OriginRef)
+%  Ref is a 3 x 3 orthogonal matrix representing the unit vectors
+%   of the reference frame to be drawn
+%  DimSpace is a 3 x 2 matrix with min an max dimensions of the space
+%   [xmin xmax; ymin ymax; zmin zmax]
+%   default value = [-1,5 +1.5] for all dimensions
+%  OriginRef is the reference frame origin point
+%   default value = [0 0 0]'
+
+%	Copright (C) Basilio Bona 2007
+
+n=nargin;
+if n == 1
+    OriginRef=[0 0 0]';
+    colorcode=['r','g','b'];
+    rhoRatio=0.05;
+    stemRatio=0.75;
+    axsize=1;
+end
+if n == 2
+    colorcode=['r','g','b'];
+    rhoRatio=0.05;
+    stemRatio=0.75;
+    axsize=1;
+end    
+if n == 3 
+    colorcode=['r','g','b'];
+    stemRatio=0.75;
+    axsize=1;
+end    
+
+if n == 4 
+    colorcode=['r','g','b'];
+    axsize=1;
+end  
+
+if n == 5 
+    colorcode=['r','g','b'];
+end  
+  
+
+% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1);
+
+%hold off;
+arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio);
+arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio);
+arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio);
+axis equal;   xlabel('X'); ylabel('Y'); zlabel('Z');
+% lighting phong; 
+% camlight left;

matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m

@@ -0,0 +1,82 @@
+function rotatedData = rotatePoints(alignmentVector, originalData)
+
+% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 
+%     Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector
+% 
+%       Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1]
+% 
+%       Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho);  alignmentVector = [dX,dY,dZ];
+% 
+% Example:
+%   %% Rotate the point [3,4,-7] with respect to the following:
+%   %%%% Original associated vector is always [1,0,0]
+%   %%%% Calculate the appropriate rotation requested with respect to the x-axis.  For example, if only a rotation about the z-axis is
+%   %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero
+%   rotData = rotatePoints(alignmentVector, [3,4,-7]);
+% 
+%     Author: Shawn Arseneau
+%     Created: Feb.2, 2006
+% 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+    alignmentDim = numel(alignmentVector);
+    DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data)
+    
+    if alignmentDim~=DOF    
+        error('Alignment vector does not agree with originalData dimensions');      
+    end
+    if DOF<2 || DOF>3      
+        error('rotatePoints only does rotation in two or three dimensions');        
+    end
+    
+        
+    if DOF==2  % 2D rotation...        
+        [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2));    
+        deg_theta = -1 * rad_theta * (180/pi);
+        ctheta = cosd(deg_theta);  stheta = sind(deg_theta);
+        
+        Rmatrix = [ctheta, -1.*stheta;...
+                   stheta,     ctheta];
+        rotatedData = originalData*Rmatrix;        
+        
+    else    % 3D rotation...        
+        [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3));
+        rad_theta = rad_theta * -1; 
+        deg_theta = rad_theta * (180/pi);
+        deg_phi = rad_phi * (180/pi); 
+        ctheta = cosd(deg_theta);  stheta = sind(deg_theta);
+        Rz = [ctheta,   -1.*stheta,     0;...
+              stheta,       ctheta,     0;...
+              0,                 0,     1];                  %% First rotate as per theta around the Z axis
+        rotatedData = originalData*Rz;
+
+        [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1);          %% Second rotation corresponding to phi
+        rotationAxis = [rotX, rotY, rotZ];
+        u = rotationAxis(:)/norm(rotationAxis);        %% Code extract from rotate.m from MATLAB
+        cosPhi = cosd(deg_phi);
+        sinPhi = sind(deg_phi);
+        invCosPhi = 1 - cosPhi;
+        x = u(1);
+        y = u(2);
+        z = u(3);
+        Rmatrix = [cosPhi+x^2*invCosPhi        x*y*invCosPhi-z*sinPhi     x*z*invCosPhi+y*sinPhi; ...
+                   x*y*invCosPhi+z*sinPhi      cosPhi+y^2*invCosPhi       y*z*invCosPhi-x*sinPhi; ...
+                   x*z*invCosPhi-y*sinPhi      y*z*invCosPhi+x*sinPhi     cosPhi+z^2*invCosPhi]';
+
+        rotatedData = rotatedData*Rmatrix;        
+    end
+
+
+
+
+
+
+
+
+
+
+
+
+
+

matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m

@@ -0,0 +1,14 @@
+function R = uth2rot(u,teta)
+
+% Uso: R = uth2rot(u,teta)
+%
+% calcola la matrice R 
+% a partire da asse u ed angolo di rotazione teta (in rad)
+
+S=antisim(u);
+t=teta;
+ 
+n=norm(u);
+
+R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2;
+

matlab/unstable_examples/+imuSimulator/IMUComparison.m

@@ -0,0 +1,146 @@
+import gtsam.*;
+
+deltaT = 0.001;
+summarizedDeltaT = 0.1;
+timeElapsed = 1;
+times = 0:deltaT:timeElapsed;
+
+omega = [0;0;2*pi];
+velocity = [1;0;0];
+
+summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
+    gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
+    1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
+
+%% Set initial conditions for the true trajectory and for the estimates
+% (one estimate is obtained by integrating in the body frame, the other
+% by integrating in the navigation frame)
+% Initial state (body)
+currentPoseGlobal = Pose3;
+currentVelocityGlobal = velocity;
+% Initial state estimate (integrating in navigation frame)
+currentPoseGlobalIMUnav = currentPoseGlobal;
+currentVelocityGlobalIMUnav = currentVelocityGlobal;
+% Initial state estimate (integrating in the body frame)
+currentPoseGlobalIMUbody = currentPoseGlobal;
+currentVelocityGlobalIMUbody = currentVelocityGlobal;
+
+%% Prepare data structures for actual trajectory and estimates
+% Actual trajectory
+positions = zeros(3, length(times)+1);
+positions(:,1) = currentPoseGlobal.translation.vector;
+poses(1).p = positions(:,1);
+poses(1).R = currentPoseGlobal.rotation.matrix;
+
+% Trajectory estimate (integrated in the navigation frame)
+positionsIMUnav = zeros(3, length(times)+1);
+positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
+posesIMUnav(1).p = positionsIMUnav(:,1);
+posesIMUnav(1).R = poses(1).R;
+
+% Trajectory estimate (integrated in the body frame)
+positionsIMUbody = zeros(3, length(times)+1);
+positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
+posesIMUbody(1).p = positionsIMUbody(:,1);
+posesIMUbody(1).R = poses(1).R;
+
+%% Solver object
+isamParams = ISAM2Params;
+isamParams.setRelinearizeSkip(1);
+isam = gtsam.ISAM2(isamParams);
+
+initialValues = Values;
+initialValues.insert(symbol('x',0), currentPoseGlobal);
+initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
+initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
+initialFactors = NonlinearFactorGraph;
+initialFactors.add(PriorFactorPose3(symbol('x',0), ...
+    currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0)));
+initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
+    LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0)));
+initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
+    imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0)));
+
+%% Main loop
+i = 2;
+lastSummaryTime = 0;
+lastSummaryIndex = 0;
+currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
+for t = times
+  %% Create the actual trajectory, using the velocities and
+  % accelerations in the inertial frame to compute the positions
+  [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
+    currentPoseGlobal, omega, velocity, velocity, deltaT);
+  
+  %% Simulate IMU measurements, considering Coriolis effect 
+  % (in this simple example we neglect gravity and there are no other forces acting on the body)
+  acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
+    omega, omega, velocity, velocity, deltaT);
+
+  %% Accumulate preintegrated measurement
+  currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
+  
+  %% Update solver
+  if t - lastSummaryTime >= summarizedDeltaT
+      % Create IMU factor
+      initialFactors.add(ImuFactor( ...
+          symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
+          symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
+          symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ...
+          noiseModel.Isotropic.Sigma(9, 1e-6)));
+      
+      % Predict movement in a straight line (bad initialization)
+      if lastSummaryIndex > 0
+          initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
+              .compose(Pose3(Rot3, Point3(  velocity * t - lastSummaryTime)  ));
+          initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
+      else
+          initialPose = Pose3;
+          initialVel = LieVector(velocity);
+      end
+      initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
+      initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);
+      
+      % Update solver
+      isam.update(initialFactors, initialValues);
+      initialFactors = NonlinearFactorGraph;
+      initialValues = Values;
+      
+      lastSummaryIndex = lastSummaryIndex + 1;
+      lastSummaryTime = t;
+      currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
+  end
+  
+  %% Integrate in the body frame
+  [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
+    currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
+
+  %% Integrate in the navigation frame
+  [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
+    currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
+  
+  %% Store data in some structure for statistics and plots
+  positions(:,i) = currentPoseGlobal.translation.vector;
+  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
+  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;  
+  % - 
+  poses(i).p = positions(:,i);
+  posesIMUbody(i).p = positionsIMUbody(:,i);
+  posesIMUnav(i).p = positionsIMUnav(:,i); 
+  % -
+  poses(i).R = currentPoseGlobal.rotation.matrix;
+  posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
+  posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
+  i = i + 1;
+end
+
+figure(1)
+hold on;
+plot(positions(1,:), positions(2,:), '-b');
+plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
+plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
+plot3DTrajectory(isam.calculateEstimate, 'g-');
+axis equal;
+legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')
+
+

matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m

@@ -0,0 +1,128 @@
+close all
+clc
+
+import gtsam.*;
+
+deltaT = 0.001;
+summarizedDeltaT = 0.1;
+timeElapsed = 1;
+times = 0:deltaT:timeElapsed;
+
+omega = [0;0;2*pi];
+velocity = [1;0;0];
+
+g = [0;0;0];
+cor_v = [0;0;0];
+
+summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
+    gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
+    1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
+
+%% Set initial conditions for the true trajectory and for the estimates
+% (one estimate is obtained by integrating in the body frame, the other
+% by integrating in the navigation frame)
+% Initial state (body)
+currentPoseGlobal = Pose3;
+currentVelocityGlobal = velocity;
+
+%% Prepare data structures for actual trajectory and estimates
+% Actual trajectory
+positions = zeros(3, length(times)+1);
+positions(:,1) = currentPoseGlobal.translation.vector;
+poses(1).p = positions(:,1);
+poses(1).R = currentPoseGlobal.rotation.matrix;
+
+%% Solver object
+isamParams = ISAM2Params;
+isamParams.setRelinearizeSkip(1);
+isam = gtsam.ISAM2(isamParams);
+
+sigma_init_x = 1.0;
+sigma_init_v = 1.0;
+sigma_init_b = 1.0;
+
+initialValues = Values;
+initialValues.insert(symbol('x',0), currentPoseGlobal);
+initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
+initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
+initialFactors = NonlinearFactorGraph;
+% Prior on initial pose
+initialFactors.add(PriorFactorPose3(symbol('x',0), ...
+    currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
+% Prior on initial velocity 
+initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
+    LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
+% Prior on initial bias
+initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
+    imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
+
+%% Main loop
+i = 2;
+lastSummaryTime = 0;
+lastSummaryIndex = 0;
+currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
+for t = times
+  %% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions
+  [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
+    currentPoseGlobal, omega, velocity, velocity, deltaT);
+  
+  %% Simulate IMU measurements, considering Coriolis effect 
+  % (in this simple example we neglect gravity and there are no other forces acting on the body)
+  acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
+    omega, omega, velocity, velocity, deltaT);
+
+  %% Accumulate preintegrated measurement
+  currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
+  
+  %% Update solver
+  if t - lastSummaryTime >= summarizedDeltaT
+    
+      % Create IMU factor
+      initialFactors.add(ImuFactor( ...
+          symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
+          symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
+          symbol('b',0), currentSummarizedMeasurement, g, cor_v, ...
+          noiseModel.Isotropic.Sigma(9, 1e-6)));
+      
+      % Predict movement in a straight line (bad initialization)
+      if lastSummaryIndex > 0
+          initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
+              .compose(Pose3(Rot3, Point3(  velocity * t - lastSummaryTime)  ));
+          initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
+      else
+          initialPose = Pose3;
+          initialVel = LieVector(velocity);
+      end
+      initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
+      initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); 
+      
+      key_pose = symbol('x',lastSummaryIndex+1);
+      
+      % Update solver
+      isam.update(initialFactors, initialValues);
+      initialFactors = NonlinearFactorGraph;
+      initialValues = Values;
+      
+       isam.calculateEstimate(key_pose);
+       M = isam.marginalCovariance(key_pose);
+      
+      lastSummaryIndex = lastSummaryIndex + 1;
+      lastSummaryTime = t;
+      currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
+  end
+  
+  %% Store data in some structure for statistics and plots
+  positions(:,i) = currentPoseGlobal.translation.vector;
+  i = i + 1;
+end
+
+figure(1)
+hold on;
+plot(positions(1,:), positions(2,:), '-b');
+plot3DTrajectory(isam.calculateEstimate, 'g-');
+
+
+axis equal;
+legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')
+
+

matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m

@@ -0,0 +1,26 @@
+function pos_ECEF = LatLonHRad_to_ECEF(LatLonH)
+% convert latitude, longitude, height to XYZ in ECEF coordinates 
+% LatLonH(1) : latitude in radian 
+% LatLonH(2) : longitude in radian
+% LatLonH(3) : height in meter
+%
+% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial
+% Navigation", 1997. pp. 10 Eq 1.18
+%
+
+% constants
+a = 6378137.0; %m
+e_sqr =0.006694379990141317; 
+% b = 6356752.3142; % m 
+
+%RAD_PER_DEG = pi/180;
+phi = LatLonH(1);%*RAD_PER_DEG;
+lambda = LatLonH(2);%*RAD_PER_DEG;
+h = LatLonH(3);
+
+RN = a/sqrt(1 - e_sqr*sin(phi)^2);
+
+pos_ECEF = zeros(3,1);
+pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda);
+pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda);
+pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ;

matlab/unstable_examples/+imuSimulator/OdometryExample3D.m

@@ -0,0 +1,68 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Example of a simple 2D localization example
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% Copied Original file. Modified by David Jensen to use Pose3 instead of
+% Pose2. Everything else is the same.
+
+import gtsam.*
+
+%% Assumptions
+%  - Robot poses are facing along the X axis (horizontal, to the right in 2D)
+%  - The robot moves 2 meters each step
+%  - The robot is on a grid, moving 2 meters each step
+
+%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
+graph = NonlinearFactorGraph;
+
+%% Add a Gaussian prior on pose x_1
+priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph
+
+%% Add two odometry factors
+odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case)
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise));
+graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise));
+
+%% print
+graph.print(sprintf('\nFactor graph:\n'));
+
+%% Initialize to noisy points
+initialEstimate = Values;
+%initialEstimate.insert(1, Pose3.Expmap([0.2; 0.1; 0.1; 0.5; 0.0; 0.0]));
+%initialEstimate.insert(2, Pose3.Expmap([-0.2; 0.1; -0.1; 2.3; 0.1; 0.1]));
+%initialEstimate.insert(3, Pose3.Expmap([0.1; -0.1; 0.1; 4.1; 0.1; -0.1]));
+%initialEstimate.print(sprintf('\nInitial estimate:\n  '));
+
+for i=1:3
+  deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
+  deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
+  deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]);
+  currentPose = currentPose.compose(deltaPose);
+  initialEstimate.insert(i, currentPose);
+end
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
+result = optimizer.optimizeSafely();
+result.print(sprintf('\nFinal result:\n  '));
+
+%% Plot trajectory and covariance ellipses
+cla;
+hold on;
+
+plot3DTrajectory(result, [], Marginals(graph, result));
+
+axis([-0.6 4.8 -1 1])
+axis equal
+view(2)

matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m

@@ -0,0 +1,14 @@
+function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT)
+
+import gtsam.*;
+
+% gyro measured rotation rate
+measuredOmega = omega1Body;
+
+% Acceleration measurement (in this simple toy example no other forces 
+% act on the body and the only acceleration is the centripetal Coriolis acceleration)
+measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
+acc_omega = [ measuredAcc; measuredOmega ];
+
+end
+

matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m

@@ -0,0 +1,26 @@
+function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame)
+%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in
+%frame 1 and measured angular rates in frame 1.
+
+import gtsam.*;
+
+% Calculate gyro measured rotation rate by transforming body rotation rate
+% into the IMU frame.
+measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
+
+% Transform both velocities into IMU frame, accounting for the velocity
+% induced by rigid body rotation on a lever arm (Coriolis effect).
+velocity1inertial = imuFrame.rotation.unrotate( ...
+    Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
+
+imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
+velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
+    Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
+
+% Acceleration in IMU frame
+measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;
+
+acc_omega = [ measuredAcc; measuredOmega ];
+
+end
+

matlab/unstable_examples/+imuSimulator/coriolisExample.m

@@ -0,0 +1,508 @@
+%% coriolisExample.m
+% Author(s): David Jensen (david.jensen@gtri.gatech.edu)
+% This script demonstrates the relationship between object motion in inertial and rotating reference frames.
+% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially
+% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude),
+% which rotates with the earth.
+% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame.
+% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would
+% experience the body's motion.
+
+import gtsam.*;
+
+addpath(genpath('./Libraries'))
+
+% Check for an external configuarion. This is useful for setting up
+% multiple tests
+if ~exist('externalCoriolisConfiguration', 'var')
+    clc
+    clear all
+    close all
+    %% General configuration
+    navFrameRotating = 1;       % 0 = perform navigation in the fixed frame
+                                % 1 = perform navigation in the rotating frame
+    IMU_type = 1;               % IMU type 1 or type 2
+    useRealisticValues = 1;     % use reaslist values for initial position and earth rotation
+    record_movie = 0;           % 0 = do not record movie
+                                % 1 = record movie of the trajectories. One
+                                % frame per time step (15 fps)
+    incrementalPlotting = 1;    % turn incremental plotting on and off. Turning plotting off increases
+                                % speed for batch testing
+    estimationEnabled = 1;
+
+    %% Scenario Configuration                            
+    deltaT = 0.01;              % timestep
+    timeElapsed = 5;            % Total elapsed time
+    times = 0:deltaT:timeElapsed;
+
+    % Initial Conditions
+    omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
+    radiusEarth = 6378.1*1000;             % radius of Earth is 6,378.1 km
+    
+    if useRealisticValues == 1
+        omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame
+        omegaFixed = [0;0;0];       % constant rotation rate measurement
+        accelFixed = [-0.5;0.5;2];  % constant acceleration measurement
+        g = [0;0;0];                % Gravity
+        % initial position at some [longitude, latitude] location on earth's
+        % surface (approximating Earth as a sphere)
+        initialLongitude = 45;      % longitude in degrees
+        initialLatitude = 30;       % latitude in degrees
+        initialAltitude = 0;        % Altitude above Earth's surface in meters
+        [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
+        initialPosition = [x; y; z];
+        initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
+                                    % (ignoring the velocity due to the earth's rotation)
+    else
+        omegaRotatingFrame = [0;0;pi/5];  % rotation of the moving frame wrt fixed frame
+        omegaFixed = [0;0;0];       % constant rotation rate measurement
+        accelFixed = [0.5;0.5;0.5];     % constant acceleration measurement
+        g = [0;0;0];                % Gravity
+        initialPosition = [1; 0; 0];% initial position in both frames
+        initialVelocity = [0;0;0];  % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
+    end
+    
+    if navFrameRotating == 0
+      omegaCoriolisIMU = [0;0;0];
+    else
+      omegaCoriolisIMU = omegaRotatingFrame;
+    end
+end
+
+
+% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is
+% position vector and W is angular velocity tensor
+% We add the initial velocity in the rotating frame because they
+% are the same frame at t=0, so no transformation is needed
+angularVelocityTensor = [         0             -omegaRotatingFrame(3)  omegaRotatingFrame(2);
+                         omegaRotatingFrame(3)            0            -omegaRotatingFrame(1);
+                         -omegaRotatingFrame(2) omegaRotatingFrame(1)             0         ];
+initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity;
+initialVelocityRotatingFrame = initialVelocity;
+                     
+%
+currentRotatingFrame = Pose3;   % rotating frame initially coincides with fixed frame at t=0
+currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
+currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0
+currentVelocityFixedGT = initialVelocityFixedFrame;
+currentVelocityRotatingGT = initialVelocityRotatingFrame;
+%
+epsBias = 1e-20;
+sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10);
+sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10);
+sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias);
+
+% Imu metadata
+zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero
+IMU_metadata.AccelerometerSigma = 1e-5;
+IMU_metadata.GyroscopeSigma = 1e-7;
+IMU_metadata.IntegrationSigma = 1e-10;
+IMU_metadata.BiasAccelerometerSigma = epsBias;
+IMU_metadata.BiasGyroscopeSigma = epsBias;
+IMU_metadata.BiasAccOmegaInit = epsBias;
+
+%% Initialize storage variables
+positionsInFixedGT = zeros(3, length(times));
+velocityInFixedGT = zeros(3, length(times));
+
+positionsInRotatingGT = zeros(3, length(times));
+velocityInRotatingGT = zeros(3, length(times));
+
+positionsEstimates = zeros(3,length(times));
+velocitiesEstimates = zeros(3,length(times));
+
+rotationsErrorVectors = zeros(3,length(times));   % Rotating/Fixed frame selected later
+
+changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step
+h = figure;
+
+% Solver object
+isamParams = ISAM2Params;
+isamParams.setFactorization('CHOLESKY');
+isamParams.setRelinearizeSkip(10);
+isam = gtsam.ISAM2(isamParams);
+newFactors = NonlinearFactorGraph;
+newValues = Values;
+
+% Video recording object
+if record_movie == 1
+    writerObj = VideoWriter('trajectories.avi');
+    writerObj.Quality = 100; 
+    writerObj.FrameRate = 15; %10; 
+    open(writerObj);
+    set(gca,'nextplot','replacechildren');
+    set(gcf,'Renderer','zbuffer');
+end
+
+%% Print Info about the test
+fprintf('\n-------------------------------------------------\n');
+if navFrameRotating == 0
+    fprintf('Performing navigation in the Fixed frame.\n');
+else
+    fprintf('Performing navigation in the Rotating frame.\n');
+end
+fprintf('Estimation Enabled = %d\n', estimationEnabled);
+fprintf('IMU_type = %d\n', IMU_type);
+fprintf('Record Movie = %d\n', record_movie);
+fprintf('Realistic Values = %d\n', useRealisticValues);
+fprintf('deltaT = %f\n', deltaT);
+fprintf('timeElapsed = %f\n', timeElapsed);
+fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3));
+fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3));
+fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3));
+fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3));
+if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var'))
+    fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ...
+        initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3));
+else
+    fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3));
+end
+fprintf('\n');
+
+%% Main loop: iterate through the ground truth trajectory, add factors
+% and values to the factor graph, and perform inference
+for i = 1:length(times)
+    t = times(i);
+    
+    % Create keys for current state
+    currentPoseKey = symbol('x', i);
+    currentVelKey = symbol('v', i);
+    currentBiasKey = symbol('b', i);
+    
+    %% Set priors on the first iteration
+    if(i == 1)
+        % known initial conditions
+        currentPoseEstimate = currentPoseFixedGT;
+        if navFrameRotating == 1
+            currentVelocityEstimate = LieVector(currentVelocityRotatingGT);
+        else
+            currentVelocityEstimate = LieVector(currentVelocityFixedGT);
+        end
+        
+        % Set Priors
+        newValues.insert(currentPoseKey, currentPoseEstimate);
+        newValues.insert(currentVelKey, currentVelocityEstimate);
+        newValues.insert(currentBiasKey, zeroBias);
+        % Initial values, same for IMU types 1 and 2
+        newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
+        newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
+        newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
+        
+        % Store data
+        positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
+        velocityInFixedGT(:,1) = currentVelocityFixedGT;
+        positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
+        %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector;
+        positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
+        velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
+        currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
+        currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
+    else
+        
+        %% Create ground truth trajectory
+        % Update the position and velocity
+        % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
+        % v_t = v_0 + a_0*dt
+        currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ...
+            + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
+        currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
+        
+        currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
+        
+        % Rotate pose in fixed frame to get pose in rotating frame
+        previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
+        currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
+        inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
+        currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
+        currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
+        
+        % Get velocity in rotating frame by treating it like a position and using compose 
+        % Maybe Luca knows a better way to do this within GTSAM. 
+        %currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame));
+        %currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT);
+        
+        %currentRot3RotatingGT = currentRotatingFrame.rotation();
+        %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT));
+        % TODO: check if initial velocity in rotating frame is correct
+        currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT;
+        %currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ...
+        %    - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
+        
+        % Store GT (ground truth) poses
+        positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
+        velocityInFixedGT(:,i) = currentVelocityFixedGT;
+        positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
+        velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
+        currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
+        currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
+        
+        %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
+        % Instantiate preintegrated measurements class
+        if IMU_type == 1
+            currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
+                zeroBias, ...
+                IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
+                IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
+                IMU_metadata.IntegrationSigma.^2 * eye(3));
+        elseif IMU_type == 2
+            currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ...
+                zeroBias, ...
+                IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
+                IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
+                IMU_metadata.IntegrationSigma.^2 * eye(3), ...
+                IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ...
+                IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ...
+                IMU_metadata.BiasAccOmegaInit.^2 * eye(6));
+        else
+            error('imuSimulator:coriolisExample:IMU_typeNotFound', ...
+                'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type);
+        end
+        
+        % Add measurement
+        currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT);
+        
+        % Add factors to graph
+        if IMU_type == 1
+            newFactors.add(ImuFactor( ...
+                currentPoseKey-1, currentVelKey-1, ...
+                currentPoseKey, currentVelKey, ...
+                currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU));
+            newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ...
+                noiseModel.Isotropic.Sigma(6, epsBias)));
+            newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ...
+                noiseModel.Isotropic.Sigma(6, epsBias)));
+        elseif IMU_type == 2
+            newFactors.add(CombinedImuFactor( ...
+                currentPoseKey-1, currentVelKey-1, ...
+                currentPoseKey, currentVelKey, ...
+                currentBiasKey-1, currentBiasKey, ...
+                currentSummarizedMeasurement, g, omegaCoriolisIMU, ...
+                noiseModel.Isotropic.Sigma(15, epsBias)));
+            % TODO: prior on biases?
+        end
+
+        % Add values to the graph. Use the current pose and velocity
+        % estimates as to values when interpreting the next pose and
+        % velocity estimates
+        %ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU);
+        newValues.insert(currentPoseKey, currentPoseEstimate);
+        newValues.insert(currentVelKey, currentVelocityEstimate);
+        newValues.insert(currentBiasKey, zeroBias);
+        
+        %newFactors.print(''); newValues.print('');
+        
+        %% Solve factor graph
+        if(i > 1 && estimationEnabled)
+            isam.update(newFactors, newValues);
+            newFactors = NonlinearFactorGraph;
+            newValues = Values;
+            
+            % Get the new pose, velocity, and bias estimates
+            currentPoseEstimate = isam.calculateEstimate(currentPoseKey);
+            currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
+            currentBias = isam.calculateEstimate(currentBiasKey);
+            
+            positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
+            velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
+            biasEstimates(:,i) = currentBias.vector;
+            
+            % In matrix form: R_error = R_gt'*R_estimate
+            % Perform Logmap on the rotation matrix to get a vector
+            if navFrameRotating == 1
+                rotationError = Rot3(currentPoseRotatingGT.rotation.matrix' * currentPoseEstimate.rotation.matrix);
+            else
+                rotationError = Rot3(currentPoseFixedGT.rotation.matrix' * currentPoseEstimate.rotation.matrix);
+            end
+            
+            rotationsErrorVectors(:,i) = Rot3.Logmap(rotationError);
+        end
+    end
+    
+    %% incremental plotting for animation (ground truth)
+    if incrementalPlotting == 1
+        figure(h)
+        %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
+        %hold on;
+        plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r');
+        hold on;
+        plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
+        plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or');
+
+        plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g');
+        plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
+        plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og');
+
+        if(estimationEnabled)
+          plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
+          plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
+          plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
+        end
+
+        hold off;
+        xlabel('X axis')
+        ylabel('Y axis')
+        zlabel('Z axis')
+        axis equal
+        grid on;
+        %pause(0.1);
+
+        % record frames for movie
+        if record_movie == 1
+            frame = getframe(gcf); 
+            writeVideo(writerObj,frame);
+        end
+    end
+end
+
+if record_movie == 1
+    close(writerObj);
+end
+
+% Calculate trajectory length
+trajectoryLengthEstimated = 0;
+trajectoryLengthFixedFrameGT = 0;
+trajectoryLengthRotatingFrameGT = 0;
+for i = 2:length(positionsEstimates)
+    trajectoryLengthEstimated = trajectoryLengthEstimated + norm(positionsEstimates(:,i) - positionsEstimates(:,i-1));
+    trajectoryLengthFixedFrameGT = trajectoryLengthFixedFrameGT + norm(positionsInFixedGT(:,i) - positionsInFixedGT(:,i-1));
+    trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1));
+end
+
+%% Print and plot error results
+% Calculate errors for appropriate navigation scheme
+if navFrameRotating == 0
+    axisPositionsError = positionsInFixedGT - positionsEstimates;
+    axisVelocityError = velocityInFixedGT - velocitiesEstimates;
+else
+    axisPositionsError = positionsInRotatingGT - positionsEstimates;
+    axisVelocityError = velocityInRotatingGT - velocitiesEstimates;
+end
+
+% Plot individual axis position errors
+figure
+plot(times, axisPositionsError);
+plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
+    IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+title(plotTitle);
+xlabel('Time [s]');
+ylabel('Error (ground_truth - estimate) [m]');
+legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
+
+% Plot 3D position error
+figure
+positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2);
+plot(times, positionError3D);
+plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
+    IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+title(plotTitle);
+xlabel('Time [s]');
+ylabel('3D error [meters]');
+
+% Plot 3D position error
+if navFrameRotating == 0
+    trajLen = trajectoryLengthFixedFrameGT;
+else
+    trajLen = trajectoryLengthRotatingFrameGT;
+end
+
+figure
+plot(times, (positionError3D/trajLen)*100);
+plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
+    IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+title(plotTitle);
+xlabel('Time [s]');
+ylabel('normalized 3D error [% of distance traveled]');
+
+% Plot individual axis velocity errors
+figure
+plot(times, axisVelocityError);
+plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
+    IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+title(plotTitle);
+xlabel('Time [s]');
+ylabel('Error (ground_truth - estimate) [m/s]');
+legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
+
+% Plot 3D velocity error
+figure
+velocityError3D = sqrt(axisVelocityError(1,:).^2+axisVelocityError(2,:).^2 + axisVelocityError(3,:).^2);
+plot(times, velocityError3D);
+plotTitle = sprintf('3D Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
+    IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
+title(plotTitle);
+xlabel('Time [s]');
+ylabel('3D error [meters/s]');
+
+% Plot magnitude of rotation errors
+figure
+for i = 1:size(rotationsErrorVectors,2)
+    rotationsErrorMagnitudes(i) = norm(rotationsErrorVectors(:,i));
+end
+plot(times,rotationsErrorMagnitudes);
+title('Rotation Error');
+xlabel('Time [s]');
+ylabel('Error [rads]');
+
+
+% Text output for errors
+if navFrameRotating == 0
+    fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT);
+    fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated);
+    fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ...
+        norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100);
+else
+    fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT);
+    fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated);
+    fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ...
+        norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100);
+end
+fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end));
+fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end)));
+
+%% Plot final trajectories
+figure
+[x,y,z] = sphere(30);
+if useRealisticValues
+    mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere  % sphere for reference
+else
+    mesh(x,y,z);
+end
+hold on;
+axis equal
+% Ground truth trajectory in fixed reference frame
+plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4);
+plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4);
+plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4);
+
+% Ground truth trajectory in rotating reference frame
+plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4);
+plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4);
+plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4);
+
+% Estimates
+if(estimationEnabled)
+  plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4);
+  plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4);
+  plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4);
+end
+
+xlabel('X axis')
+ylabel('Y axis')
+zlabel('Z axis')
+axis equal
+grid on;
+hold off;
+
+% TODO: logging rotation errors
+%for all time steps
+%    Rerror =  Rgt' * Restimated;
+%    % transforming rotation matrix to axis-angle representation
+%    vector_error = Rot3.Logmap(Rerror);
+%    norm(vector_error)
+%    
+%   axis angle: [u,theta], with norm(u)=1
+%   vector_error = u * theta;
+   
+% TODO: logging velocity errors
+%velocities..

matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m

@@ -0,0 +1,182 @@
+clc
+clear all
+close all
+
+% Flag to mark external configuration to the main test script
+externalCoriolisConfiguration = 1;
+
+%% General configuration
+navFrameRotating = 0;       % 0 = perform navigation in the fixed frame
+                            % 1 = perform navigation in the rotating frame
+IMU_type = 1;               % IMU type 1 or type 2
+useRealisticValues = 1;     % use reaslist values for initial position and earth rotation
+record_movie = 0;           % 0 = do not record movie
+                            % 1 = record movie of the trajectories. One
+                            % frame per time step (15 fps)
+incrementalPlotting = 0;
+estimationEnabled = 1;
+
+%% Scenario Configuration                            
+deltaT = 0.01;              % timestep
+timeElapsed = 5;            % Total elapsed time
+times = 0:deltaT:timeElapsed;
+
+% Initial Conditions
+omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
+radiusEarth = 6378.1*1000;             % radius of Earth is 6,378.1 km
+
+if useRealisticValues == 1
+    omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame
+    omegaFixed = [0;0;0];       % constant rotation rate measurement
+    accelFixed = [0.1;0;1];  % constant acceleration measurement
+    g = [0;0;0];                % Gravity
+    
+    initialLongitude = 45;      % longitude in degrees
+    initialLatitude = 30;       % latitude in degrees
+    initialAltitude = 0;        % Altitude above Earth's surface in meters
+    [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
+    initialPosition = [x; y; z];
+    
+    initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
+                                % (ignoring the velocity due to the earth's rotation)
+                                
+else
+    omegaRotatingFrame = [0;0;pi/300];  % rotation of the moving frame wrt fixed frame
+    omegaFixed = [0;0;0];       % constant rotation rate measurement
+    accelFixed = [1;0;0];     % constant acceleration measurement
+    g = [0;0;0];                % Gravity
+    initialPosition = [0; 1; 0];% initial position in both frames
+    initialVelocity = [0;0;0];  % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
+end
+
+%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% Run tests with randomly generated initialPosition and accelFixed values
+% For each initial position, a number of acceleration vectors are
+% generated. For each (initialPosition, accelFixed) pair, the coriolis test
+% is run for the following 3 scenarios
+%   - Navigation performed in fixed frame
+%   - Navigation performed in rotating frame, including the coriolis effect
+%   - Navigation performed in rotating frame, ignoring coriolis effect
+%
+
+% Testing configuration
+numPosTests = 1;
+numAccelTests = 10;
+totalNumTests = numPosTests * numAccelTests;
+
+% Storage variables
+
+posFinErrorFixed = zeros(numPosTests, numAccelTests);
+rotFinErrorFixed = zeros(numPosTests, numAccelTests);
+velFinErrorFixed = zeros(numPosTests, numAccelTests);
+
+posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
+rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
+velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
+
+posFinErrorRot = zeros(numPosTests, numAccelTests);
+rotFinErrorRot = zeros(numPosTests, numAccelTests);
+velFinErrorRot = zeros(numPosTests, numAccelTests);
+
+
+numErrors = 0;
+testIndPos = 1;
+testIndAccel = 1;
+
+while testIndPos <= numPosTests
+    %generate a random initial position vector
+    initialLongitude = rand()*360 - 180;    % longitude in degrees (-90 to 90)
+    initialLatitude = rand()*180 - 90;      % latitude in degrees (-180 to 180)
+    initialAltitude = rand()*150;           % Altitude above Earth's surface in meters (0-150)
+    [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
+    initialPosition = [x; y; z];
+    
+    while testIndAccel <= numAccelTests
+        [testIndPos testIndAccel]
+        % generate a random acceleration vector
+        accelFixed = 2*rand(3,1)-ones(3,1);
+        
+        %lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA;
+        %initialLongitude = lla(1);
+        %initialLatitude = lla(2);
+        %initialAltitude = lla(3);
+        %initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF;
+        
+        testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude]; 
+        testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition;
+        testInfo(testIndPos, testIndAccel).accelFixed = accelFixed;
+        
+        try
+            omegaCoriolisIMU = [0;0;0];
+            navFrameRotating = 0;
+            imuSimulator.coriolisExample
+            posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100;
+            rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
+            velFinErrorFixed(testIndPos, testIndAccel) =  norm(axisVelocityError(:,end));
+            testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end);
+            testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end);
+            testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end);
+            testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated;
+            testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT;
+            testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT;
+            
+            close all;
+            
+            % Run the same initial conditions but navigating in the rotating frame
+            %  enable coriolis effect by setting:
+            omegaCoriolisIMU = omegaRotatingFrame;
+            navFrameRotating = 1;
+            imuSimulator.coriolisExample
+            posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
+            rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
+            velFinErrorRotCoriolis(testIndPos, testIndAccel) =  norm(axisVelocityError(:,end));
+            testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end);
+            testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end);
+            testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end);
+            testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated;
+            
+            close all;
+            
+            % Run the same initial conditions but navigating in the rotating frame
+            % disable coriolis effect by setting:
+            omegaCoriolisIMU = [0;0;0];
+            navFrameRotating = 1;
+            imuSimulator.coriolisExample
+            posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
+            rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
+            velFinErrorRot(testIndPos, testIndAccel) =  norm(axisVelocityError(:,end));
+            testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end);
+            testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end);
+            testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end);
+            testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated;
+            
+            close all;
+        catch
+            numErrors = numErrors + 1;
+            fprintf('*ERROR*');
+            fprintf('%d tests cancelled due to errors\n', numErrors);
+            fprintf('restarting test with new accelFixed');
+            testIndAccel = testIndAccel - 1;
+        end
+        testIndAccel = testIndAccel + 1;
+    end
+    testIndAccel = 1;
+    testIndPos = testIndPos + 1;
+end
+fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors);
+
+mean(posFinErrorFixed);
+max(posFinErrorFixed);
+% box(posFinErrorFixed);
+
+mean(posFinErrorRotCoriolis);
+max(posFinErrorRotCoriolis);
+% box(posFinErrorRotCoriolis);
+
+mean(posFinErrorRot);
+max(posFinErrorRot);
+% box(posFinErrorRot);
+
+

matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m

@@ -0,0 +1,325 @@
+import gtsam.*;
+
+% Test GTSAM covariances on a factor graph with:
+% Between Factors
+% IMU factors (type 1 and type 2)
+% GPS prior factors on poses
+% SmartProjectionPoseFactors
+% Authors: Luca Carlone, David Jensen
+% Date: 2014/4/6
+
+
+% Check for an extneral configuration, used when running multiple tests
+if ~exist('externallyConfigured', 'var')
+  clc
+  clear all
+  close all
+  
+  saveResults = 0;
+  
+  %% Configuration
+  % General options
+  options.useRealData = 1;           % controls whether or not to use the real data (if available) as the ground truth traj
+  options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses
+  
+  options.includeIMUFactors = 1;     % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
+  options.imuFactorType = 1;         % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
+  options.imuNonzeroBias = 0;        % if true, a nonzero bias is applied to IMU measurements
+  
+  options.includeCameraFactors = 1;  % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks
+  options.numberOfLandmarks = 1000;  % Total number of visual landmarks (randomly generated in a box around the trajectory)
+  
+  options.includeGPSFactors = 0;     % if true, GPS factors will be added as priors to poses
+  options.gpsStartPose = 100;        % Pose number to start including GPS factors at
+  
+  options.trajectoryLength = 100;%209;    % length of the ground truth trajectory
+  options.subsampleStep = 20;        % number of poses to skip when using real data (to reduce computation on long trajectories)
+  
+  numMonteCarloRuns = 2;             % number of Monte Carlo runs to perform
+  
+  % Noise values to be adjusted
+  sigma_ang = 1e-2;       % std. deviation for rotational noise, typical 1e-2
+  sigma_cart = 1e-1;      % std. deviation for translational noise, typical 1e-1
+  sigma_accel = 1e-3;     % std. deviation for accelerometer noise, typical 1e-3
+  sigma_gyro = 1e-5;      % std. deviation for gyroscope noise, typical 1e-5
+  sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3
+  sigma_gyroBias = 1e-6;  % std. deviation for added gyroscope constant bias, typical 1e-5
+  sigma_gps = 1e-4;       % std. deviation for noise in GPS position measurements, typical 1e-4
+  sigma_camera = 1;  % std. deviation for noise in camera measurements (pixels)
+  
+  % Set log files
+  testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro)
+  folderName = 'results/'
+else
+  fprintf('Tests have been externally configured.\n');
+end
+
+%% Between metadata
+noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)];
+noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
+
+%% Imu metadata
+metadata.imu.epsBias = 1e-10; % was 1e-7
+metadata.imu.g = [0;0;0];
+metadata.imu.omegaCoriolis = [0;0;0];
+metadata.imu.IntegrationSigma = 1e-5;
+metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
+metadata.imu.AccelerometerSigma = sigma_accel;
+metadata.imu.GyroscopeSigma = sigma_gyro;
+metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias;  % noise on expected change in accelerometer bias over time
+metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias;      % noise on expected change in gyroscope bias over time
+% noise on initial accelerometer and gyroscope biases
+if options.imuNonzeroBias == 1
+  metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)];
+else
+  metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1);
+end
+
+noiseVel =  noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
+noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);...
+                                               metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases
+noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit);
+
+noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1);
+noiseVectorGyro = metadata.imu.GyroscopeSigma  * ones(3,1);
+
+%% GPS metadata
+noiseVectorGPS = sigma_gps * ones(3,1);
+noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]);
+
+%% Camera metadata
+metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/2); % Camera calibration
+metadata.camera.xlims = [-100, 650];    % x limits on area for landmark creation
+metadata.camera.ylims = [-100, 700];    % y limits on area for landmark creation
+metadata.camera.zlims = [-30, 30];      % z limits on area for landmark creation
+metadata.camera.visualRange = 100;      % maximum distance from the camera that a landmark can be seen (meters)
+metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body
+metadata.camera.CameraSigma = sigma_camera;
+cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma);
+noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1);
+
+% Create landmarks and smart factors
+if options.includeCameraFactors == 1
+  for i = 1:options.numberOfLandmarks
+    metadata.camera.gtLandmarkPoints(i) = Point3( ...
+      [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ...  
+       rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ...
+       rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]);
+  end
+end
+
+
+%% Create ground truth trajectory and measurements
+[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata);
+
+%% Create ground truth graph
+% Set up noise models
+gtNoiseModels.noisePose = noisePose;
+gtNoiseModels.noiseVel = noiseVel;
+gtNoiseModels.noiseBiasBetween = noiseBiasBetween;
+gtNoiseModels.noisePriorPose = noisePose;
+gtNoiseModels.noisePriorBias = noisePriorBias;
+gtNoiseModels.noiseGPS = noiseGPS;
+gtNoiseModels.noiseCamera = cameraMeasurementNoise;
+
+% Set measurement noise to 0, because this is ground truth
+gtMeasurementNoise.poseNoiseVector = zeros(6,1);
+gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1);
+gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1);
+gtMeasurementNoise.cameraNoiseVector = zeros(2,1);
+gtMeasurementNoise.gpsNoiseVector = zeros(3,1);
+  
+% Set IMU biases to zero
+metadata.imu.accelConstantBiasVector = zeros(3,1);
+metadata.imu.gyroConstantBiasVector = zeros(3,1);
+    
+[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
+    gtMeasurements, ...     % ground truth measurements
+    gtValues, ...           % ground truth Values
+    gtNoiseModels, ...      % noise models to use in this graph
+    gtMeasurementNoise, ... % noise to apply to measurements
+    options, ...            % options for the graph (e.g. which factors to include)
+    metadata);              % misc data necessary for factor creation
+
+%% Display, printing, and plotting of ground truth
+%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
+%gtValues.print(sprintf('\nGround Truth Values:\n  '));
+
+figure(1)
+hold on;
+
+if options.includeCameraFactors
+  b = [-1000 2000 -2000 2000 -30 30];
+  for i = 1:size(metadata.camera.gtLandmarkPoints,2)
+      p = metadata.camera.gtLandmarkPoints(i).vector;
+      if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
+          plot3(p(1), p(2), p(3), 'k+');
+      end
+  end
+  pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
+  for i = 1:length(pointsToPlot)
+      p = pointsToPlot(i).vector;
+      plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
+  end
+end
+plot3DPoints(gtValues);
+%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
+plot3DTrajectory(gtValues, '-r');
+
+axis equal
+
+% optimize
+optimizer = GaussNewtonOptimizer(gtGraph, gtValues);
+gtEstimate = optimizer.optimize();
+plot3DTrajectory(gtEstimate, '-k');
+% estimate should match gtValues if graph is correct.
+fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) );
+fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) );
+
+disp('Plotted ground truth')
+
+%% Monte Carlo Runs
+
+% Set up noise models
+monteCarloNoiseModels.noisePose = noisePose;
+monteCarloNoiseModels.noiseVel = noiseVel;
+monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween;
+monteCarloNoiseModels.noisePriorPose = noisePose;
+monteCarloNoiseModels.noisePriorBias = noisePriorBias;
+monteCarloNoiseModels.noiseGPS = noiseGPS;
+monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise;
+
+% Set measurement noise for monte carlo runs
+monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose;
+monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel;
+monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro;
+monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS;
+monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera;
+  
+for k=1:numMonteCarloRuns
+  fprintf('Monte Carlo Run %d...\n', k');
+
+  % Create a random bias for each run
+  if options.imuNonzeroBias == 1
+    metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1);
+    metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1);
+    %metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1);
+    %metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1);
+  else
+    metadata.imu.accelConstantBiasVector = zeros(3,1);
+    metadata.imu.gyroConstantBiasVector = zeros(3,1);
+  end
+  
+  % Create a new graph using noisy measurements
+  [graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
+    gtMeasurements, ...     % ground truth measurements
+    gtValues, ...           % ground truth Values
+    monteCarloNoiseModels, ...      % noise models to use in this graph
+    monteCarloMeasurementNoise, ... % noise to apply to measurements
+    options, ...            % options for the graph (e.g. which factors to include)
+    metadata);              % misc data necessary for factor creation
+      
+  %graph.print('graph')
+  
+  % optimize
+  optimizer = GaussNewtonOptimizer(graph, gtValues);
+  estimate = optimizer.optimize();
+  figure(1)
+  plot3DTrajectory(estimate, '-b');
+  
+  marginals = Marginals(graph, estimate);
+  
+  % for each pose in the trajectory
+  for i=0:options.trajectoryLength
+    % compute estimation errors
+    currentPoseKey = symbol('x', i);
+    gtPosition  = gtValues.at(currentPoseKey).translation.vector;
+    estPosition = estimate.at(currentPoseKey).translation.vector;
+    estR = estimate.at(currentPoseKey).rotation.matrix;
+    errPosition = estPosition - gtPosition;
+    
+    % compute covariances:
+    cov = marginals.marginalCovariance(currentPoseKey);
+    covPosition = estR * cov(4:6,4:6) * estR';
+    % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
+    NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
+  end
+  
+  figure(2)
+  hold on
+  plot(NEES(k,:),'-b','LineWidth',1.5)
+end
+%%
+ANEES = mean(NEES);
+plot(ANEES,'-r','LineWidth',2)
+plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof
+box on
+set(gca,'Fontsize',16)
+title('NEES and ANEES');
+if saveResults
+  saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig');
+  saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png');
+end
+
+%%
+figure(1)
+box on
+set(gca,'Fontsize',16)
+title('Ground truth and estimates for each MC runs');
+if saveResults
+  saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig');
+  saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png');
+end
+
+%% Let us compute statistics on the overall NEES
+n = 3; % position vector dimension
+N = numMonteCarloRuns; % number of runs
+alpha = 0.01; % confidence level
+
+% mean_value = n*N; % mean value of the Chi-square distribution
+% (we divide by n * N and for this reason we expect ANEES around 1)
+r1 = chi2inv(alpha, n * N)  / (n * N);
+r2 = chi2inv(1-alpha, n * N)  / (n * N);
+
+% output here
+fprintf(1, 'r1 = %g\n', r1);
+fprintf(1, 'r2 = %g\n', r2);
+
+figure(3)
+hold on
+plot(ANEES/n,'-b','LineWidth',2)
+plot(ones(size(ANEES,2),1),'r-');
+plot(r1*ones(size(ANEES,2),1),'k-.');
+plot(r2*ones(size(ANEES,2),1),'k-.');
+box on
+set(gca,'Fontsize',16)
+title('NEES normalized by dof VS bounds');
+if saveResults
+  saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
+  saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png');
+  logFile = horzcat(folderName,'log-',testName);
+  save(logFile)
+end
+
+%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
+% the nees for a single experiment (i) is defined as
+%               NEES_i = xtilda' * inv(P) * xtilda,
+% where xtilda in R^n is the estimation
+% error, and P is the covariance estimated by the approach we want to test
+%
+% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average
+% NEES is:
+%                   ANEES = sum(NEES_i)/N
+% The quantity N*ANEES is distributed according to a Chi-square
+% distribution with N*n degrees of freedom.
+%
+% For the single run case, N=1, therefore NEES = ANEES is distributed
+% according to a chi-square distribution with n degrees of freedom (e.g. n=3
+% if we are testing a position estimate)
+% Therefore its mean should be n (difficult to see from a single run)
+% and, with probability alpha, it should hold:
+%
+% NEES in [r1, r2]
+%
+% where r1 and r2 are built from the Chi-square distribution
+

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m

@@ -0,0 +1,199 @@
+function [ graph projectionFactorSeenBy] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
+% Create a factor graph based on provided measurements, values, and noises.
+% Used for covariance analysis scripts.
+% 'options' contains fields for including various factor types.
+% 'metadata' is a storage variable for miscellaneous factor-specific values
+% Authors: Luca Carlone, David Jensen
+% Date: 2014/04/16
+
+import gtsam.*;
+
+graph = NonlinearFactorGraph;
+
+% Iterate through the trajectory
+for i=0:length(measurements)
+  % Get the current pose
+  currentPoseKey = symbol('x', i);
+  currentPose = values.at(currentPoseKey);
+  
+  if i==0
+    %% first time step, add priors
+    % Pose prior (poses used for all factors)
+    noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1); 
+    initialPose = Pose3.Expmap(noisyInitialPoseVector);
+    graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
+    
+    % IMU velocity and bias priors
+    if options.includeIMUFactors == 1
+      currentVelKey = symbol('v', 0);
+      currentVel = values.at(currentVelKey).vector;
+      graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
+      
+      currentBiasKey = symbol('b', 0);
+      currentBias = values.at(currentBiasKey);
+      graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
+    end
+    
+    %% Create a SmartProjectionFactor for each landmark
+    projectionFactorSeenBy = [];
+    if options.includeCameraFactors == 1
+      for j=1:options.numberOfLandmarks
+        SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01);
+        % Use constructor with default values, but express the pose of the
+        % camera as a 90 degree rotation about the X axis
+%         SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
+%             1, ...      % rankTol
+%             -1, ...     % linThreshold
+%             false, ...  % manageDegeneracy
+%             false, ...  % enableEPI
+%             metadata.camera.bodyPoseCamera);    % Pose of camera in body frame
+      end
+      projectionFactorSeenBy = zeros(options.numberOfLandmarks,1);
+    end
+    
+  else
+    
+    %% Add Between factors
+    if options.includeBetweenFactors == 1
+      prevPoseKey = symbol('x', i-1);
+      % Create the noisy pose estimate
+      deltaPose = Pose3.Expmap(measurements(i).deltaVector ...
+        + (measurementNoise.poseNoiseVector .* randn(6,1)));  % added noise
+      % Add the between factor to the graph
+      graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
+    end % end of Between pose factor creation
+    
+    %% Add IMU factors
+    if options.includeIMUFactors == 1
+      % Update keys
+      currentVelKey = symbol('v', i);  % not used if includeIMUFactors is false
+      currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
+      
+      if options.imuFactorType == 1
+        use2ndOrderIntegration = true;
+        % Initialize preintegration
+        imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
+          metadata.imu.zeroBias, ...
+          metadata.imu.AccelerometerSigma.^2 * eye(3), ...
+          metadata.imu.GyroscopeSigma.^2 * eye(3), ...
+          metadata.imu.IntegrationSigma.^2 * eye(3), ...
+          use2ndOrderIntegration);
+        % Generate IMU measurements with noise
+        for j=1:length(measurements(i).imu) % all measurements to preintegrate
+          accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1));
+          imuAccel = measurements(i).imu(j).accel ...
+            + accelNoise ...  % added noise
+            + metadata.imu.accelConstantBiasVector;     % constant bias
+        
+          gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1));
+          imuGyro = measurements(i).imu(j).gyro ...
+            + gyroNoise ...   % added noise
+            + metadata.imu.gyroConstantBiasVector;      % constant bias
+          
+          % Used for debugging
+          %fprintf('  A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
+          %    measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ...
+          %    accelNoise(1), accelNoise(2), accelNoise(3), ...
+          %    metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ...
+          %    imuAccel(1), imuAccel(2), imuAccel(3));
+          %fprintf('  G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
+          %    measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ...
+          %    gyroNoise(1), gyroNoise(2), gyroNoise(3), ...
+          %    metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ...
+          %    imuGyro(1), imuGyro(2), imuGyro(3));
+          
+          % Preintegrate
+          imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
+        end
+        %imuMeasurement.print('imuMeasurement');
+        
+        % Add Imu factor
+        graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
+          currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
+        % Add between factor on biases
+        graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
+          noiseModels.noiseBiasBetween));
+      end
+          
+      if options.imuFactorType == 2
+        use2ndOrderIntegration = true;
+        % Initialize preintegration
+        imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
+          metadata.imu.zeroBias, ...
+          metadata.imu.AccelerometerSigma.^2 * eye(3), ...
+          metadata.imu.GyroscopeSigma.^2 * eye(3), ...
+          metadata.imu.IntegrationSigma.^2 * eye(3), ...
+          metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...  % how bias changes over time
+          metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...      % how bias changes over time
+          diag(metadata.imu.BiasAccOmegaInit.^2), ...           % prior on bias
+          use2ndOrderIntegration);
+        % Generate IMU measurements with noise
+        for j=1:length(measurements(i).imu) % all measurements to preintegrate
+          imuAccel = measurements(i).imu(j).accel ...
+            + (measurementNoise.imu.accelNoiseVector .* randn(3,1))...  % added noise
+            + metadata.imu.accelConstantBiasVector;     % constant bias
+          imuGyro = measurements(i).imu(j).gyro ...
+            + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))...   % added noise
+            + metadata.imu.gyroConstantBiasVector;      % constant bias
+          
+          % Preintegrate
+          imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
+        end
+        
+        % Add Imu factor
+        graph.add(CombinedImuFactor( ...
+          currentPoseKey-1, currentVelKey-1, ...
+          currentPoseKey, currentVelKey, ...
+          currentBiasKey-1, currentBiasKey, ...
+          imuMeasurement, ...
+          metadata.imu.g, metadata.imu.omegaCoriolis));
+      end
+      
+    end % end of IMU factor creation
+    
+    %% Build Camera Factors
+    if options.includeCameraFactors == 1        
+      for j = 1:length(measurements(i).landmarks)
+        cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1);
+        cameraPixelMeasurement = measurements(i).landmarks(j).vector;
+        % Only add the measurement if it is in the image frame (based on calibration)
+        if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
+             && cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...
+             && cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py)
+          cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise;
+          SmartProjectionFactors(j).add( ...
+             Point2(cameraPixelMeasurement), ...
+             currentPoseKey, noiseModels.noiseCamera, ...
+             metadata.camera.calibration);
+           projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1;
+        end
+      end
+    end % end of Camera factor creation
+    
+    %% Add GPS factors
+    if options.includeGPSFactors == 1 && i >= options.gpsStartPose
+      gpsPosition = measurements(i).gpsPositionVector ...
+          + measurementNoise.gpsNoiseVector .* randn(3,1);
+      graph.add(PriorFactorPose3(currentPoseKey, ...
+          Pose3(currentPose.rotation, Point3(gpsPosition)), ...
+          noiseModels.noiseGPS)); 
+    end % end of GPS factor creation
+    
+  end % end of else (i=0)
+  
+end % end of for over trajectory
+
+%% Add Camera Factors to the graph
+% Only factors for landmarks that have been viewed at least once are added
+% to the graph
+%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
+if options.includeCameraFactors == 1
+  for j = 1:options.numberOfLandmarks
+    if projectionFactorSeenBy(j) > 0
+      graph.add(SmartProjectionFactors(j));
+    end
+  end
+end
+
+end % end of function
+

matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m

@@ -0,0 +1,150 @@
+function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
+% Create a trajectory for running covariance analysis scripts.
+% 'options' contains fields for including various factor types and setting trajectory length
+% 'metadata' is a storage variable for miscellaneous factor-specific values
+% Authors: Luca Carlone, David Jensen
+% Date: 2014/04/16
+
+import gtsam.*;
+
+values = Values;
+
+warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors')
+
+
+if options.useRealData == 1
+  %% Create a ground truth trajectory from Real data (if available)
+  fprintf('\nUsing real data as ground truth\n');
+  gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
+    'VEast', 'VNorth', 'VUp');
+  
+  % Limit the trajectory length
+  options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]);
+  fprintf('Scenario Ind: ');
+  
+  for i=0:options.trajectoryLength
+    scenarioInd = options.subsampleStep * i + 1;
+    fprintf('%d, ', scenarioInd);
+    if (mod(i,12) == 0) fprintf('\n'); end
+    
+    %% Generate the current pose
+    currentPoseKey = symbol('x', i);
+    currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
+    
+    %% FOR TESTING - this is currently moved to getPoseFromGtScenario
+    %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
+    %currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
+    
+    % add to values
+    values.insert(currentPoseKey, currentPose);
+    
+    %% gt Between measurements
+    if options.includeBetweenFactors == 1 && i > 0
+      prevPose = values.at(currentPoseKey - 1);
+      deltaPose = prevPose.between(currentPose);
+      measurements(i).deltaVector = Pose3.Logmap(deltaPose);
+    end
+    
+    %% gt IMU measurements
+    if options.includeIMUFactors == 1
+      currentVelKey = symbol('v', i);
+      currentBiasKey = symbol('b', i);
+      deltaT = 1;   % amount of time between IMU measurements
+      if i == 0
+        currentVel = [0 0 0]';
+      else
+        % integrate & store intermediate measurements       
+        for j=1:options.subsampleStep % we integrate all the intermediate measurements
+          previousScenarioInd = options.subsampleStep * (i-1) + 1;
+          scenarioIndIMU1 = previousScenarioInd+j-1;
+          scenarioIndIMU2 = previousScenarioInd+j;
+          IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
+          IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
+          IMURot1 = IMUPose1.rotation.matrix;
+                    
+          IMUdeltaPose = IMUPose1.between(IMUPose2);
+          IMUdeltaPoseVector     = Pose3.Logmap(IMUdeltaPose);
+          IMUdeltaRotVector      = IMUdeltaPoseVector(1:3);
+          IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
+          
+          measurements(i).imu(j).deltaT = deltaT;
+          
+          % gyro rate: Logmap(R_i' * R_i+1) / deltaT
+          measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT;
+          
+          % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+          % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
+          measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
+          
+          % Update velocity
+          currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT;
+        end
+      end
+      
+      % Add Values: velocity and bias
+      values.insert(currentVelKey, LieVector(currentVel));
+      values.insert(currentBiasKey, metadata.imu.zeroBias);
+    end
+    
+    %% gt GPS measurements
+    if options.includeGPSFactors == 1 && i > 0
+      gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
+      measurements(i).gpsPositionVector = gpsPositionVector;
+    end
+    
+    %% gt Camera measurements
+    if options.includeCameraFactors == 1 && i > 0     
+      % Create the camera based on the current pose and the pose of the
+      % camera in the body
+      cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
+      camera = SimpleCamera(cameraPose, metadata.camera.calibration);
+      %camera = SimpleCamera(currentPose, metadata.camera.calibration);
+      
+      % Record measurements if the landmark is within visual range
+      for j=1:length(metadata.camera.gtLandmarkPoints)
+        distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j));
+        if distanceToLandmark < metadata.camera.visualRange
+          try
+            z = camera.project(metadata.camera.gtLandmarkPoints(j));
+            measurements(i).landmarks(j) = z;
+          catch
+            % point is probably out of the camera's view
+          end
+        end
+      end
+    end
+    
+  end
+  fprintf('\n');
+else
+  error('Please use RealData')
+  %% Create a random trajectory as ground truth
+  currentPose = Pose3; % initial pose  % initial pose
+  
+  unsmooth_DP = 0.5; % controls smoothness on translation norm
+  unsmooth_DR = 0.1; % controls smoothness on rotation norm
+  
+  fprintf('\nCreating a random ground truth trajectory\n');
+  currentPoseKey = symbol('x', 0);
+  values.insert(currentPoseKey, currentPose);
+  
+  for i=1:options.trajectoryLength
+    % Update the pose key
+    currentPoseKey = symbol('x', i);
+    
+    % Generate the new measurements
+    gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0]
+    gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0]
+    measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
+    
+    % Create the next pose
+    deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)');
+    currentPose = currentPose.compose(deltaPose);
+    
+    % Add the current pose as a value
+    values.insert(currentPoseKey, currentPose);
+  end  % end of random trajectory creation
+end % end of else
+
+end % end of function
+

matlab/unstable_examples/+imuSimulator/ct2ENU.m

@@ -0,0 +1,55 @@
+function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon)
+% CT2LG  Converts CT coordinate differences to local geodetic.
+%   Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz
+%   are referenced to orgin at lat,lon of same index. If
+%   astronomic lat,lon input, output is in local astronomic
+%   system. Vectorized in both dx,dy,dz and lat,lon. See also
+%   LG2CT.
+% Version: 2011-02-19
+% Useage:  [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon)
+% Input:   dX  - vector of X coordinate differences in CT
+%          dY  - vector of Y coordinate differences in CT
+%          dZ  - vector of Z coordinate differences in CT
+%          lat - lat(s) of local system origin (rad); may be vector
+%          lon - lon(s) of local system origin (rad); may be vector
+% Output:  dx  - vector of x coordinates in local system (east)
+%          dy  - vector of y coordinates in local system (north)
+%          dz  - vector of z coordinates in local system (up)
+
+% Copyright (c) 2011, Michael R. Craymer
+% All rights reserved.
+% Email: mike@craymer.com
+
+if nargin ~= 5
+  warning('Incorrect number of input arguements');
+  return
+end
+
+n=length(dX);
+if length(lat)==1
+  lat=ones(n,1)*lat;
+  lon=ones(n,1)*lon;
+end
+R=zeros(3,3,n);
+
+R(1,1,:)=-sin(lat').*cos(lon');
+R(1,2,:)=-sin(lat').*sin(lon');
+R(1,3,:)=cos(lat');
+R(2,1,:)=sin(lon');
+R(2,2,:)=-cos(lon');
+R(2,3,:)=zeros(1,n);
+R(3,1,:)=cos(lat').*cos(lon');
+R(3,2,:)=cos(lat').*sin(lon');
+R(3,3,:)=sin(lat');
+
+RR=reshape(R(1,:,:),3,n);
+dx_temp=sum(RR'.*[dX dY dZ],2);
+RR=reshape(R(2,:,:),3,n);
+dy_temp=sum(RR'.*[dX dY dZ],2);
+RR=reshape(R(3,:,:),3,n);
+dz=sum(RR'.*[dX dY dZ],2);
+
+dx = -dy_temp;
+dy = dx_temp;
+
+

matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m

@@ -0,0 +1,39 @@
+function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd)
+% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw)
+% The function picks the index 'scenarioInd' in those vectors and 
+% computes the corresponding pose by
+% 1) Converting (Lat,Lon,Alt) to local coordinates
+% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix
+
+import gtsam.*;
+
+Org_lat = gtScenario.Lat(1);
+Org_lon = gtScenario.Lon(1);
+initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
+
+gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
+% truth in ENU
+dX = gtECEF(1) - initialPositionECEF(1);
+dY = gtECEF(2) - initialPositionECEF(2);
+dZ = gtECEF(3) - initialPositionECEF(3);
+[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
+
+gtPosition = Point3([xlt, ylt, zlt]');
+% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data
+%   yaw = measured positively to the right
+%   pitch = measured positively up
+%   roll = measured positively to right
+% Assumes vehice X forward, Y right, Z down
+%
+% In the gtScenario data
+%   heading (yaw) = measured positively to the left from Y-axis
+%   pitch = 
+%   roll = 
+% Coordinate frame is Y forward, X is right, Z is up
+gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
+currentPose = Pose3(gtBodyRotation, gtPosition);
+
+%% Rotate the pose
+currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
+
+end

matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m

@@ -0,0 +1,17 @@
+function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ...
+    initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
+%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
+
+import gtsam.*;
+
+imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
+accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector;
+
+finalPosition = Point3(initialPoseGlobal.translation.vector ...
+    + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
+finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
+finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
+finalPose = Pose3(finalRotation, finalPosition);
+
+end
+

matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m

@@ -0,0 +1,26 @@
+function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ...
+    initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body)
+
+% Before integrating in the body frame we need to compensate for the Coriolis 
+% effect
+acc_body =  acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector;
+% after compensating for coriolis this will be essentially zero
+% since we are moving at constant body velocity 
+
+import gtsam.*;
+%% Integrate in the body frame
+% Integrate rotations
+imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
+% Integrate positions
+finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT;
+finalVelocityBody = velocity1Body + acc_body * deltaT; 
+
+%% Express the integrated quantities in the global frame
+finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() );
+finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ;
+finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
+% Include position and rotation in a pose
+finalPose = Pose3(finalRotation, Point3(finalPosition) );
+
+end
+

matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m

@@ -0,0 +1,21 @@
+function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ...
+    initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
+%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
+
+import gtsam.*;
+% Integrate rotations
+imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
+finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
+
+intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 ));
+% Integrate positions (equation (1) in Lupton)
+accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector;
+finalPosition = Point3(initialPoseGlobal.translation.vector ...
+    + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
+finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
+
+% Include position and rotation in a pose
+finalPose = Pose3(finalRotation, finalPosition);
+
+end
+

matlab/unstable_examples/+imuSimulator/integrateTrajectory.m

@@ -0,0 +1,24 @@
+function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ...
+    initialPose, omega1Body, velocity1Body, velocity2Body, deltaT)
+%INTEGRATETRAJECTORY Integrate one trajectory step
+
+import gtsam.*;
+% Rotation: R^1_2
+body2in1 = Rot3.Expmap(omega1Body * deltaT);
+% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
+velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector;
+% Acceleration: a^1 = (v^1_2 - v^1_1)/dt
+accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
+
+% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
+initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector;
+% Acceleration in frame W: a^W = R^W_1 a^1
+accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector;
+
+finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
+finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
+finalRotation = initialPose.rotation.compose(body2in1);
+finalPose = Pose3(finalRotation, finalPosition);
+
+end
+

matlab/unstable_examples/+imuSimulator/runConsistencyTests.m

@@ -0,0 +1,173 @@
+% use this script to easily run and save results for multiple consistency
+% tests without having to pay attention to the computer every 5 minutes
+
+import gtsam.*;
+
+resultsDir = 'results/'
+if (~exist(resultsDir, 'dir'))
+    mkdir(resultsDir);
+end
+
+testOptions = [ ...
+    %    1       2       3       4       5       6       7     8      9        10         11      12
+    % RealData? Between? IMU? IMUType  Bias?  Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns
+         %1        0       1      2       0       0      100    0     100       209          20       100   ;... % 1
+         %1        0       1      2       0       0      100    0     100       209          20       100   ;... % 2
+        % 1        0       1      2       0       0      100    0     100       209          20       100   ;... % 3
+         1        0       1      2       0       1      100    0     100       209          20       20   ;... % 4
+         1        0       1      2       0       1      100    0     100       209          20       20   ;... % 5
+         1        0       1      2       0       0      100    0     100       209          20       20   ];%... % 6
+        % 1        0       1      2       0       0      100    0     100       209          20       100   ;... % 7
+         %1        0       1      2       0       0      100    0     100       209          20       1   ;... % 8
+         %1        0       1      2       0       0      100    0     100       209          20       1   ];   % 9
+   
+noises = [ ...
+    %      1         2          3           4          5               6              7         8
+    % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera
+         %1e-2      1e-1       1e-3        1e-5         0               0            1e-4       1;... % 1
+         %1e-2      1e-1       1e-2        1e-5         0               0            1e-4       1;... % 2
+        % 1e-2      1e-1       1e-1        1e-5         0               0            1e-4       1;... % 3
+         1e-2      1e-1       1e-3        1e-4         0               0            1e-4        1;... % 4
+         1e-2      1e-1       1e-3        1e-3         0               0            1e-4        1;... % 5
+         1e-2      1e-1       1e-3        1e-2         0               0            1e-4        1];%... % 6
+        % 1e-2      1e-1       1e-3        1e-1         0               0            1e-4       1;... % 7
+         %1e-2      1e-1       1e-3        1e-2         1e-3            1e-5         1e-4       1;... % 8
+         %1e-2      1e-1       1e-3        1e-2         1e-4            1e-6         1e-4       1];   % 9
+     
+if(size(testOptions,1) ~= size(noises,1))
+  error('testOptions and noises do not have same number of rows');
+end
+
+% Set flag so the script knows there is an external configuration
+externallyConfigured = 1;
+
+% Set the flag to save the results
+saveResults = 0;
+
+errorRuns = [];
+
+% Go through tests
+for i = 1:size(testOptions,1)
+    % Clean up from last test
+    close all;
+    %clc;
+    
+    % Set up variables for test
+    options.useRealData = testOptions(i,1);
+    options.includeBetweenFactors = testOptions(i,2);
+    options.includeIMUFactors = testOptions(i,3);
+    options.imuFactorType = testOptions(i,4);
+    options.imuNonzeroBias = testOptions(i,5);
+    options.includeCameraFactors = testOptions(i,6);
+    options.numberOfLandmarks = testOptions(i,7);
+    options.includeGPSFactors = testOptions(i,8);
+    options.gpsStartPose = testOptions(i,9);
+    options.trajectoryLength = testOptions(i,10);
+    options.subsampleStep = testOptions(i,11);
+    numMonteCarloRuns = testOptions(i,12);
+    
+    sigma_ang = noises(i,1);
+    sigma_cart = noises(i,2);
+    sigma_accel = noises(i,3);
+    sigma_gyro = noises(i,4);
+    sigma_accelBias = noises(i,5);
+    sigma_gyroBias = noises(i,6);
+    sigma_gps = noises(i,7);
+    sigma_camera = noises(i,8);
+    
+    % Create folder name
+    f_between = '';
+    f_imu = '';
+    f_bias = '';
+    f_gps = '';
+    f_camera = '';
+    f_runs = '';
+    
+    if (options.includeBetweenFactors == 1);
+        f_between = 'between_';
+    end
+    if (options.includeIMUFactors == 1)
+        f_imu = sprintf('imu%d_', options.imuFactorType);
+        if (options.imuNonzeroBias == 1);
+            f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias);
+        end
+    end
+    if (options.includeGPSFactors == 1);
+        f_between = sprintf('gps_%d_', gpsStartPose);
+    end
+    if (options.includeCameraFactors == 1)
+        f_camera = sprintf('camera_%d_', options.numberOfLandmarks);
+    end
+    f_runs = sprintf('mc%d', numMonteCarloRuns);
+    
+    folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/'];
+    
+    % make folder if it doesnt exist
+    if (~exist(folderName, 'dir'))
+        mkdir(folderName);
+    end
+    
+    testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro);
+
+    % Run the test
+    fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName);
+    fprintf('Test Name: %s\n', testName);
+    
+    try
+        imuSimulator.covarianceAnalysisBetween;
+    catch
+        errorRuns = [errorRuns i];
+        fprintf('\n*****\n   Something went wrong, most likely indeterminant linear system error.\n');
+        disp('Test Options:\n');
+        disp(testOptions(i,:));
+        disp('Noises');
+        disp(noises(i,:));
+        fprintf('\n*****\n\n');
+    end
+end
+
+% Print error summary
+fprintf('*************************\n');
+fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns));
+for i = 1:length(errorRuns)
+    k = errorRuns(i);
+    fprintf('\nTest %d:\n', k);
+    fprintf('  options.useRealData = %d\n', testOptions(k,1));
+    fprintf('  options.includeBetweenFactors = %d\n', testOptions(k,2));
+    fprintf('  options.includeIMUFactors = %d\n', testOptions(k,3));
+    fprintf('  options.imuFactorType = %d\n', testOptions(k,4));
+    fprintf('  options.imuNonzeroBias = %d\n', testOptions(k,5));
+    fprintf('  options.includeCameraFactors = %d\n', testOptions(k,6));
+    fprintf('  numberOfLandmarks = %d\n', testOptions(k,7));
+    fprintf('  options.includeGPSFactors = %d\n', testOptions(k,8));
+    fprintf('  options.gpsStartPose = %d\n', testOptions(k,9));
+    fprintf('  options.trajectoryLength = %d\n', testOptions(k,10));
+    fprintf('  options.subsampleStep = %d\n', testOptions(k,11));
+    fprintf('  numMonteCarloRuns = %d\n', testOptions(k,12));
+    fprintf('\n');
+    fprintf('  sigma_ang = %f\n', noises(i,1));
+    fprintf('  sigma_cart = %f\n', noises(i,2));
+    fprintf('  sigma_accel = %f\n', noises(i,3));
+    fprintf('  sigma_gyro = %f\n', noises(i,4));
+    fprintf('  sigma_accelBias = %f\n', noises(i,5));
+    fprintf('  sigma_gyroBias = %f\n', noises(i,6));
+    fprintf('  sigma_gps = %f\n', noises(i,7));
+end
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

matlab/unstable_examples/+imuSimulator/test1onestep.m

@@ -0,0 +1,48 @@
+import gtsam.*;
+
+deltaT = 0.01;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Constant global velocity w/ lever arm
+disp('--------------------------------------------------------');
+disp('Constant global velocity w/ lever arm');
+omega = [0;0;0.1];
+velocity = [1;0;0];
+R = Rot3.Expmap(omega * deltaT);
+
+velocity2body = R.unrotate(Point3(velocity)).vector;
+acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1];
+acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0])));
+disp('IMU measurement discrepancy:');
+disp(acc_omegaActual - acc_omegaExpected);
+
+initialPose = Pose3;
+finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT));
+finalVelocityExpected = velocity;
+[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT);
+disp('Final pose discrepancy:');
+disp(finalPoseExpected.between(finalPoseActual).matrix);
+disp('Final velocity discrepancy:');
+disp(finalVelocityActual - finalVelocityExpected);
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Constant body velocity w/ lever arm
+disp('--------------------------------------------------------');
+disp('Constant body velocity w/ lever arm');
+omega = [0;0;0.1];
+velocity = [1;0;0];
+
+acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1];
+acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0])));
+disp('IMU measurement discrepancy:');
+disp(acc_omegaActual - acc_omegaExpected);
+
+initialPose = Pose3;
+initialVelocity = velocity;
+finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT);
+finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector;
+[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT);
+disp('Final pose discrepancy:');
+disp(finalPoseExpected.between(finalPoseActual).matrix);
+disp('Final velocity discrepancy:');
+disp(finalVelocityActual - finalVelocityExpected);

matlab/unstable_examples/+imuSimulator/test2constglobal.m

@@ -0,0 +1,34 @@
+import gtsam.*;
+
+deltaT = 0.01;
+timeElapsed = 1000;
+
+times = 0:deltaT:timeElapsed;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Constant global velocity w/ lever arm
+disp('--------------------------------------------------------');
+disp('Constant global velocity w/ lever arm');
+omega = [0;0;0.1];
+velocity = [1;0;0];
+
+% Initial state
+currentPoseGlobal = Pose3;
+currentVelocityGlobal = velocity;
+
+% Positions
+positions = zeros(3, length(times)+1);
+
+i = 2;
+for t = times
+    velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector;
+    R = Rot3.Expmap(omega * deltaT);
+    velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector;
+    [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT);
+    
+    positions(:,i) = currentPoseGlobal.translation.vector;
+    i = i + 1;
+end
+
+figure;
+plot(positions(1,:), positions(2,:), '.-');

matlab/unstable_examples/+imuSimulator/test3constbody.m

@@ -0,0 +1,96 @@
+clc
+clear all
+close all
+
+import gtsam.*;
+
+addpath(genpath('./Libraries'))
+
+deltaT = 0.01;
+timeElapsed = 25;
+
+times = 0:deltaT:timeElapsed;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Constant body velocity w/ lever arm
+disp('--------------------------------------------------------');
+disp('Constant body velocity w/ lever arm');
+omega = [0;0;0.1];
+velocity = [1;0;0];
+RIMUinBody = Rot3.Rz(-pi/2);
+% RIMUinBody = eye(3)
+IMUinBody = Pose3(RIMUinBody, Point3([1;0;0]));
+
+% Initial state (body)
+currentPoseGlobal = Pose3;
+currentVelocityGlobal = velocity;
+% Initial state (IMU)
+currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody);
+%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here? 
+currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ...
+     Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector;
+   
+
+% Positions
+% body trajectory
+positions = zeros(3, length(times)+1);
+positions(:,1) = currentPoseGlobal.translation.vector;
+poses(1).p = positions(:,1);
+poses(1).R = currentPoseGlobal.rotation.matrix;
+
+% Expected IMU trajectory (from body trajectory and lever arm)
+positionsIMUe = zeros(3, length(times)+1);
+positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+posesIMUe(1).p = positionsIMUe(:,1);
+posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix;
+
+% Integrated IMU trajectory (from IMU measurements)
+positionsIMU = zeros(3, length(times)+1);
+positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+posesIMU(1).p = positionsIMU(:,1);
+posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
+
+i = 2;
+for t = times
+    [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
+        currentPoseGlobal, omega, velocity, velocity, deltaT);
+    
+    acc_omega = imuSimulator.calculateIMUMeasurement( ...
+        omega, omega, velocity, velocity, deltaT, IMUinBody);
+    
+    [ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ...
+        currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT);
+
+    % Store data in some structure for statistics and plots  
+    positions(:,i) = currentPoseGlobal.translation.vector;
+    positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector;
+    positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
+    
+    poses(i).p = positions(:,i);
+    posesIMUe(i).p = positionsIMUe(:,i);
+    posesIMU(i).p = positionsIMU(:,i);   
+    
+    poses(i).R = currentPoseGlobal.rotation.matrix;
+    posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix;
+    posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;      
+    i = i + 1;   
+end
+
+
+figure(1)
+plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1)
+hold on
+plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1)
+
+figure(2)
+hold on;
+plot(positions(1,:), positions(2,:), '-b');
+plot(positionsIMU(1,:), positionsIMU(2,:), '-r');
+plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k');
+axis equal;
+
+disp('Mismatch between final integrated IMU position and expected IMU position')
+disp(norm(posesIMUe(end).p - posesIMU(end).p))
+disp('Mismatch between final integrated IMU orientation and expected IMU orientation')
+disp(norm(posesIMUe(end).R - posesIMU(end).R))
+

matlab/unstable_examples/+imuSimulator/test4circle.m

@@ -0,0 +1,97 @@
+clc
+clear all
+close all
+
+import gtsam.*;
+
+deltaT = 0.001;
+timeElapsed = 1;
+times = 0:deltaT:timeElapsed;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('--------------------------------------------------------');
+disp('Integration in body frame VS integration in navigation frame');
+disp('TOY EXAMPLE:');
+disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)');
+disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame')
+disp('- Navigation frame is assumed inertial for simplicity')
+omega = [0;0;2*pi];
+velocity = [1;0;0];
+
+%% Set initial conditions for the true trajectory and for the estimates
+% (one estimate is obtained by integrating in the body frame, the other
+% by integrating in the navigation frame)
+% Initial state (body)
+currentPoseGlobal = Pose3;
+currentVelocityGlobal = velocity;
+% Initial state estimate (integrating in navigation frame)
+currentPoseGlobalIMUnav = currentPoseGlobal;
+currentVelocityGlobalIMUnav = currentVelocityGlobal;
+% Initial state estimate (integrating in the body frame)
+currentPoseGlobalIMUbody = currentPoseGlobal;
+currentVelocityGlobalIMUbody = currentVelocityGlobal;
+
+%% Prepare data structures for actual trajectory and estimates
+% Actual trajectory
+positions = zeros(3, length(times)+1);
+positions(:,1) = currentPoseGlobal.translation.vector;
+poses(1).p = positions(:,1);
+poses(1).R = currentPoseGlobal.rotation.matrix;
+
+% Trajectory estimate (integrated in the navigation frame)
+positionsIMUnav = zeros(3, length(times)+1);
+positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
+posesIMUnav(1).p = positionsIMUnav(:,1);
+posesIMUnav(1).R = poses(1).R;
+
+% Trajectory estimate (integrated in the body frame)
+positionsIMUbody = zeros(3, length(times)+1);
+positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
+posesIMUbody(1).p = positionsIMUbody(:,1);
+posesIMUbody(1).R = poses(1).R;
+
+%% Main loop
+i = 2;
+for t = times
+  %% Create the actual trajectory, using the velocities and
+  % accelerations in the inertial frame to compute the positions
+  [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
+    currentPoseGlobal, omega, velocity, velocity, deltaT);
+  
+  %% Simulate IMU measurements, considering Coriolis effect 
+  % (in this simple example we neglect gravity and there are no other forces acting on the body)
+  acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
+    omega, omega, velocity, velocity, deltaT);
+  
+  %% Integrate in the body frame
+  [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
+    currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
+
+  %% Integrate in the navigation frame
+  [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
+    currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
+  
+  %% Store data in some structure for statistics and plots
+  positions(:,i) = currentPoseGlobal.translation.vector;
+  positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
+  positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;  
+  % - 
+  poses(i).p = positions(:,i);
+  posesIMUbody(i).p = positionsIMUbody(:,i);
+  posesIMUnav(i).p = positionsIMUnav(:,i); 
+  % -
+  poses(i).R = currentPoseGlobal.rotation.matrix;
+  posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
+  posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
+  i = i + 1;
+end
+
+figure(1)
+hold on;
+plot(positions(1,:), positions(2,:), '-b');
+plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
+plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
+axis equal;
+legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')
+
+

tests/testPCGSolver.cpp

@@ -0,0 +1,157 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testPCGSolver.cpp
+ * @brief   Unit tests for PCGSolver class
+ * @author  Yong-Dian Jian
+ */
+
+#include <tests/smallExample.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Matrix.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+const double tol = 1e-3;
+
+using symbol_shorthand::X;
+using symbol_shorthand::L;
+
+/* ************************************************************************* */
+TEST( PCGSolver, llt ) {
+  Matrix R = (Matrix(3,3) <<
+                1., -1., -1.,
+                0.,  2., -1.,
+                0.,  0.,  1.);
+  Matrix AtA = R.transpose() * R;
+
+  Vector Rvector = (Vector(9) << 1., -1., -1.,
+                                 0.,  2., -1.,
+                                 0.,  0.,  1.);
+//  Vector Rvector = (Vector(6) << 1., -1., -1.,
+//                                      2., -1.,
+//                                           1.);
+
+  Vector b = (Vector(3) << 1., 2., 3.);
+
+  Vector x = (Vector(3) << 6.5, 2.5, 3.) ;
+
+  /* test cholesky */
+  Matrix Rhat = AtA.llt().matrixL().transpose();
+  EXPECT(assert_equal(R, Rhat, 1e-5));
+
+  /* test backward substitution */
+  Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
+  EXPECT(assert_equal(x, xhat, 1e-5));
+
+  /* test in-place back substitution */
+  xhat = b;
+  Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
+  EXPECT(assert_equal(x, xhat, 1e-5));
+
+  /* test triangular matrix map */
+  Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
+  xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
+  EXPECT(assert_equal(x, xhat, 1e-5));
+
+}
+
+/* ************************************************************************* */
+TEST( PCGSolver, dummy )
+{
+  LevenbergMarquardtParams paramsPCG;
+  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
+  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
+  pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
+  paramsPCG.iterativeParams = pcg;
+
+  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
+
+  Point2 x0(10,10);
+  Values c0;
+  c0.insert(X(1), x0);
+
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+
+  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+}
+
+/* ************************************************************************* */
+TEST( PCGSolver, blockjacobi )
+{
+  LevenbergMarquardtParams paramsPCG;
+  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
+  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
+  pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
+  paramsPCG.iterativeParams = pcg;
+
+  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
+
+  Point2 x0(10,10);
+  Values c0;
+  c0.insert(X(1), x0);
+
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+
+  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+}
+
+/* ************************************************************************* */
+TEST( PCGSolver, subgraph )
+{
+  LevenbergMarquardtParams paramsPCG;
+  paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
+  PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
+  pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
+  paramsPCG.iterativeParams = pcg;
+
+  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
+
+  Point2 x0(10,10);
+  Values c0;
+  c0.insert(X(1), x0);
+
+  Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
+
+  DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+