Merge remote-tracking branch 'origin/develop' into feature/LevenbergMarquardt

gtsam/base/LieMatrix.cpp

@@ -19,20 +19,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-LieMatrix::LieMatrix(size_t m, size_t n, ...)
-: Matrix(m,n) {
-  va_list ap;
-  va_start(ap, n);
-  for(size_t i = 0; i < m; ++i) {
-    for(size_t j = 0; j < n; ++j) {
-      double value = va_arg(ap, double);
-      (*this)(i,j) = value;
-    }
-  }
-  va_end(ap);
-}
-
 /* ************************************************************************* */
 void LieMatrix::print(const std::string& name) const {
   gtsam::print(matrix(), name);

gtsam/base/LieMatrix.h

@@ -48,9 +48,6 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
   LieMatrix(size_t m, size_t n, const double* const data) :
       Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
 
-  /** Specify arguments directly, as in Matrix_() - always force these to be doubles */
-  GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...);
-
   /// @}
   /// @name Testable interface
   /// @{

gtsam/base/LieVector.cpp

@@ -24,21 +24,10 @@ namespace gtsam {
 
 /* ************************************************************************* */
 LieVector::LieVector(size_t m, const double* const data)
-: Vector(Vector_(m,data))
-{
-}
-
-/* ************************************************************************* */
-LieVector::LieVector(size_t m, ...)
 : Vector(m)
 {
-  va_list ap;
+  for(size_t i = 0; i < m; i++)
-  va_start(ap, m);
+    (*this)(i) = data[i];
-  for( size_t i = 0 ; i < m ; i++) {
-    double value = va_arg(ap, double);
-    (*this)(i) = value;
-  }
-  va_end(ap);
 }
 
 /* ************************************************************************* */

gtsam/base/LieVector.h

@@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
   /** constructor with size and initial data, row order ! */
   GTSAM_EXPORT LieVector(size_t m, const double* const data);
 
-  /** Specify arguments directly, as in Vector_() - always force these to be doubles */
-  GTSAM_EXPORT LieVector(size_t m, ...);
-
   /** get the underlying vector */
   Vector vector() const {
     return static_cast<Vector>(*this);

gtsam/base/Matrix.cpp

@@ -36,38 +36,6 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Matrix Matrix_( size_t m, size_t n, const double* const data) {
-  MatrixRowMajor A(m,n);
-  copy(data, data+m*n, A.data());
-  return Matrix(A);
-}
-
-/* ************************************************************************* */
-Matrix Matrix_( size_t m, size_t n, const Vector& v)
-{
-  Matrix A(m,n);
-  // column-wise copy
-  for( size_t j = 0, k=0  ; j < n ; j++)
-    for( size_t i = 0; i < m ; i++,k++)
-      A(i,j) = v(k);
-  return A;
-}
-
-/* ************************************************************************* */
-Matrix Matrix_(size_t m, size_t n, ...) {
-  Matrix A(m,n);
-  va_list ap;
-  va_start(ap, n);
-  for( size_t i = 0 ; i < m ; i++)
-    for( size_t j = 0 ; j < n ; j++) {
-      double value = va_arg(ap, double);
-      A(i,j) = value;
-    }
-  va_end(ap);
-  return A;
-}
-
 /* ************************************************************************* */
 Matrix zeros( size_t m, size_t n ) {
   return Matrix::Zero(m,n);
@@ -211,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
   x += alpha * A.transpose() * e;
 }
 
-/* ************************************************************************* */
-Vector Vector_(const Matrix& A)
-{
-  size_t m = A.rows(), n = A.cols();
-  Vector v(m*n);
-  for( size_t j = 0, k=0  ; j < n ; j++)
-    for( size_t i = 0; i < m ; i++,k++)
-      v(k) = A(i,j);
-  return v;
-}
-
 /* ************************************************************************* */
 void print(const Matrix& A, const string &s, ostream& stream) {
   size_t m = A.rows(), n = A.cols();

gtsam/base/Matrix.h

@@ -45,27 +45,6 @@ typedef Eigen::Matrix<double,6,6> Matrix6;
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
-/**
- *  constructor with size and initial data, row order !
- */
-GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data);
-
-/**
- *  constructor with size and vector data, column order !!!
- */
-GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v);
-
-/**
- *  constructor from Vector yielding v.size()*1 vector
- */
-inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);}
-
-/**
- *  nice constructor, dangerous as number of arguments must be exactly right
- *  and you have to pass doubles !!! always use 0.0 never 0
-*/
-GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...);
-
 // Matlab-like syntax
 
 /**
@@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
   return result;
 }
 
-/**
- * convert to column vector, column order !!!
- */
-GTSAM_EXPORT Vector Vector_(const Matrix& A);
-
 /**
  * print a matrix
  */

gtsam/base/Vector.cpp

@@ -79,33 +79,6 @@ void odprintf(const char *format, ...) {
 //#endif
 }
 
-/* ************************************************************************* */
-Vector Vector_( size_t m, const double* const data) {
-  Vector A(m);
-  copy(data, data+m, A.data());
-  return A;
-}
-
-/* ************************************************************************* */
-Vector Vector_(size_t m, ...) {
-  Vector v(m);
-  va_list ap;
-  va_start(ap, m);
-  for( size_t i = 0 ; i < m ; i++) {
-    double value = va_arg(ap, double);
-    v(i) = value;
-  }
-  va_end(ap);
-  return v;
-}
-
-/* ************************************************************************* */
-Vector Vector_(const std::vector<double>& d) {
-  Vector v(d.size());
-  copy(d.begin(), d.end(), v.data());
-  return v;
-}
-
 /* ************************************************************************* */
 bool zero(const Vector& v) {
   bool result = true;

gtsam/base/Vector.h

@@ -46,22 +46,6 @@ typedef Eigen::VectorBlock<const Vector> ConstSubVector;
  */
 GTSAM_EXPORT void odprintf(const char *format, ...);
 
-/**
- *  constructor with size and initial data, row order !
- */
-GTSAM_EXPORT Vector Vector_( size_t m, const double* const data);
-
-/**
- *  nice constructor, dangerous as number of arguments must be exactly right
- *  and you have to pass doubles !!! always use 0.0 never 0
- */
-GTSAM_EXPORT Vector Vector_(size_t m, ...);
-
-/**
- * Create a numeric vector from an STL vector of doubles
- */
-GTSAM_EXPORT Vector Vector_(const std::vector<double>& data);
-
 /**
  * Create vector initialized to a constant value
  * @param n is the size of the vector

gtsam/base/numericalDerivative.h

@@ -59,7 +59,7 @@ namespace gtsam {
 
   /** global functions for converting to a LieVector for use with numericalDerivative */
   inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
-  inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
+  inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); }
 
   /**
    * Numerically compute gradient of scalar function

gtsam/base/tests/testLieMatrix.cpp

@@ -39,20 +39,17 @@ TEST( LieMatrix, construction ) {
 TEST( LieMatrix, other_constructors ) {
   Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
   LieMatrix exp(init);
-  LieMatrix a(2,2,10.0,20.0,30.0,40.0);
   double data[] = {10,30,20,40};
   LieMatrix b(2,2,data);
-  EXPECT(assert_equal(exp, a));
   EXPECT(assert_equal(exp, b));
-  EXPECT(assert_equal(b, a));
 }
 
 /* ************************************************************************* */
 TEST(LieMatrix, retract) {
-  LieMatrix init(2,2, 1.0,2.0,3.0,4.0);
+  LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0));
-  Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0);
+  Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0);
 
-  LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0);
+  LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0));
   LieMatrix actual = init.retract(update);
 
   EXPECT(assert_equal(expected, actual));
@@ -63,7 +60,7 @@ TEST(LieMatrix, retract) {
   EXPECT(assert_equal(expectedUpdate, actualUpdate));
 
   Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4);
-  Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
+  Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0)));
   EXPECT(assert_equal(expectedLogmap, actualLogmap));
 }
 

gtsam/base/tests/testLieScalar.cpp

@@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) {
 TEST( testLieScalar, localCoordinates ) {
   LieScalar lie1(1.), lie2(3.);
 
-  EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2)));
+  EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2)));
 }
 
 /* ************************************************************************* */

gtsam/base/tests/testLieVector.cpp

@@ -39,14 +39,9 @@ TEST( testLieVector, construction ) {
 TEST( testLieVector, other_constructors ) {
   Vector init = (Vector(2) << 10.0, 20.0);
   LieVector exp(init);
-  LieVector a(2,10.0,20.0);
   double data[] = {10,20};
   LieVector b(2,data);
-  LieVector c(2.3), c_exp(LieVector(1, 2.3));
-  EXPECT(assert_equal(exp, a));
   EXPECT(assert_equal(exp, b));
-  EXPECT(assert_equal(b, a));
-  EXPECT(assert_equal(c_exp, c));
 }
 
 /* ************************************************************************* */

gtsam/base/tests/testMatrix.cpp

@@ -43,18 +43,6 @@ TEST( matrix, constructor_data )
   EQUALITY(A,B);
 }
 
-/* ************************************************************************* */
-/*
-TEST( matrix, constructor_vector )
-{
-  double data[] = { -5, 3, 0, -5 };
-  Matrix A = Matrix_(2, 2, -5, 3, 0, -5);
-  Vector v(4);
-  copy(data, data + 4, v.data());
-  Matrix B = Matrix_(2, 2, v); // this one is column order !
-  EQUALITY(A,trans(B));
-}
-*/
 /* ************************************************************************* */
 TEST( matrix, Matrix_ )
 {

gtsam/base/tests/testNumericalDerivative.cpp

@@ -48,7 +48,8 @@ double f2(const LieVector& x) {
 
 /* ************************************************************************* */
 TEST(testNumericalDerivative, numericalHessian2) {
-  LieVector center(2, 0.5, 1.0);
+  Vector v_center = (Vector(2) << 0.5, 1.0);
+  LieVector center(v_center);
 
   Matrix expected = (Matrix(2,2) <<
       -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
@@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) {
 
 /* ************************************************************************* */
 TEST(testNumericalDerivative, numericalHessian211) {
-  LieVector center1(1, 1.0), center2(1, 5.0);
+  Vector v_center1 = (Vector(1) << 1.0);
+  Vector v_center2 = (Vector(1) << 5.0);
+  LieVector center1(v_center1), center2(v_center2);
 
   Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
   Matrix actual11 = numericalHessian211(f3, center1, center2);
@@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
 
 /* ************************************************************************* */
 TEST(testNumericalDerivative, numericalHessian311) {
-  LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
+  Vector v_center1 = (Vector(1) << 1.0);
+  Vector v_center2 = (Vector(1) << 2.0);
+  Vector v_center3 = (Vector(1) << 3.0);
+  LieVector center1(v_center1), center2(v_center2), center3(v_center3);
+
   double x = center1(0), y = center2(0), z = center3(0);
   Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
   Matrix actual11 = numericalHessian311(f4, center1, center2, center3);

gtsam/base/tests/testVector.cpp

@@ -23,15 +23,6 @@
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
-TEST( TestVector, Vector_variants )
-{
-  Vector a = (Vector(2) << 10.0,20.0);
-  double data[] = {10,20};
-  Vector b = Vector_(2, data);
-  EXPECT(assert_equal(a, b));
-}
-
 namespace {
   /* ************************************************************************* */
   template<typename Derived>

gtsam/geometry/EssentialMatrix.cpp

@@ -0,0 +1,152 @@
+/*
+ * @file EssentialMatrix.cpp
+ * @brief EssentialMatrix class
+ * @author Frank Dellaert
+ * @date December 5, 2014
+ */
+
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <iostream>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+    boost::optional<Matrix&> H) {
+  const Rot3& _1R2_ = _1P2_.rotation();
+  const Point3& _1T2_ = _1P2_.translation();
+  if (!H) {
+    // just make a direction out of translation and create E
+    Sphere2 direction(_1T2_);
+    return EssentialMatrix(_1R2_, direction);
+  } else {
+    // Calculate the 5*6 Jacobian H = D_E_1P2
+    // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
+    // First get 2*3 derivative from Sphere2::FromPoint3
+    Matrix D_direction_1T2;
+    Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2);
+    H->resize(5, 6);
+    H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
+    H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
+    H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
+    H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
+    return EssentialMatrix(_1R2_, direction);
+  }
+}
+
+/* ************************************************************************* */
+void EssentialMatrix::print(const string& s) const {
+  cout << s;
+  aRb_.print("R:\n");
+  aTb_.print("d: ");
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
+  assert(xi.size() == 5);
+  Vector3 omega(sub(xi, 0, 3));
+  Vector2 z(sub(xi, 3, 5));
+  Rot3 R = aRb_.retract(omega);
+  Sphere2 t = aTb_.retract(z);
+  return EssentialMatrix(R, t);
+}
+
+/* ************************************************************************* */
+Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
+  return Vector(5) << //
+      aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
+}
+
+/* ************************************************************************* */
+Point3 EssentialMatrix::transform_to(const Point3& p,
+    boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
+  Pose3 pose(aRb_, aTb_.point3());
+  Point3 q = pose.transform_to(p, DE, Dpoint);
+  if (DE) {
+    // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
+    // The last 3 columns are derivative with respect to change in translation
+    // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
+    // Duy made an educated guess that this needs to be rotated to the local frame
+    Matrix H(3, 5);
+    H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
+    *DE = H;
+  }
+  return q;
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
+    boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
+
+  // The rotation must be conjugated to act in the camera frame
+  Rot3 c1Rc2 = aRb_.conjugate(cRb);
+
+  if (!HE && !HR) {
+    // Rotate translation direction and return
+    Sphere2 c1Tc2 = cRb * aTb_;
+    return EssentialMatrix(c1Rc2, c1Tc2);
+  } else {
+    // Calculate derivatives
+    Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+    Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+    if (HE) {
+      *HE = zeros(5, 5);
+      HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
+      HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
+    }
+    if (HR) {
+      throw runtime_error(
+          "EssentialMatrix::rotate: derivative HR not implemented yet");
+      /*
+       HR->resize(5, 3);
+       HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
+       HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
+       */
+    }
+    return EssentialMatrix(c1Rc2, c1Tc2);
+  }
+}
+
+/* ************************************************************************* */
+double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
+    boost::optional<Matrix&> H) const {
+  if (H) {
+    H->resize(1, 5);
+    // See math.lyx
+    Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+        * aTb_.basis();
+    *H << HR, HD;
+  }
+  return dot(vA, E_ * vB);
+}
+
+/* ************************************************************************* */
+ostream& operator <<(ostream& os, const EssentialMatrix& E) {
+  Rot3 R = E.rotation();
+  Sphere2 d = E.direction();
+  os.precision(10);
+  os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
+  return os;
+}
+
+/* ************************************************************************* */
+istream& operator >>(istream& is, EssentialMatrix& E) {
+  double rx, ry, rz, dx, dy, dz;
+  is >> rx >> ry >> rz; // Read the rotation rxyz
+  is >> dx >> dy >> dz; // Read the translation dxyz
+
+  // Create EssentialMatrix from rotation and translation
+  Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
+  Sphere2 dt = Sphere2(dx, dy, dz);
+  E = EssentialMatrix(rot, dt);
+
+  return is;
+}
+
+/* ************************************************************************* */
+
+} // gtsam
+

gtsam/geometry/EssentialMatrix.h

@@ -26,7 +26,7 @@ private:
 
   Rot3 aRb_; ///< Rotation between a and b
   Sphere2 aTb_; ///< translation direction from a to b
-  Matrix E_; ///< Essential matrix
+  Matrix3 E_; ///< Essential matrix
 
 public:
 
@@ -48,6 +48,10 @@ public:
       aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
   }
 
+  /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
+  static EssentialMatrix FromPose3(const Pose3& _1P2_,
+      boost::optional<Matrix&> H = boost::none);
+
   /// Random, using Rot3::Random and Sphere2::Random
   template<typename Engine>
   static EssentialMatrix Random(Engine & rng) {
@@ -60,11 +64,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const {
+  void print(const std::string& s = "") const;
-    std::cout << s;
-    aRb_.print("R:\n");
-    aTb_.print("d: ");
-  }
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@@ -87,20 +87,10 @@ public:
   }
 
   /// Retract delta to manifold
-  virtual EssentialMatrix retract(const Vector& xi) const {
+  virtual EssentialMatrix retract(const Vector& xi) const;
-    assert(xi.size() == 5);
-    Vector3 omega(sub(xi, 0, 3));
-    Vector2 z(sub(xi, 3, 5));
-    Rot3 R = aRb_.retract(omega);
-    Sphere2 t = aTb_.retract(z);
-    return EssentialMatrix(R, t);
-  }
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& other) const {
+  virtual Vector localCoordinates(const EssentialMatrix& other) const;
-    return Vector(5) << //
-        aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
-  }
 
   /// @}
 
@@ -108,17 +98,17 @@ public:
   /// @{
 
   /// Rotation
-  const Rot3& rotation() const {
+  inline const Rot3& rotation() const {
     return aRb_;
   }
 
   /// Direction
-  const Sphere2& direction() const {
+  inline const Sphere2& direction() const {
     return aTb_;
   }
 
   /// Return 3*3 matrix representation
-  const Matrix& matrix() const {
+  inline const Matrix3& matrix() const {
     return E_;
   }
 
@@ -131,20 +121,7 @@ public:
    */
   Point3 transform_to(const Point3& p,
       boost::optional<Matrix&> DE = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const {
+      boost::optional<Matrix&> Dpoint = boost::none) const;
-    Pose3 pose(aRb_, aTb_.point3());
-    Point3 q = pose.transform_to(p, DE, Dpoint);
-    if (DE) {
-      // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
-      // The last 3 columns are derivative with respect to change in translation
-      // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
-      // Duy made an educated guess that this needs to be rotated to the local frame
-      Matrix H(3, 5);
-      H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
-      *DE = H;
-    }
-    return q;
-  }
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -152,36 +129,7 @@ public:
    * @param E essential matrix E in camera frame C
    */
   EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
-      boost::none, boost::optional<Matrix&> HR = boost::none) const {
+      boost::none, boost::optional<Matrix&> HR = boost::none) const;
-
-    // The rotation must be conjugated to act in the camera frame
-    Rot3 c1Rc2 = aRb_.conjugate(cRb);
-
-    if (!HE && !HR) {
-      // Rotate translation direction and return
-      Sphere2 c1Tc2 = cRb * aTb_;
-      return EssentialMatrix(c1Rc2, c1Tc2);
-    } else {
-      // Calculate derivatives
-      Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
-      Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
-      if (HE) {
-        *HE = zeros(5, 5);
-        HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
-        HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
-      }
-      if (HR) {
-        throw std::runtime_error(
-            "EssentialMatrix::rotate: derivative HR not implemented yet");
-        /*
-         HR->resize(5, 3);
-         HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
-         HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
-         */
-      }
-      return EssentialMatrix(c1Rc2, c1Tc2);
-    }
-  }
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -194,17 +142,18 @@ public:
 
   /// epipolar error, algebraic
   double error(const Vector& vA, const Vector& vB, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const;
-    if (H) {
+
-      H->resize(1, 5);
+  /// @}
-      // See math.lyx
+
-      Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+  /// @name Streaming operators
-      Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+  /// @{
-          * aTb_.basis();
+
-      *H << HR, HD;
+  /// stream to stream
-    }
+  friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
-    return dot(vA, E_ * vB);
+
-  }
+  /// stream from stream
+  friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
 
   /// @}
 

gtsam/geometry/Point2.h

@@ -172,7 +172,7 @@ public:
   static inline Point2 Expmap(const Vector& v) { return Point2(v); }
 
   /// Log map around identity - just return the Point2 as a vector
-  static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
+  static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }
 
   /// @}
   /// @name Vector Space

gtsam/geometry/Point3.cpp

@@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
 
 /* ************************************************************************* */
 bool Point3::equals(const Point3 & q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
+  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
+      && fabs(z_ - q.z()) < tol);
 }
 
 /* ************************************************************************* */
@@ -37,18 +38,18 @@ void Point3::print(const string& s) const {
 
 /* ************************************************************************* */
 
-bool Point3::operator== (const Point3& q) const {
+bool Point3::operator==(const Point3& q) const {
   return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
 }
 
 /* ************************************************************************* */
 Point3 Point3::operator+(const Point3& q) const {
-  return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
+  return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
 }
 
 /* ************************************************************************* */
-Point3 Point3::operator- (const Point3& q) const {
+Point3 Point3::operator-(const Point3& q) const {
-  return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
+  return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
 }
 
 /* ************************************************************************* */
@@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
 }
 
 /* ************************************************************************* */
-Point3 Point3::add(const Point3 &q,
+Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
+  if (H1)
-  if (H2) *H2 = eye(3,3);
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = eye(3, 3);
   return *this + q;
 }
 
 /* ************************************************************************* */
-Point3 Point3::sub(const Point3 &q,
+Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = eye(3,3);
+  if (H1)
-  if (H2) *H2 = -eye(3,3);
+    *H1 = eye(3, 3);
+  if (H2)
+    *H2 = -eye(3, 3);
   return *this - q;
 }
 
 /* ************************************************************************* */
 Point3 Point3::cross(const Point3 &q) const {
-  return Point3( y_*q.z_ - z_*q.y_,
+  return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
-      z_*q.x_ - x_*q.z_,
+      x_ * q.y_ - y_ * q.x_);
-      x_*q.y_ - y_*q.x_ );
 }
 
 /* ************************************************************************* */
 double Point3::dot(const Point3 &q) const {
-  return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
+  return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
 }
 
 /* ************************************************************************* */
 double Point3::norm() const {
-  return sqrt( x_*x_ + y_*y_ + z_*z_ );
+  return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
+}
+
+/* ************************************************************************* */
+Point3 Point3::normalize(boost::optional<Matrix&> H) const {
+  Point3 normalized = *this / norm();
+  if (H) {
+    // 3*3 Derivative
+    double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
+    double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
+    H->resize(3, 3);
+    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
+    *H /= pow(x2 + y2 + z2, 1.5);
+  }
+  return normalized;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point3.h

@@ -176,6 +176,9 @@ namespace gtsam {
     /** Distance of the point from the origin */
     double norm() const;
 
+    /** normalize, with optional Jacobian */
+    Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
+
     /** cross product @return this x q */
     Point3 cross(const Point3 &q) const;
 

gtsam/geometry/Pose3.cpp

@@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
 /* ************************************************************************* */
 Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
   // Bernoulli numbers, from Wikipedia
-  static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
+  static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
       0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
   static const int N = 5; // order of approximation
   Matrix res = I6;

gtsam/geometry/Rot2.h

@@ -170,7 +170,7 @@ namespace gtsam {
 
     ///Log map at identity - return the canonical coordinates of this rotation
     static inline Vector Logmap(const Rot2& r) {
-      return Vector_(1, r.theta());
+      return (Vector(1) << r.theta());
     }
 
     /// @}

gtsam/geometry/Rot3.h

@@ -194,7 +194,7 @@ namespace gtsam {
      * @return incremental rotation matrix
      */
     static Rot3 rodriguez(double wx, double wy, double wz)
-      { return rodriguez(Vector_(3,wx,wy,wz));}
+      { return rodriguez((Vector(3) << wx, wy, wz));}
 
     /// @}
     /// @name Testable

gtsam/geometry/Sphere2.cpp

@@ -28,6 +28,21 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Sphere2 Sphere2::FromPoint3(const Point3& point,
+    boost::optional<Matrix&> H) {
+  Sphere2 direction(point);
+  if (H) {
+    // 3*3 Derivative of representation with respect to point is 3*3:
+    Matrix D_p_point;
+    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
+    // Calculate the 2*3 Jacobian
+    H->resize(2, 3);
+    *H << direction.basis().transpose() * D_p_point;
+  }
+  return direction;
+}
+
 /* ************************************************************************* */
 Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
   // TODO allow any engine without including all of boost :-(
@@ -170,7 +185,7 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
     double alpha = p.transpose() * q;
     assert(alpha != 0.0);
     Matrix coeffs = (B.transpose() * q) / alpha;
-    Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
+    Vector result = (Vector(2) << coeffs(0, 0), coeffs(1, 0));
     return result;
   } else {
     assert (false);

gtsam/geometry/Sphere2.h

@@ -70,6 +70,10 @@ public:
     p_ = p_ / p_.norm();
   }
 
+  /// Named constructor from Point3 with optional Jacobian
+  static Sphere2 FromPoint3(const Point3& point,
+      boost::optional<Matrix&> H = boost::none);
+
   /// Random direction, using boost::uniform_on_sphere
   static Sphere2 Random(boost::random::mt19937 & rng);
 
@@ -90,7 +94,11 @@ public:
   /// @name Other functionality
   /// @{
 
-  /// Returns the local coordinate frame to tangent plane
+  /**
+   * Returns the local coordinate frame to tangent plane
+   * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
+   * tangent to the sphere at the current direction.
+   */
   Matrix basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere

gtsam/geometry/tests/testCal3DS2.cpp

@@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate)
   double g = 1+k[0]*r+k[1]*r*r ;
   double tx = 2*k[2]*p.x()*p.y()       +   k[3]*(r+2*p.x()*p.x()) ;
   double ty =   k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
-  Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ;
+  Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ;
   Vector v_i = K.K() * v_hat ;
   Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
   Point2 q = K.uncalibrate(p);

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -10,6 +10,7 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Testable.h>
 #include <CppUnitLite/TestHarness.h>
+#include <sstream>
 
 using namespace std;
 using namespace gtsam;
@@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) {
   // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_a) {
+  Matrix actualH;
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_b) {
+  Matrix actualH;
+  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Point3 c1Tc2(0.4, 0.5, 0.6);
+  EssentialMatrix trueE(c1Rc2, c1Tc2);
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*************************************************************************
+TEST (EssentialMatrix, streaming) {
+  EssentialMatrix expected(c1Rc2, c1Tc2), actual;
+  stringstream ss;
+  ss << expected;
+  ss >> actual;
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPoint3.cpp

@@ -14,73 +14,84 @@
  * @brief  Unit tests for Point3 class
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Point3)
 GTSAM_CONCEPT_LIE_INST(Point3)
 
-static Point3 P(0.2,0.7,-2);
+static Point3 P(0.2, 0.7, -2);
 
 /* ************************************************************************* */
 TEST(Point3, Lie) {
-  Point3 p1(1,2,3);
+  Point3 p1(1, 2, 3);
-  Point3 p2(4,5,6);
+  Point3 p2(4, 5, 6);
   Matrix H1, H2;
 
-  EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
+  EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
   EXPECT(assert_equal(eye(3), H1));
   EXPECT(assert_equal(eye(3), H2));
 
-  EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
+  EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
   EXPECT(assert_equal(-eye(3), H1));
   EXPECT(assert_equal(eye(3), H2));
 
-  EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
+  EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
-  EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
+  EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
 }
 
 /* ************************************************************************* */
-TEST( Point3, arithmetic)
+TEST( Point3, arithmetic) {
-{
+  CHECK(P * 3 == 3 * P);
-  CHECK(P*3==3*P);
+  CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
-  CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) ));
+  CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
-  CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1)));
+  CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
-  CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1)));
+  CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
-  CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2));
+  CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
-  CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3)));
+  CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
-  CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
 }
 
 /* ************************************************************************* */
-TEST( Point3, equals)
+TEST( Point3, equals) {
-{
   CHECK(P.equals(P));
   Point3 Q;
   CHECK(!P.equals(Q));
 }
 
 /* ************************************************************************* */
-TEST( Point3, dot)
+TEST( Point3, dot) {
-{
+  Point3 origin, ones(1, 1, 1);
-  Point3 origin, ones(1,1,1);
+  CHECK(origin.dot(Point3(1, 1, 0)) == 0);
-  CHECK(origin.dot(Point3(1,1,0)) == 0);
+  CHECK(ones.dot(Point3(1, 1, 0)) == 2);
-  CHECK(ones.dot(Point3(1,1,0)) == 2);
 }
 
 /* ************************************************************************* */
-TEST( Point3, stream)
+TEST( Point3, stream) {
-{
+  Point3 p(1, 2, -3);
-  Point3 p(1,2, -3);
   std::ostringstream os;
   os << p;
   EXPECT(os.str() == "[1, 2, -3]';");
 }
 
+//*************************************************************************
+TEST (Point3, normalize) {
+  Matrix actualH;
+  Point3 point(1, -2, 3); // arbitrary point
+  Point3 expected(point / sqrt(14));
+  EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<Point3, Point3>(
+      boost::bind(&Point3::normalize, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 

gtsam/geometry/tests/testRot2.cpp

@@ -96,7 +96,7 @@ TEST(Rot2, logmap)
 {
   Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
   Rot2 rot(Rot2::fromAngle(M_PI));
-  Vector expected = Vector_(1, M_PI/2.0);
+  Vector expected = (Vector(1) << M_PI/2.0);
   Vector actual = rot0.localCoordinates(rot);
   CHECK(assert_equal(expected, actual));
 }

gtsam/geometry/tests/testSphere2.cpp

@@ -27,6 +27,7 @@
 #include <boost/foreach.hpp>
 #include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
+#include <cmath>
 
 using namespace boost::assign;
 using namespace gtsam;
@@ -212,9 +213,9 @@ inline static Vector randomVector(const Vector& minLimits,
 TEST(Sphere2, localCoordinates_retract) {
 
   size_t numIterations = 10000;
-  Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
+  Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
-      Vector_(3, 1.0, 1.0, 1.0);
+      (Vector(3) << 1.0, 1.0, 1.0);
-  Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0);
+  Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0);
   for (size_t i = 0; i < numIterations; i++) {
 
     // Sleep for the random number generator (TODO?: Better create all of them first).
@@ -242,9 +243,9 @@ TEST(Sphere2, localCoordinates_retract) {
 TEST(Sphere2, localCoordinates_retract_expmap) {
   
   size_t numIterations = 10000;
-  Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
+  Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
-      Vector_(3, 1.0, 1.0, 1.0);
+      (Vector(3) << 1.0, 1.0, 1.0);
-  Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
+  Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI);
   for (size_t i = 0; i < numIterations; i++) {
 
     // Sleep for the random number generator (TODO?: Better create all of them first).
@@ -287,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
 //  EXPECT(assert_equal(expected,actual1));
 //  EXPECT(assert_equal(expected,actual2));
 //
-//  Matrix expectedH1 = Matrix_(3,3,
+//  Matrix expectedH1 = (Matrix(3,3) <<
 //      0.0,-1.0,-2.0,
 //      1.0, 0.0,-2.0,
 //      0.0, 0.0,-1.0
@@ -298,7 +299,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
 //  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
 //  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
 //
-//  Matrix expectedH2 = Matrix_(3,3,
+//  Matrix expectedH2 = (Matrix(3,3) <<
 //       1.0, 0.0, 0.0,
 //       0.0, 1.0, 0.0,
 //       0.0, 0.0, 1.0
@@ -324,6 +325,17 @@ TEST(Sphere2, Random) {
   EXPECT(assert_equal(expectedMean,actualMean,0.1));
 }
 
+//*************************************************************************
+TEST (Sphere2, FromPoint3) {
+  Matrix actualH;
+  Point3 point(1, -2, 3); // arbitrary point
+  Sphere2 expected(point);
+  EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<Sphere2, Point3>(
+      boost::bind(Sphere2::FromPoint3, _1, boost::none), point);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/geometry/tests/timePose2.cpp

@@ -138,7 +138,7 @@ int main()
 
   // create a random pose:
   double x = 4.0, y = 2.0, r = 0.3;
-  Vector v = Vector_(3,x,y,r);
+  Vector v = (Vector(3) << x, y, r);
   Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
 
   TEST(Expmap, Pose2::Expmap(v));

gtsam/geometry/tests/timeRot2.cpp

@@ -95,7 +95,7 @@ int main()
   // create a random direction:
   double norm=sqrt(16.0+4.0);
   double x=4.0/norm, y=2.0/norm;
-  Vector v = Vector_(2,x,y);
+  Vector v = (Vector(2) << x, y);
   Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6);
 
   TEST(Rot2_Expmap, Rot2::Expmap(v));

gtsam/geometry/tests/timeRot3.cpp

@@ -39,7 +39,7 @@ int main()
   // create a random direction:
   double norm=sqrt(1.0+16.0+4.0);
   double x=1.0/norm, y=4.0/norm, z=2.0/norm;
-  Vector v = Vector_(3,x,y,z);
+  Vector v = (Vector(3) << x, y, z);
   Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
 
   TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))

gtsam/linear/VectorValues.h

@@ -49,15 +49,15 @@ namespace gtsam {
    * Example:
    * \code
      VectorValues values;
-     values.insert(3, Vector_(3, 1.0, 2.0, 3.0));
+     values.insert(3, (Vector(3) << 1.0, 2.0, 3.0));
-     values.insert(4, Vector_(2, 4.0, 5.0));
+     values.insert(4, (Vector(2) << 4.0, 5.0));
-     values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0));
+     values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0));
 
      // Prints [ 3.0 4.0 ]
      gtsam::print(values[1]);
 
      // Prints [ 8.0 9.0 ]
-     values[1] = Vector_(2, 8.0, 9.0);
+     values[1] = (Vector(2) << 8.0, 9.0);
      gtsam::print(values[1]);
      \endcode
    *

gtsam/linear/tests/testSampler.cpp

@@ -24,7 +24,7 @@ const double tol = 1e-5;
 
 /* ************************************************************************* */
 TEST(testSampler, basic) {
-  Vector sigmas = Vector_(3, 1.0, 0.1, 0.0);
+  Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0);
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
   char seed = 'A';
   Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);

gtsam/navigation/CombinedImuFactor.h

@@ -236,7 +236,7 @@ namespace gtsam {
         // overall Jacobian wrt preintegrated measurements (df/dx)
         Matrix F(15,15);
         F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3,
-                H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,                         Z_3x3,
+                H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,              Z_3x3,
                 H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega,
                 Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3,
                 Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3;
@@ -274,6 +274,7 @@ namespace gtsam {
 
         // Update preintegrated measurements
         /* ----------------------------------------------------------------------------------------------------------------------- */
+        // deltaPij += deltaVij * deltaT;
         deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
         deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
         deltaRij = deltaRij * Rincr;
@@ -341,8 +342,11 @@ namespace gtsam {
   public:
 
     /** Shorthand for a smart pointer to a factor */
-    typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#ifndef _MSC_VER
-
+    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)) {}
 

gtsam/navigation/ImuBias.h

@@ -113,7 +113,7 @@ namespace imuBias {
 //
 //      return measurement - biasGyro_ - w_earth_rate_I;
 //
-////      Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
+////      Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
 ////      return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
 //    }
 

gtsam/navigation/ImuFactor.h

@@ -304,7 +304,11 @@ namespace gtsam {
   public:
 
     /** Shorthand for a smart pointer to a factor */
+#ifndef _MSC_VER
+    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()) {}

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
-  LieVector v1(3, 0.5, 0.0, 0.0);
+  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
-  LieVector v2(3, 0.5, 0.0, 0.0);
+  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 
   // Measurements
   Vector3 gravity; gravity << 0, 0, 9.81;

gtsam/navigation/tests/testImuFactor.cpp

@@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
   // Linearization point
   imuBias::ConstantBias bias; // Bias
   Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
-  LieVector v1(3, 0.5, 0.0, 0.0);
+  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
   Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
-  LieVector v2(3, 0.5, 0.0, 0.0);
+  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 
   // Measurements
   Vector3 gravity; gravity << 0, 0, 9.81;
@@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases )
   // Linearization point
 //  Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
 //  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
-//  LieVector v1(3, 0.5, 0.0, 0.0);
+//  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
 //  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
-//  LieVector v2(3, 0.5, 0.0, 0.0);
+//  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 
 
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
-  LieVector v1(3, 0.5, 0.0, 0.0);
+  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
-  LieVector v2(3, 0.5, 0.0, 0.0);
+  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 
   // Measurements
   Vector3 gravity; gravity << 0, 0, 9.81;
@@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
 //{
 //  // Linearization point
 //  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
-//  LieVector v1(3, 0.5, 0.0, 0.0);
+//  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
 //  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
-//  LieVector v2(3, 0.5, 0.0, 0.0);
+//  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 //  imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
 //
 //  // Pre-integrator
@@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
 
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
-  LieVector v1(3, 0.5, 0.0, 0.0);
+  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
-  LieVector v2(3, 0.5, 0.0, 0.0);
+  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
 
   // Measurements
   Vector3 gravity; gravity << 0, 0, 9.81;

gtsam/nonlinear/ISAM2.h

@@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
    * entries would be added with:
    * \code
      FastMap<char,Vector> thresholds;
-     thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
+     thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
-     thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0);                // 1.0 m landmark position threshold
+     thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0);                // 1.0 m landmark position threshold
      params.relinearizeThreshold = thresholds;
      \endcode
    */

gtsam/nonlinear/tests/testLinearContainerFactor.cpp

@@ -19,7 +19,7 @@ using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
 
-const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
+const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0));
 const double tol = 1e-5;
 
 gtsam::Key  l1 = 101, l2 = 102, x1 = 1, x2 = 2;

gtsam/nonlinear/tests/testValues.cpp

@@ -65,7 +65,8 @@ public:
 TEST( Values, equals1 )
 {
   Values expected;
-  LieVector v(3, 5.0, 6.0, 7.0);
+  LieVector v((Vector(3) << 5.0, 6.0, 7.0));
+
   expected.insert(key1,v);
   Values actual;
   actual.insert(key1,v);
@@ -76,8 +77,9 @@ TEST( Values, equals1 )
 TEST( Values, equals2 )
 {
   Values cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2(3, 5.0, 6.0, 8.0);
+  LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
+
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -88,8 +90,9 @@ TEST( Values, equals2 )
 TEST( Values, equals_nan )
 {
   Values cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2(3, inf, inf, inf);
+  LieVector v2((Vector(3) << inf, inf, inf));
+
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -100,10 +103,11 @@ TEST( Values, equals_nan )
 TEST( Values, insert_good )
 {
   Values cfg1, cfg2, expected;
-  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
-  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
-  LieVector v4(3, 8.0, 3.0, 7.0);
+  LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
+
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key3, v4);
@@ -121,10 +125,11 @@ TEST( Values, insert_good )
 TEST( Values, insert_bad )
 {
   Values cfg1, cfg2;
-  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
-  LieVector v3(3, 2.0, 4.0, 3.0);
+  LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
-  LieVector v4(3, 8.0, 3.0, 7.0);
+  LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
+
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key2, v3);
@@ -137,8 +142,8 @@ TEST( Values, insert_bad )
 TEST( Values, update_element )
 {
   Values cfg;
-  LieVector v1(3, 5.0, 6.0, 7.0);
+  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2(3, 8.0, 9.0, 1.0);
+  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
@@ -181,8 +186,8 @@ TEST(Values, basic_functions)
 //TEST(Values, dim_zero)
 //{
 //  Values config0;
-//  config0.insert(key1, LieVector(2, 2.0, 3.0));
+//  config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
-//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+//  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
 //  LONGS_EQUAL(5, config0.dim());
 //
 //  VectorValues expected;
@@ -195,16 +200,16 @@ TEST(Values, basic_functions)
 TEST(Values, expmap_a)
 {
   Values config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
 
   VectorValues increment = pair_list_of<Key, Vector>
     (key1, (Vector(3) << 1.0, 1.1, 1.2))
     (key2, (Vector(3) << 1.3, 1.4, 1.5));
 
   Values expected;
-  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+  expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
-  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
 
   CHECK(assert_equal(expected, config0.retract(increment)));
 }
@@ -213,15 +218,15 @@ TEST(Values, expmap_a)
 TEST(Values, expmap_b)
 {
   Values config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
 
   VectorValues increment = pair_list_of<Key, Vector>
     (key2, (Vector(3) << 1.3, 1.4, 1.5));
 
   Values expected;
-  expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
 
   CHECK(assert_equal(expected, config0.retract(increment)));
 }
@@ -230,16 +235,16 @@ TEST(Values, expmap_b)
 //TEST(Values, expmap_c)
 //{
 //  Values config0;
-//  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+//  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+//  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
 //
 //  Vector increment = LieVector(6,
 //      1.0, 1.1, 1.2,
 //      1.3, 1.4, 1.5);
 //
 //  Values expected;
-//  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
+//  expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
-//  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+//  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
 //
 //  CHECK(assert_equal(expected, config0.retract(increment)));
 //}
@@ -248,8 +253,8 @@ TEST(Values, expmap_b)
 TEST(Values, expmap_d)
 {
   Values config0;
-  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
@@ -266,8 +271,8 @@ TEST(Values, expmap_d)
 TEST(Values, localCoordinates)
 {
   Values valuesA;
-  valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
+  valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
 
   VectorValues expDelta = pair_list_of<Key, Vector>
     (key1, (Vector(3) << 0.1, 0.2, 0.3))
@@ -314,17 +319,17 @@ TEST(Values, exists_)
 TEST(Values, update)
 {
   Values config0;
-  config0.insert(key1, LieVector(1, 1.));
+  config0.insert(key1, LieVector((Vector(1) << 1.)));
-  config0.insert(key2, LieVector(1, 2.));
+  config0.insert(key2, LieVector((Vector(1) << 2.)));
 
   Values superset;
-  superset.insert(key1, LieVector(1, -1.));
+  superset.insert(key1, LieVector((Vector(1) << -1.)));
-  superset.insert(key2, LieVector(1, -2.));
+  superset.insert(key2, LieVector((Vector(1) << -2.)));
   config0.update(superset);
 
   Values expected;
-  expected.insert(key1, LieVector(1, -1.));
+  expected.insert(key1, LieVector((Vector(1) << -1.)));
-  expected.insert(key2, LieVector(1, -2.));
+  expected.insert(key2, LieVector((Vector(1) << -2.)));
   CHECK(assert_equal(expected,config0));
 }
 

gtsam/slam/BearingRangeFactor.h

@@ -96,7 +96,7 @@ namespace gtsam {
       Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
 
       double y2 = pose.range(point, H21_, H22_);
-      Vector e2 = Vector_(1, y2 - measuredRange_);
+      Vector e2 = (Vector(1) << y2 - measuredRange_);
 
       if (H1) *H1 = gtsam::stack(2, &H11, &H21);
       if (H2) *H2 = gtsam::stack(2, &H12, &H22);

gtsam/slam/EssentialMatrixConstraint.cpp

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2014, 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   EssentialMatrixConstraint.cpp
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date   Jan 5, 2014
+ **/
+
+#include <gtsam/slam/EssentialMatrixConstraint.h>
+//#include <gtsam/linear/GaussianFactor.h>
+//#include <gtsam/base/Testable.h>
+
+#include <ostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void EssentialMatrixConstraint::print(const std::string& s,
+    const KeyFormatter& keyFormatter) const {
+  std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
+      << "," << keyFormatter(this->key2()) << ")\n";
+  measuredE_.print("  measured: ");
+  this->noiseModel_->print("  noise model: ");
+}
+
+/* ************************************************************************* */
+bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
+    double tol) const {
+  const This *e = dynamic_cast<const This*>(&expected);
+  return e != NULL && Base::equals(*e, tol)
+      && this->measuredE_.equals(e->measuredE_, tol);
+}
+
+/* ************************************************************************* */
+Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
+    const Pose3& p2, boost::optional<Matrix&> Hp1,
+    boost::optional<Matrix&> Hp2) const {
+
+  // compute relative Pose3 between p1 and p2
+  Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
+
+  // convert to EssentialMatrix
+  Matrix D_hx_1P2;
+  EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
+      (Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
+
+  // Calculate derivatives if needed
+  if (Hp1) {
+    // Hp1 will already contain the 6*6 derivative D_1P2_p1
+    const Matrix& D_1P2_p1 = *Hp1;
+    // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
+    *Hp1 = D_hx_1P2 * D_1P2_p1;
+  }
+  if (Hp2) {
+    // Hp2 will already contain the 6*6 derivative D_1P2_p1
+    const Matrix& D_1P2_p2 = *Hp2;
+    // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
+    *Hp2 = D_hx_1P2 * D_1P2_p2;
+  }
+
+  // manifold equivalent of h(x)-z -> log(z,h(x))
+  return measuredE_.localCoordinates(hx); // 5D error
+}
+
+} /// namespace gtsam

gtsam/slam/EssentialMatrixConstraint.h

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2014, 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   EssentialMatrixConstraint.h
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date   Jan 5, 2014
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
+ * @addtogroup SLAM
+ */
+class EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
+
+private:
+
+  typedef EssentialMatrixConstraint This;
+  typedef NoiseModelFactor2<Pose3, Pose3> Base;
+
+  EssentialMatrix measuredE_; /** The measurement is an essential matrix */
+
+public:
+
+  // shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  EssentialMatrixConstraint() {
+  }
+
+  /**
+   *  Constructor
+   *  @param key1 key for first pose
+   *  @param key2 key for second pose
+   *  @param measuredE measured EssentialMatrix
+   *  @param model noise model, 5D !
+   */
+  EssentialMatrixConstraint(Key key1, Key key2,
+      const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
+      Base(model, key1, key2), measuredE_(measuredE) {
+  }
+
+  virtual ~EssentialMatrixConstraint() {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors */
+  virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
+      boost::optional<Matrix&> Hp1 = boost::none, //
+      boost::optional<Matrix&> Hp2 = boost::none) const;
+
+  /** return the measured */
+  const EssentialMatrix& measured() const {
+    return measuredE_;
+  }
+
+  /** number of variables attached to this factor */
+  std::size_t size() const {
+    return 2;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor2",
+            boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measuredE_);
+  }
+};
+// \class EssentialMatrixConstraint
+
+}/// namespace gtsam

gtsam/slam/RangeFactor.h

@@ -73,7 +73,7 @@ namespace gtsam {
       } else {
         hx = pose.range(point, H1, H2);
       }
-      return Vector_(1, hx - measured_);
+      return (Vector(1) << hx - measured_);
     }
 
     /** return the measured */

gtsam/slam/tests/testEssentialMatrixConstraint.cpp

@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testEssentialMatrixConstraint.cpp
+ *  @brief Unit tests for EssentialMatrixConstraint Class
+ *  @author Frank Dellaert
+ *  @author Pablo Alcantarilla
+ *  @date Jan 5, 2014
+ */
+
+#include <gtsam/slam/EssentialMatrixConstraint.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( EssentialMatrixConstraint, test ) {
+  // Create a factor
+  Key poseKey1(1);
+  Key poseKey2(2);
+  Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20);
+  Point3 trueTranslation(+0.5, -1.0, +1.0);
+  Sphere2 trueDirection(trueTranslation);
+  EssentialMatrix measurement(trueRotation, trueDirection);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25);
+  EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model);
+
+  // Create a linearization point at the zero-error point
+  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
+  Pose3 pose2(
+      Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
+      Point3(-3.37493895, 6.14660244, -8.93650986));
+
+  Vector expected = zero(5);
+  Vector actual = factor.evaluateError(pose1,pose2);
+  CHECK(assert_equal(expected, actual, 1e-8));
+
+  // Calculate numerical derivatives
+  Matrix expectedH1 = numericalDerivative11<Pose3>(
+      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
+          boost::none, boost::none), pose1);
+  Matrix expectedH2 = numericalDerivative11<Pose3>(
+      boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
+          boost::none, boost::none), pose2);
+
+  // Use the factor to calculate the derivative
+  Matrix actualH1;
+  Matrix actualH2;
+  factor.evaluateError(pose1, pose2, actualH1, actualH2);
+
+  // Verify we get the expected error
+  CHECK(assert_equal(expectedH1, actualH1, 1e-9));
+  CHECK(assert_equal(expectedH2, actualH2, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) {
     EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
 
     // Check evaluation
-    Point3 P1 = data.tracks[i].p;
+    Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
-    const Point2 pi = camera2.project(P1);
+    const Point2 pi = SimpleCamera::project_to_camera(P2);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 
@@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
     truth.insert(i, LieScalar(baseline / P1.z()));
   }
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  // parameters.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actual = result.at<EssentialMatrix>(100);
+  EXPECT(assert_equal(bodyE, actual, 1e-1));
+  for (size_t i = 0; i < 5; i++)
+    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
 } // namespace example1

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
   Pose3 x1(R,t1);
   values.insert(X(1), GeneralCamera(x1));
   Point3 l1;  values.insert(L(1), l1);
-  EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
+  EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values)));
 }
 
 static const double baseline = 5.0 ;

gtsam_unstable/base/FixedVector.h

@@ -45,23 +45,6 @@ public:
     std::copy(values, values+N, this->data());
   }
 
-  /**
-   *  nice constructor, dangerous as number of arguments must be exactly right
-   *  and you have to pass doubles !!! always use 0.0 never 0
-   *
-   *  NOTE: this will throw warnings/explode if there is no argument
-   *  before the variadic section, so there is a meaningless size argument.
-   */
-  FixedVector(size_t n, ...) {
-      va_list ap;
-      va_start(ap, n);
-      for(size_t i = 0 ; i < N ; i++) {
-        double value = va_arg(ap, double);
-        (*this)(i) = value;
-      }
-      va_end(ap);
-  }
-
   /**
    * Create vector initialized to a constant value
    * @param value constant value

gtsam_unstable/base/tests/testFixedVector.cpp

@@ -28,7 +28,7 @@ static const double tol = 1e-9;
 /* ************************************************************************* */
 TEST( testFixedVector, conversions ) {
   double data1[] = {1.0, 2.0, 3.0};
-  Vector v1  = Vector_(3, data1);
+  Vector v1  = (Vector(3) << data1[0], data1[1], data1[2]);
   TestVector3 fv1(v1), fv2(data1);
 
   Vector actFv2(fv2);
@@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) {
 
 /* ************************************************************************* */
 TEST( testFixedVector, variable_constructor ) {
-  TestVector3 act(3, 1.0, 2.0, 3.0);
+  TestVector3 act((Vector(3) << 1.0, 2.0, 3.0));
   EXPECT_DOUBLES_EQUAL(1.0, act(0), tol);
   EXPECT_DOUBLES_EQUAL(2.0, act(1), tol);
   EXPECT_DOUBLES_EQUAL(3.0, act(2), tol);
@@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) {
 
 /* ************************************************************************* */
 TEST( testFixedVector, equals ) {
-  TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0);
+  TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)),
-  TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0);
+      vec3((Vector(3) << 2.0, 3.0, 4.0));
+  TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0));
 
   EXPECT(assert_equal(vec1, vec1, tol));
   EXPECT(assert_equal(vec1, vec2, tol));
@@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) {
 /* ************************************************************************* */
 TEST( testFixedVector, static_constructors ) {
   TestVector3 actZero = TestVector3::zero();
-  TestVector3 expZero(3, 0.0, 0.0, 0.0);
+  TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0));
   EXPECT(assert_equal(expZero, actZero, tol));
 
   TestVector3 actOnes = TestVector3::ones();
-  TestVector3 expOnes(3, 1.0, 1.0, 1.0);
+  TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0));
   EXPECT(assert_equal(expOnes, actOnes, tol));
 
   TestVector3 actRepeat = TestVector3::repeat(2.3);
-  TestVector3 expRepeat(3, 2.3, 2.3, 2.3);
+  TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3));
   EXPECT(assert_equal(expRepeat, actRepeat, tol));
 
   TestVector3 actBasis = TestVector3::basis(1);
-  TestVector3 expBasis(3, 0.0, 1.0, 0.0);
+  TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0));
   EXPECT(assert_equal(expBasis, actBasis, tol));
 
   TestVector3 actDelta = TestVector3::delta(1, 2.3);
-  TestVector3 expDelta(3, 0.0, 2.3, 0.0);
+  TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0));
   EXPECT(assert_equal(expDelta, actDelta, tol));
 }
 

gtsam_unstable/geometry/BearingS2.cpp

@@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const {
 
 /* ************************************************************************* */
 Vector BearingS2::localCoordinates(const BearingS2& x) const {
-  return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0),
+  return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0),
                     elevation_.localCoordinates(x.elevation_)(0));
 }
 

gtsam_unstable/geometry/InvDepthCamera3.h

@@ -166,7 +166,7 @@ public:
     gtsam::Point3 ray = pw - pt;
     double theta = atan2(ray.y(), ray.x()); // longitude
     double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
-    return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi),
+    return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
         gtsam::LieScalar(1./depth));
   }
 

gtsam_unstable/geometry/SimPolygon2D.cpp

@@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
     // extend line by random dist and angle to get BC
     double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
     double tABC = randomAngle().theta();
-    Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC));
+    Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC));
 
     // extend from B to find C
     double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);

gtsam_unstable/geometry/SimWall2D.cpp

@@ -128,7 +128,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
 
   // calculate angle to change by
   Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
-  Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0)));
+  Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)));
 
   // create a segment to use for intersection checking
   // find the closest intersection

gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp

@@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
   Point2 expected_uv = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
+  LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
   LieScalar inv_depth(1./4);
   Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
   EXPECT(assert_equal(expected_uv, actual_uv));
@@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
   Point2 expected = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)));
+  LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
   LieScalar inv_depth(1/sqrt(3.0));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
@@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
   Point2 expected = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)));
+  LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
   LieScalar inv_depth( 1./sqrt(1.0+1+4));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
@@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
   Point2 expected = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)));
+  LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
   LieScalar inv_depth(1./sqrt(1.+16.+4.));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
@@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i
 
 TEST( InvDepthFactor, Dproject_pose)
 {
-  LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2);
+  LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
   LieScalar inv_depth(1./4);
   Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
 /* ************************************************************************* */
 TEST( InvDepthFactor, Dproject_landmark)
 {
-  LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
+  LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
   LieScalar inv_depth(1./4);
   Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
 /* ************************************************************************* */
 TEST( InvDepthFactor, Dproject_inv_depth)
 {
-  LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
+  LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
   LieScalar inv_depth(1./4);
   Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
 /* ************************************************************************* */
 TEST(InvDepthFactor, backproject)
 {
-  LieVector expected(5,0.,0.,1., 0.1,0.2);
+  LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
   LieScalar inv_depth(1./4);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
   Point2 z = inv_camera.project(expected, inv_depth);
@@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject)
 TEST(InvDepthFactor, backproject2)
 {
   // backwards facing camera
-  LieVector expected(5,-5.,-5.,2., 3., -0.1);
+  LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
   LieScalar inv_depth(1./10);
   InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
   Point2 z = inv_camera.project(expected, inv_depth);

gtsam_unstable/geometry/tests/testPose3Upright.cpp

@@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) {
   EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
   EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
 
-  Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
+  Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
   EXPECT(assert_equal(x2, x1.retract(delta12), tol));
   EXPECT(assert_equal(x1, x2.retract(delta21), tol));
 

gtsam_unstable/slam/BetweenFactorEM.h

@@ -266,7 +266,7 @@ namespace gtsam {
         }
       }
 
-      return Vector_(2, p_inlier, p_outlier);
+      return (Vector(2) << p_inlier, p_outlier);
     }
 
     /* ************************************************************************* */

gtsam_unstable/slam/CombinedImuFactor.h

@@ -1,673 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- *  @file  CombinedImuFactor.h
- *  @author Luca Carlone, Stephen Williams
- **/
-
-#pragma once
-
-/* GTSAM includes */
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/debug.h>
-
-/* External or standard includes */
-#include <ostream>
-
-
-namespace gtsam {
-
-  /**
-   * 
-   * @addtogroup SLAM
-   *
-   * REFERENCES:
-   * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-   * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
-   * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
-   * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
-   */
-
-  class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> {
-
-  public:
-
-    /** Struct to store results of preintegrating IMU measurements.  Can be build
-     * incrementally so as to avoid costly integration at time of factor construction. */
-
-    /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
-    static Matrix3 rightJacobianExpMapSO3(const Vector3& x)    {
-      // x is the axis-angle representation (exponential coordinates) for a rotation
-      double normx = norm_2(x); // rotation angle
-      Matrix3 Jr;
-      if (normx < 10e-8){
-        Jr = Matrix3::Identity();
-      }
-      else{
-        const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-        Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
-            ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
-      }
-      return Jr;
-    }
-
-    /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
-    static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x)    {
-      // x is the axis-angle representation (exponential coordinates) for a rotation
-      double normx = norm_2(x); // rotation angle
-      Matrix3 Jrinv;
-
-      if (normx < 10e-8){
-        Jrinv = Matrix3::Identity();
-      }
-      else{
-        const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-        Jrinv = Matrix3::Identity() +
-            0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
-      }
-      return Jrinv;
-    }
-
-    /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
-     * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
-    class CombinedPreintegratedMeasurements {
-    public:
-      imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
-      Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector
-      ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
-
-      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
-
-      Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias
-      Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-      Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-      Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
-      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*)
-      ///< 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 */
-      CombinedPreintegratedMeasurements(
-          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& 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
-          ///< (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))
-      {
-        // 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(),
-                                       Matrix3::Zero(), measuredAccCovariance,  Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),   Matrix3::Zero(),
-                                       Matrix3::Zero(),   Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),  Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(),   Matrix3::Zero(),     Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(),     Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),     biasAccOmegaInit.block(0,0,3,3),    biasAccOmegaInit.block(0,3,3,3),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),     biasAccOmegaInit.block(3,0,3,3),     biasAccOmegaInit.block(3,3,3,3);
-      }
-
-      CombinedPreintegratedMeasurements() :
-      biasHat(imuBias::ConstantBias()), 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))
-      {
-      }
-
-      /** print */
-      void print(const std::string& s = "Preintegrated Measurements:") const {
-        std::cout << s << std::endl;
-        biasHat.print("  biasHat");
-        std::cout << "  deltaTij " << deltaTij << std::endl;
-        std::cout << "  deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
-        std::cout << "  deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
-        deltaRij.print("  deltaRij ");
-        std::cout << "  measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
-        std::cout << "  PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
-      }
-
-      /** equals */
-      bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const {
-        return biasHat.equals(expected.biasHat, tol)
-            && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
-            && equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
-            && equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
-            && deltaRij.equals(expected.deltaRij, tol)
-            && std::fabs(deltaTij - expected.deltaTij) < tol
-            && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
-            && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
-            && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
-            && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
-            && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
-      }
-
-      /** Add a single IMU measurement to the preintegration. */
-      void integrateMeasurement(
-          const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
-          const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
-          double deltaT, ///< Time step
-          boost::optional<Pose3> body_P_sensor = boost::none ///< Sensor frame
-      ) {
-        // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
-        // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
-        Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
-        Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
-
-        // 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
-        }
-
-        const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-        const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-        const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
-
-        // Update Jacobians
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        delPdelBiasAcc += delVdelBiasAcc * deltaT;
-        delPdelBiasOmega += delVdelBiasOmega * deltaT;
-        delVdelBiasAcc += -deltaRij.matrix() * deltaT;
-        delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
-        delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr  * deltaT;
-
-        // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
-        // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
-        // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        Matrix3 Z_3x3 = Matrix3::Zero();
-        Matrix3 I_3x3 = Matrix3::Identity();
-        const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
-        const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
-
-        Rot3 Rot_j = deltaRij * Rincr;
-        const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-        const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
-
-        // Single Jacobians to propagate covariance
-        Matrix3 H_pos_pos    = I_3x3;
-        Matrix3 H_pos_vel    = I_3x3 * deltaT;
-        Matrix3 H_pos_angles = Z_3x3;
-
-        Matrix3 H_vel_pos    = Z_3x3;
-        Matrix3 H_vel_vel    = I_3x3;
-        Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-        Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
-
-        Matrix3 H_angles_pos   = Z_3x3;
-        Matrix3 H_angles_vel    = Z_3x3;
-        Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-        Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-        // overall Jacobian wrt preintegrated measurements (df/dx)
-        Matrix F(15,15);
-        F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3,
-            H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,                  Z_3x3,
-            H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega,
-            Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3,
-            Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3;
-
-
-        // first order uncertainty propagation
-        // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
-
-        Matrix G_measCov_Gt = Matrix::Zero(15,15);
-        // BLOCK DIAGONAL TERMS
-        G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
-
-//        G_measCov_Gt.block(3,3,3,3) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) +
-//            (H_vel_biasacc) * (1/deltaT) *
-//            (  measurementCovariance.block(9,9,3,3) +  measurementCovariance.block(15,15,3,3) ) *
-//            (H_vel_biasacc.transpose());
-
-        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) ) *
-                                                   (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) ) *
-                                                    (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
-        Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3);
-        G_measCov_Gt.block(3,9,3,3) = block24;
-        G_measCov_Gt.block(9,3,3,3) = block24.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();
-
-        /*
-        // overall Jacobian wrt raw measurements (df/du)
-                Matrix3 H_vel_initbiasacc =  H_vel_biasacc;
-                Matrix3 H_angles_initbiasomega =   H_angles_biasomega;
-
-                // COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases
-                Matrix G(15,21);
-                G << I_3x3 * deltaT,     Z_3x3,       Z_3x3,          Z_3x3,        Z_3x3,        Z_3x3,          Z_3x3,
-                    Z_3x3,       - H_vel_biasacc,   Z_3x3,          H_vel_biasacc,    Z_3x3,        H_vel_initbiasacc,    Z_3x3,
-                    Z_3x3,       Z_3x3,       - H_angles_biasomega,   Z_3x3,        H_angles_biasomega, Z_3x3,          H_angles_initbiasomega,
-                    Z_3x3,       Z_3x3,       Z_3x3,          I_3x3 * deltaT,   Z_3x3,        Z_3x3,          Z_3x3,
-                    Z_3x3,       Z_3x3,       Z_3x3,          Z_3x3,        I_3x3 * deltaT,   Z_3x3,          Z_3x3;
-
-        Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt;
-        std::cout  << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl;
-        double max_err=0;
-        for(int i=0;i<15;i++)
-        {
-          for(int j=0;j<15;j++)
-          {
-            if(fabs(ErrorMatrix(i,j))>max_err)
-              max_err = fabs(ErrorMatrix(i,j));
-          }
-        }
-        std::cout  << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl;
-
-        if(max_err>10e-15)
-        std::cout  << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl;
-
-        PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose();
-        */
-
-        PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt;
-
-        // Update preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        deltaPij += deltaVij * deltaT;
-        deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
-        deltaRij = deltaRij * Rincr;
-        deltaTij += deltaT;
-      }
-
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-              const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-        Vector body_t_a_body = msr_acc_t;
-        Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-      }
-
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-              const Vector3& delta_angles){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-          // Calculate the corrected measurements using the Bias object
-        Vector body_t_omega_body= msr_gyro_t;
-
-          Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-          return Rot3::Logmap(R_t_to_t0);
-      }
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-
-    private:
-      /** Serialization function */
-      friend class boost::serialization::access;
-      template<class ARCHIVE>
-      void serialize(ARCHIVE & ar, const unsigned int version) {
-        ar & BOOST_SERIALIZATION_NVP(biasHat);
-        ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
-        ar & BOOST_SERIALIZATION_NVP(deltaPij);
-        ar & BOOST_SERIALIZATION_NVP(deltaVij);
-        ar & BOOST_SERIALIZATION_NVP(deltaRij);
-        ar & BOOST_SERIALIZATION_NVP(deltaTij);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
-        ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
-      }
-    };
-
-  private:
-
-    typedef CombinedImuFactor This;
-    typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base;
-
-    CombinedPreintegratedMeasurements preintegratedMeasurements_;
-    Vector3 gravity_;
-    Vector3 omegaCoriolis_;
-
-  public:
-
-    /** Shorthand for a smart pointer to a factor */
-#ifndef _MSC_VER
-    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,
-        const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
-        const SharedNoiseModel& model) :
-      Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
-      preintegratedMeasurements_(preintegratedMeasurements),
-      gravity_(gravity),
-      omegaCoriolis_(omegaCoriolis) {
-    }
-
-    virtual ~CombinedImuFactor() {}
-
-    /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-
-    /** implement functions needed for Testable */
-
-    /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "CombinedImuFactor("
-          << keyFormatter(this->key1()) << ","
-          << keyFormatter(this->key2()) << ","
-          << keyFormatter(this->key3()) << ","
-          << keyFormatter(this->key4()) << ","
-          << keyFormatter(this->key5()) << ","
-          << keyFormatter(this->key6()) << ")\n";
-      preintegratedMeasurements_.print("  preintegrated measurements:");
-      std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-      std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
-      this->noiseModel_->print("  noise model: ");
-    }
-
-    /** equals */
-    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_)
-          && equal_with_abs_tol(gravity_, e->gravity_, tol)
-          && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol);
-    }
-
-    /** Access the preintegrated measurements. */
-    const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
-      return preintegratedMeasurements_; }
-
-    /** implement functions needed to derive from Factor */
-
-    /** vector of errors */
-    Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
-        const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
-        boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none,
-        boost::optional<Matrix&> H3 = boost::none,
-        boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none,
-        boost::optional<Matrix&> H6 = boost::none) const
-    {
-
-      const double& deltaTij = preintegratedMeasurements_.deltaTij;
-      const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
-      const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
-
-      // we give some shorter name to rotations and translations
-      const Rot3 Rot_i = pose_i.rotation();
-      const Rot3 Rot_j = pose_j.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-      const Vector3 pos_j = pose_j.translation().vector();
-
-      // We compute factor's Jacobians, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-      const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-      const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
-      const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-      const Matrix3 Jrinv_fRhat = 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(),
-                // dfV/dRi
-                Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
-                    + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
-                    // dfV/dPi
-                    Matrix3::Zero(),
-                    // 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) {
-        H2->resize(15,3);
-        (*H2) <<
-            // dfP/dVi
-            - Matrix3::Identity() * deltaTij
-            + 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
-        // dfR/dVi
-        Matrix3::Zero(),
-        //dBiasAcc/dVi
-        Matrix3::Zero(),
-        //dBiasOmega/dVi
-        Matrix3::Zero();
-
-      }
-
-      if(H3) {
-
-        H3->resize(15,6);
-        (*H3) <<
-            // dfP/dPosej
-            Matrix3::Zero(), Rot_j.matrix(),
-            // dfV/dPosej
-            Matrix::Zero(3,6),
-            // dfR/dPosej
-            Jrinv_fRhat *  ( Matrix3::Identity() ), Matrix3::Zero(),
-            //dBiasAcc/dPosej
-            Matrix3::Zero(), Matrix3::Zero(),
-            //dBiasOmega/dPosej
-            Matrix3::Zero(), Matrix3::Zero();
-      }
-
-      if(H4) {
-        H4->resize(15,3);
-        (*H4) <<
-            // dfP/dVj
-            Matrix3::Zero(),
-            // dfV/dVj
-            Matrix3::Identity(),
-            // dfR/dVj
-            Matrix3::Zero(),
-            //dBiasAcc/dVj
-            Matrix3::Zero(),
-            //dBiasOmega/dVj
-            Matrix3::Zero();
-      }
-
-      if(H5) {
-        const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
-        const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
-        const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
-
-        H5->resize(15,6);
-        (*H5) <<
-            // dfP/dBias_i
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
-            // dfV/dBias_i
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
-            // dfR/dBias_i
-            Matrix::Zero(3,3),
-            Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
-            //dBiasAcc/dBias_i
-            -Matrix3::Identity(), Matrix3::Zero(),
-            //dBiasOmega/dBias_i
-            Matrix3::Zero(), -Matrix3::Identity();
-      }
-
-      if(H6) {
-
-        H6->resize(15,6);
-        (*H6) <<
-            // dfP/dBias_j
-            Matrix3::Zero(), Matrix3::Zero(),
-            // dfV/dBias_j
-            Matrix3::Zero(), Matrix3::Zero(),
-            // dfR/dBias_j
-            Matrix3::Zero(), Matrix3::Zero(),
-            //dBiasAcc/dBias_j
-            Matrix3::Identity(), Matrix3::Zero(),
-            //dBiasOmega/dBias_j
-            Matrix3::Zero(), Matrix3::Identity();
-      }
-
-
-      // Evaluate residual error, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 fp =
-          pos_j - pos_i
-          - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
-              + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
-              + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
-              - vel_i * deltaTij
-              + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-              - 0.5 * gravity_ * deltaTij*deltaTij;
-
-      const Vector3 fv =
-          vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
-              + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
-              + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
-              + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-              - gravity_ * deltaTij;
-
-      const Vector3 fR = Rot3::Logmap(fRhat);
-
-      const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
-
-      const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
-
-      Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
-
-      return r;
-    }
-
-
-    /** 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_i, imuBias::ConstantBias& bias_j,
-        const CombinedPreintegratedMeasurements preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis)
-    {
-
-      const double& deltaTij = preintegratedMeasurements.deltaTij;
-      const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
-      const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
-
-      const Rot3 Rot_i = pose_i.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-
-      // Predict state at time j
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-          + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-          + vel_i * deltaTij
-          - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          + 0.5 * gravity * deltaTij*deltaTij;
-
-      vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-          + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-          - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-          + gravity * deltaTij);
-
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-      const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-      pose_j = Pose3( Rot_j, Point3(pos_j) );
-
-      bias_j = bias_i;
-    }
-
-
-  private:
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor6",
-          boost::serialization::base_object<Base>(*this));
-      ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
-      ar & BOOST_SERIALIZATION_NVP(gravity_);
-      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
-    }
-  }; // \class CombinedImuFactor
-
-} /// namespace gtsam

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h

@@ -537,7 +537,7 @@ public:
 
     Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
 
-    Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
+    Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
     omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
 
     // Calculating g
@@ -551,7 +551,7 @@ public:
     double Ro( sqrt(Rp*Rm) );           // mean earth radius of curvature
     double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
     double g_calc( g0/( pow(1 + height/Ro, 2) ) );
-    g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
+    g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
 
 
     // Calculate rho
@@ -560,7 +560,7 @@ public:
     double rho_E = -Vn/(Rm + height);
     double rho_N = Ve/(Rp + height);
     double rho_U = Ve*tan(lat_new)/(Rp + height);
-    rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
+    rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
   }
 
   static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

gtsam_unstable/slam/ImuFactor.h

@@ -1,565 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- *  @file  ImuFactor.h
- *  @author Luca Carlone, Stephen Williams, Richard Roberts
- **/
-
-#pragma once
-
-/* GTSAM includes */
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/debug.h>
-
-/* External or standard includes */
-#include <ostream>
-
-
-namespace gtsam {
-
-  /**
-   * 
-   * @addtogroup SLAM
-   *    * REFERENCES:
-   * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-   * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
-   * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
-   * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
-   */
-
-  class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
-
-  public:
-
-    /** Struct to store results of preintegrating IMU measurements.  Can be build
-     * incrementally so as to avoid costly integration at time of factor construction. */
-
-    /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
-    static Matrix3 rightJacobianExpMapSO3(const Vector3& x)    {
-      // x is the axis-angle representation (exponential coordinates) for a rotation
-      double normx = norm_2(x); // rotation angle
-      Matrix3 Jr;
-      if (normx < 10e-8){
-        Jr = Matrix3::Identity();
-      }
-      else{
-        const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-        Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
-            ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
-      }
-      return Jr;
-    }
-
-    /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
-    static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x)    {
-      // x is the axis-angle representation (exponential coordinates) for a rotation
-      double normx = norm_2(x); // rotation angle
-      Matrix3 Jrinv;
-
-      if (normx < 10e-8){
-        Jrinv = Matrix3::Identity();
-      }
-      else{
-        const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-        Jrinv = Matrix3::Identity() +
-            0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
-      }
-      return Jrinv;
-    }
-
-    /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
-         * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
-    class PreintegratedMeasurements {
-    public:
-      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 deltaVij; ///< Preintegrated relative velocity (in global frame)
-      Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
-      double deltaTij; ///< Time interval from i to j
-
-      Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias
-      Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-      Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-      Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
-      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*)
-
-      Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i)
-      Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j)
-
-      /** 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 Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i)
-      ) : 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),
-      initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate)
-      {
-        measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
-                                       Matrix3::Zero(), measuredAccCovariance,  Matrix3::Zero(),
-                                       Matrix3::Zero(),   Matrix3::Zero(), measuredOmegaCovariance;
-        PreintMeasCov = Matrix::Zero(9,9);
-      }
-
-      PreintegratedMeasurements() :
-      biasHat(imuBias::ConstantBias()), 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),
-      initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero())
-      {
-          measurementCovariance =  Matrix::Zero(9,9);
-          PreintMeasCov = Matrix::Zero(9,9);
-      }
-
-      /** print */
-      void print(const std::string& s = "Preintegrated Measurements:") const {
-        std::cout << s << std::endl;
-        biasHat.print("  biasHat");
-        std::cout << "  deltaTij " << deltaTij << std::endl;
-        std::cout << "  deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
-        std::cout << "  deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
-        deltaRij.print("  deltaRij ");
-        std::cout << "  measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
-        std::cout << "  PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
-      }
-
-      /** equals */
-      bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
-        return biasHat.equals(expected.biasHat, tol)
-            && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
-            && equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
-            && equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
-            && deltaRij.equals(expected.deltaRij, tol)
-            && std::fabs(deltaTij - expected.deltaTij) < tol
-            && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
-            && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
-            && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
-            && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
-            && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
-      }
-
-      /** Add a single IMU measurement to the preintegration. */
-      void integrateMeasurement(
-          const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
-          const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
-          double deltaT, ///< Time step
-          boost::optional<Pose3> body_P_sensor = boost::none ///< Sensor frame
-      ) {
-
-        // NOTE: order is important here because each update uses old values.
-        // First we compensate the measurements for the bias
-        Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
-        Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
-
-        finalRotationRate = correctedOmega;
-
-        // 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
-        }
-
-        const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-        const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-
-        const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
-
-        // Update Jacobians
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        delPdelBiasAcc += delVdelBiasAcc * deltaT;
-        delPdelBiasOmega += delVdelBiasOmega * deltaT;
-        delVdelBiasAcc += -deltaRij.matrix() * deltaT;
-        delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
-        delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr  * deltaT;
-
-        // Update preintegrated mesurements covariance
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        Matrix3 Z_3x3 = Matrix3::Zero();
-        Matrix3 I_3x3 = Matrix3::Identity();
-        const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
-        const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
-
-        Rot3 Rot_j = deltaRij * Rincr;
-        const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-        const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
-
-        // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
-        // can be seen as a prediction phase in an EKF framework
-        Matrix H_pos_pos    = I_3x3;
-        Matrix H_pos_vel    = I_3x3 * deltaT;
-        Matrix H_pos_angles = Z_3x3;
-
-        Matrix H_vel_pos    = Z_3x3;
-        Matrix H_vel_vel    = I_3x3;
-        Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-
-        Matrix H_angles_pos   = Z_3x3;
-        Matrix H_angles_vel    = Z_3x3;
-        Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-        // overall Jacobian wrt preintegrated measurements (df/dx)
-        Matrix F(9,9);
-        F << H_pos_pos, H_pos_vel,  H_pos_angles,
-            H_vel_pos, H_vel_vel, H_vel_angles,
-            H_angles_pos, H_angles_vel, H_angles_angles;
-
-
-        // first order uncertainty propagation
-        // the deltaT allows to pass from continuous time noise to discrete time noise
-        PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
-
-        // Update preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        deltaPij += deltaVij * deltaT;
-        deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
-        deltaRij = deltaRij * Rincr;
-        deltaTij += deltaT;
-      }
-
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-              const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-        Vector body_t_a_body = msr_acc_t;
-        Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-      }
-
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-              const Vector3& delta_angles){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-          // Calculate the corrected measurements using the Bias object
-        Vector body_t_omega_body= msr_gyro_t;
-
-          Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-          return Rot3::Logmap(R_t_to_t0);
-      }
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-
-    private:
-      /** Serialization function */
-      friend class boost::serialization::access;
-      template<class ARCHIVE>
-      void serialize(ARCHIVE & ar, const unsigned int version) {
-        ar & BOOST_SERIALIZATION_NVP(biasHat);
-        ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
-        ar & BOOST_SERIALIZATION_NVP(deltaPij);
-        ar & BOOST_SERIALIZATION_NVP(deltaVij);
-        ar & BOOST_SERIALIZATION_NVP(deltaRij);
-        ar & BOOST_SERIALIZATION_NVP(deltaTij);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
-        ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
-      }
-    };
-
-  private:
-
-    typedef ImuFactor This;
-    typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
-
-    PreintegratedMeasurements preintegratedMeasurements_;
-    Vector3 gravity_;
-    Vector3 omegaCoriolis_;
-    boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
-
-  public:
-
-    /** Shorthand for a smart pointer to a factor */
-#ifndef _MSC_VER
-    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,
-        const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
-        const SharedNoiseModel& model, boost::optional<Pose3> body_P_sensor = boost::none) :
-      Base(model, pose_i, vel_i, pose_j, vel_j, bias),
-      preintegratedMeasurements_(preintegratedMeasurements),
-      gravity_(gravity),
-      omegaCoriolis_(omegaCoriolis),
-      body_P_sensor_(body_P_sensor) {
-    }
-
-    virtual ~ImuFactor() {}
-
-    /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-
-    /** implement functions needed for Testable */
-
-    /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "ImuFactor("
-          << keyFormatter(this->key1()) << ","
-          << keyFormatter(this->key2()) << ","
-          << keyFormatter(this->key3()) << ","
-          << keyFormatter(this->key4()) << ","
-          << keyFormatter(this->key5()) << ")\n";
-      preintegratedMeasurements_.print("  preintegrated measurements:");
-      std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-      std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
-      this->noiseModel_->print("  noise model: ");
-      if(this->body_P_sensor_)
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
-    }
-
-    /** equals */
-    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_)
-          && 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_)));
-    }
-
-    /** Access the preintegrated measurements. */
-    const PreintegratedMeasurements& preintegratedMeasurements() const {
-      return preintegratedMeasurements_; }
-
-    /** implement functions needed to derive from Factor */
-
-    /** vector of errors */
-    Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
-        const imuBias::ConstantBias& bias,
-        boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none,
-        boost::optional<Matrix&> H3 = boost::none,
-        boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none) const
-    {
-
-      const double& deltaTij = preintegratedMeasurements_.deltaTij;
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
-
-      // we give some shorter name to rotations and translations
-      const Rot3 Rot_i = pose_i.rotation();
-      const Rot3 Rot_j = pose_j.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-      const Vector3 pos_j = pose_j.translation().vector();
-
-      // We compute factor's Jacobians
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-      const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-      const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
-      const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-      const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
-
-      if(H1) {
-        H1->resize(9,6);
-        (*H1) <<
-            // dfP/dRi
-            Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
-                + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
-                // dfP/dPi
-                - Rot_i.matrix(),
-                // dfV/dRi
-                Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
-                    + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
-                    // dfV/dPi
-                    Matrix3::Zero(),
-                    // 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) <<
-            // dfP/dVi
-            - Matrix3::Identity() * deltaTij
-            + 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
-        // dfR/dVi
-        Matrix3::Zero();
-
-      }
-      if(H3) {
-
-        H3->resize(9,6);
-        (*H3) <<
-            // dfP/dPosej
-            Matrix3::Zero(), Rot_j.matrix(),
-            // dfV/dPosej
-            Matrix::Zero(3,6),
-            // dfR/dPosej
-            Jrinv_fRhat *  ( Matrix3::Identity() ), Matrix3::Zero();
-      }
-      if(H4) {
-        H4->resize(9,3);
-        (*H4) <<
-            // dfP/dVj
-            Matrix3::Zero(),
-            // dfV/dVj
-            Matrix3::Identity(),
-            // dfR/dVj
-            Matrix3::Zero();
-      }
-      if(H5) {
-
-        const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
-        const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
-        const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
-
-        H5->resize(9,6);
-        (*H5) <<
-            // dfP/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
-            // dfV/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
-            // dfR/dBias
-            Matrix::Zero(3,3),
-            Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
-      }
-
-      // Evaluate residual error, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 fp =
-          pos_j - pos_i
-          - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
-              + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
-              + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
-              - vel_i * deltaTij
-              + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-              - 0.5 * gravity_ * deltaTij*deltaTij;
-
-      const Vector3 fv =
-          vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
-              + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
-              + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
-              + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-              - gravity_ * deltaTij;
-
-      const Vector3 fR = Rot3::Logmap(fRhat);
-
-      Vector r(9); r << fp, fv, fR;
-      return r;
-    }
-
-
-    /** 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<Pose3> body_P_sensor = boost::none)
-    {
-
-      const double& deltaTij = preintegratedMeasurements.deltaTij;
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
-
-      const Rot3 Rot_i = pose_i.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-
-      // Predict state at time j
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-          + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-          + vel_i * deltaTij
-          - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          + 0.5 * gravity * deltaTij*deltaTij;
-
-      vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-          + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-          + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-          - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-          + gravity * deltaTij);
-
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-      const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-      pose_j = Pose3( Rot_j, Point3(pos_j) );
-    }
-
-
-  private:
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor5",
-          boost::serialization::base_object<Base>(*this));
-      ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
-      ar & BOOST_SERIALIZATION_NVP(gravity_);
-      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
-      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-    }
-  }; // \class ImuFactor
-
-} /// namespace gtsam

gtsam_unstable/slam/InvDepthFactor3.h

@@ -96,7 +96,7 @@ public:
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       return gtsam::ones(2) * 2.0 * K_->fx();
     }
-    return gtsam::Vector_(1, 0.0);
+    return (gtsam::Vector(1) << 0.0);
   }
 
   /** return the measurement */

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -96,7 +96,7 @@ public:
           << std::endl;
       return gtsam::ones(2) * 2.0 * K_->fx();
     }
-    return gtsam::Vector_(1, 0.0);
+    return (gtsam::Vector(1) << 0.0);
   }
 
   /// Evaluate error h(x)-z and optionally derivatives

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -99,7 +99,7 @@ public:
           << std::endl;
       return gtsam::ones(2) * 2.0 * K_->fx();
     }
-    return gtsam::Vector_(1, 0.0);
+    return (gtsam::Vector(1) << 0.0);
   }
 
   /// Evaluate error h(x)-z and optionally derivatives

gtsam_unstable/slam/Mechanization_bRn2.h

@@ -35,7 +35,7 @@ public:
   }
 
   Vector b_g(double g_e) {
-    Vector n_g = Vector_(3, 0.0, 0.0, g_e);
+    Vector n_g = (Vector(3) << 0.0, 0.0, g_e);
     return (bRn_ * n_g).vector();
   }
 

gtsam_unstable/slam/PartialPriorFactor.h

@@ -70,7 +70,7 @@ namespace gtsam {
 
     /** Single Element Constructor: acts on a single parameter specified by idx */
     PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
-      Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
+      Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
       assert(model->dim() == 1);
       this->fillH();
     }

gtsam_unstable/slam/RelativeElevationFactor.cpp

@@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
     *H2 = zeros(1, 3);
     (*H2)(0, 2) = -1.0;
   }
-  return Vector_(1, hx - measured_);
+  return (Vector(1) << hx - measured_);
 }
 
 /* ************************************************************************* */

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -276,7 +276,7 @@ namespace gtsam {
         }
       }
 
-      return Vector_(2, p_inlier, p_outlier);
+      return (Vector(2) << p_inlier, p_outlier);
     }
 
     /* ************************************************************************* */

gtsam_unstable/slam/tests/testAHRS.cpp

@@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) {
   Mechanization_bRn2 mech;
   KalmanFilter::State state;
 //  boost::tie(mech,state) = ahrs.initialize(g_e);
-//  Vector u = Vector_(3,0.05,0.0,0.0);
+//  Vector u = (Vector(3) << 0.05,0.0,0.0);
 //  double dt = 2;
 //  Rot3 expected;
 //  Mechanization_bRn2 mech2 = mech.integrate(u,dt);

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
-  LieVector Vel1(3, 0.50, -0.50, 0.40);
+  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
   imuBias::ConstantBias Bias1;
   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector expectedVel2(3, 0.51, -0.48, 0.43);
+  LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
   Pose3 actualPose2;
   LieVector actualVel2;
   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
   Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector Vel1(3, 0.50, -0.50, 0.40);
+  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
-  LieVector Vel2(3, 0.51, -0.48, 0.43);
+  LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
 
   Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
   Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
-  LieVector Vel1(3,0.0,0.0,0.0);
+  LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0));
-  LieVector Vel2(3,0.0,0.0,0.0);
+  LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
       -0.652537293,   0.709880342,   0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1(3,0.5,-0.5,0.4);
+  LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
   Rot3 R2(0.473618898,   0.119523052,   0.872582019,
        0.609241153,    0.67099888,  -0.422594037,
       -0.636011287,   0.731761397,   0.244979388);
@@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
       -0.652537293,   0.709880342,   0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1(3,0.5,-0.5,0.4);
+  LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
   Rot3 R2(0.473618898,   0.119523052,   0.872582019,
        0.609241153,    0.67099888,  -0.422594037,
       -0.636011287,   0.731761397,   0.244979388);
   Point3 t2(2.052670960415706,   0.977252139079380,   2.942482135362800);
   Pose3 Pose2(R2, t2);
-  LieVector Vel2(3,0.510000000000000,  -0.480000000000000,   0.430000000000000);
+  LieVector Vel2((Vector(3) << 0.510000000000000,  -0.480000000000000,   0.430000000000000));
   imuBias::ConstantBias Bias1;
 
   Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
@@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
-  LieVector Vel1(3, 0.50, -0.50, 0.40);
+  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
   imuBias::ConstantBias Bias1;
   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector expectedVel2(3, 0.51, -0.48, 0.43);
+  LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
   Pose3 actualPose2;
   LieVector actualVel2;
   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
   Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector Vel1(3, 0.50, -0.50, 0.40);
+  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
-  LieVector Vel2(3, 0.51, -0.48, 0.43);
+  LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
 
   Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
   Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0));
-  LieVector Vel1(3,0.0,0.0,0.0);
+  LieVector Vel1((Vector(3) << 0.0,0.0,0.0));
-  LieVector Vel2(3,0.0,0.0,0.0);
+  LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
       -0.652537293,  0.709880342,  0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1(3,0.5,-0.5,0.4);
+  LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
   Rot3 R2(0.473618898,   0.119523052,  0.872582019,
        0.609241153,   0.67099888, -0.422594037,
       -0.636011287,  0.731761397,  0.244979388);
@@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
       -0.652537293,  0.709880342,  0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1(3,0.5,-0.5,0.4);
+  LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
   Rot3 R2(0.473618898,   0.119523052,  0.872582019,
        0.609241153,   0.67099888, -0.422594037,
       -0.636011287,  0.731761397,  0.244979388);
   Point3 t2(2.052670960415706,   0.977252139079380,   2.942482135362800);
   Pose3 Pose2(R2, t2);
-  LieVector Vel2(3,0.510000000000000,  -0.480000000000000,   0.430000000000000);
+  LieVector Vel2((Vector(3) << 0.510000000000000,  -0.480000000000000,   0.430000000000000));
   imuBias::ConstantBias Bias1;
 
   Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;

gtsam_unstable/slam/tests/testInvDepthFactor3.cpp

@@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) {
   Point2 expected_uv = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector inv_landmark(5, 0., 0., 1., 0., 0.);
+  LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
   // initialize inverse depth with "incorrect" depth of 1/4
   // in reality this is 1/5, but initial depth is guessed
   LieScalar inv_depth(1./4);

gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp

@@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) {
   double theta = atan2(ray.y(), ray.x());
   double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
   double rho = 1./ray.norm();
-  LieVector expected(6, x, y, z, theta, phi, rho);
+  LieVector expected((Vector(6) << x, y, z, theta, phi, rho));
 
 
   

gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp

@@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) {
   double theta = atan2(ray.y(), ray.x());
   double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
   double rho = 1./ray.norm();
-  LieVector expected(3, theta, phi, rho);
+  LieVector expected((Vector(3) << theta, phi, rho));
 
 
   

gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp

@@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) {
   double theta = atan2(landmark_p1.x(), landmark_p1.z());
   double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
   double rho = 1./landmark_p1.norm();
-  LieVector expected(3, theta, phi, rho);
+  LieVector expected((Vector(3) << theta, phi, rho));
 
 
   

tests/testBoundingConstraint.cpp

@@ -187,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
   EXPECT(!rangeBound.isGreaterThan());
   EXPECT(rangeBound.dim() == 1);
 
-  EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1)));
+  EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1)));
   EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
   EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
   EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));

tests/testGaussianJunctionTreeB.cpp

@@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) {
   // Create a simple graph
   NonlinearFactorGraph fg;
   fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
-  fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))));
+  fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0))));
 
   Values init;
   init.insert(X(0), Pose2());

tests/testNonlinearFactor.cpp

@@ -266,10 +266,10 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor4) {
   TestFactor4 tf;
   Values tv;
-  tv.insert(X(1), LieVector(1, 1.0));
+  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector(1, 2.0));
+  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector(1, 3.0));
+  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector(1, 4.0));
+  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
   EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@@ -312,11 +312,11 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor5) {
   TestFactor5 tf;
   Values tv;
-  tv.insert(X(1), LieVector(1, 1.0));
+  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector(1, 2.0));
+  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector(1, 3.0));
+  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector(1, 4.0));
+  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
-  tv.insert(X(5), LieVector(1, 5.0));
+  tv.insert(X(5), LieVector((Vector(1) << 5.0)));
   EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@@ -364,12 +364,12 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor6) {
   TestFactor6 tf;
   Values tv;
-  tv.insert(X(1), LieVector(1, 1.0));
+  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector(1, 2.0));
+  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector(1, 3.0));
+  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector(1, 4.0));
+  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
-  tv.insert(X(5), LieVector(1, 5.0));
+  tv.insert(X(5), LieVector((Vector(1) << 5.0)));
-  tv.insert(X(6), LieVector(1, 6.0));
+  tv.insert(X(6), LieVector((Vector(1) << 6.0)));
   EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));

tests/testSerializationSLAM.cpp

@@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) {
 /* ************************************************************************* */
 TEST (testSerializationSLAM, factors) {
 
-  LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0);
+  LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0));
-  LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0);
+  LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0));
   Point2 point2(1.0, 2.0);
   StereoPoint2 stereoPoint2(1.0, 2.0, 3.0);
   Point3 point3(1.0, 2.0, 3.0);

tests/testSimulated2DOriented.cpp

@@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior )
 TEST( simulated2DOriented, constructor )
 {
   Pose2 measurement(0.2, 0.3, 0.1);
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.));
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.));
   simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));
 
   simulated2DOriented::Values config;