Merged develop into feature/fixExpressionFactorKeys

gtsam.h

@@ -1798,9 +1798,6 @@ class Values {
   void insert(size_t j, Vector t);
   void insert(size_t j, Matrix t);
 
-  // Fixed size version
-  void insertFixed(size_t j, Vector t, size_t n);
-
   void update(size_t j, const gtsam::Point2& t);
   void update(size_t j, const gtsam::Point3& t);
   void update(size_t j, const gtsam::Rot2& t);
@@ -1818,12 +1815,6 @@ class Values {
   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
   T at(size_t j);
 
-  /// Fixed size versions, for n in 1..9
-  void insertFixed(size_t j, Vector t, size_t n);
-
-  /// Fixed size versions, for n in 1..9
-  Vector atFixed(size_t j, size_t n);
-
   /// version for double
   void insertDouble(size_t j, double c);
   double atDouble(size_t j) const;
@@ -2489,10 +2480,30 @@ class NavState {
   gtsam::Pose3 pose() const;
 };
 
+#include <gtsam/navigation/PreintegratedRotation.h>
+virtual class PreintegratedRotationParams {
+  PreintegratedRotationParams();
+  void setGyroscopeCovariance(Matrix cov);
+  void setOmegaCoriolis(Vector omega);
+  void setBodyPSensor(const gtsam::Pose3& pose);
+
+  Matrix getGyroscopeCovariance() const;
+
+  // TODO(frank): allow optional
+  //  boost::optional<Vector3> getOmegaCoriolis() const;
+  //  boost::optional<Pose3>   getBodyPSensor()   const;
+};
+
 #include <gtsam/navigation/PreintegrationParams.h>
-class PreintegrationParams {
+virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
-  // TODO(frank): add setters/getters or make this MATLAB wrapper feature
+  void setAccelerometerCovariance(Matrix cov);
+  void setIntegrationCovariance(Matrix cov);
+  void setUse2ndOrderCoriolis(bool flag);
+
+  Matrix getAccelerometerCovariance() const;
+  Matrix getIntegrationCovariance()   const;
+  bool   getUse2ndOrderCoriolis()     const;
 };
 
 #include <gtsam/navigation/PreintegrationBase.h>

gtsam/base/Matrix.cpp

@@ -38,26 +38,6 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Matrix zeros( size_t m, size_t n ) {
-  return Matrix::Zero(m,n);
-}
-
-/* ************************************************************************* */
-Matrix ones( size_t m, size_t n ) {
-  return Matrix::Ones(m,n);
-}
-
-/* ************************************************************************* */
-Matrix eye( size_t m, size_t n) {
-  return Matrix::Identity(m, n);
-}
-
-/* ************************************************************************* */ 
-Matrix diag(const Vector& v) {
-  return v.asDiagonal();
-}
-
 /* ************************************************************************* */
 bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) {
 
@@ -146,16 +126,6 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
   }
 }
 
-/* ************************************************************************* */
-void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) {
-  e += alpha * A * x;
-}
-
-/* ************************************************************************* */
-void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
-  e += A * x;
-}
-
 /* ************************************************************************* */
 Vector operator^(const Matrix& A, const Vector & v) {
   if (A.rows()!=v.size()) throw std::invalid_argument(
@@ -166,21 +136,6 @@ Vector operator^(const Matrix& A, const Vector & v) {
   return A.transpose() * v;
 }
 
-/* ************************************************************************* */
-void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) {
-  x += alpha * A.transpose() * e;
-}
-
-/* ************************************************************************* */
-void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
-  x += A.transpose() * e;
-}
-
-/* ************************************************************************* */
-void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
-  x += alpha * A.transpose() * e;
-}
-
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
@@ -250,7 +205,7 @@ Matrix diag(const std::vector<Matrix>& Hs) {
     rows+= Hs[i].rows();
     cols+= Hs[i].cols();
   }
-  Matrix results = zeros(rows,cols);
+  Matrix results = Matrix::Zero(rows,cols);
   size_t r = 0, c = 0;
   for (size_t i = 0; i<Hs.size(); ++i) {
     insertSub(results, Hs[i], r, c);
@@ -260,16 +215,6 @@ Matrix diag(const std::vector<Matrix>& Hs) {
   return results;
 }
 
-/* ************************************************************************* */
-void insertColumn(Matrix& A, const Vector& col, size_t j) {
-  A.col(j) = col;
-}
-
-/* ************************************************************************* */
-void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) {
-  A.col(j).segment(i, col.size()) = col;
-}
-
 /* ************************************************************************* */
 Vector columnNormSquare(const Matrix &A) {
   Vector v (A.cols()) ;
@@ -279,24 +224,13 @@ Vector columnNormSquare(const Matrix &A) {
   return v ;
 }
 
-/* ************************************************************************* */
-void solve(Matrix& A, Matrix& B) {
-  // Eigen version - untested
-  B = A.fullPivLu().solve(B);
-}
-
-/* ************************************************************************* */
-Matrix inverse(const Matrix& A) {
-  return A.inverse();
-}
-
 /* ************************************************************************* */
 /** Householder QR factorization, Golub & Van Loan p 224, explicit version    */
 /* ************************************************************************* */
 pair<Matrix,Matrix> qr(const Matrix& A) {
   const size_t m = A.rows(), n = A.cols(), kprime = min(m,n);
 
-  Matrix Q=eye(m,m),R(A);
+  Matrix Q=Matrix::Identity(m,m),R(A);
   Vector v(m);
 
   // loop over the kprime first columns
@@ -319,7 +253,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
       v(k) = k<j ? 0.0 : vjm(k-j);
 
     // create Householder reflection matrix Qj = I-beta*v*v'
-    Matrix Qj = eye(m) - beta * v * v.transpose();
+    Matrix Qj = Matrix::Identity(m,m) - beta * v * v.transpose();
 
     R = Qj * R; // update R
     Q = Q * Qj; // update Q
@@ -339,7 +273,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
   list<boost::tuple<Vector, double, double> > results;
 
   Vector pseudo(m); // allocate storage for pseudo-inverse
-  Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once
+  Vector weights = sigmas.array().square().inverse(); // calculate weights once
 
   // We loop over all columns, because the columns that can be eliminated
   // are not necessarily contiguous. For each one, estimate the corresponding
@@ -356,7 +290,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
     if (precision < 1e-8) continue;
 
     // create solution and copy into r
-    Vector r(basis(n, j));
+    Vector r(Vector::Unit(n,j));
     for (size_t j2=j+1; j2<n; ++j2)
       r(j2) = pseudo.dot(A.col(j2));
 
@@ -600,7 +534,7 @@ Matrix RtR(const Matrix &A)
 Matrix cholesky_inverse(const Matrix &A)
 {
   Eigen::LLT<Matrix> llt(A);
-  Matrix inv = eye(A.rows());
+  Matrix inv = Matrix::Identity(A.rows(),A.rows());
   llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
   return inv*inv.transpose();
 }
@@ -612,7 +546,7 @@ Matrix cholesky_inverse(const Matrix &A)
 // inv(B' * B) == A
 Matrix inverse_square_root(const Matrix& A) {
   Eigen::LLT<Matrix> llt(A);
-  Matrix inv = eye(A.rows());
+  Matrix inv = Matrix::Identity(A.rows(),A.rows());
   llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
   return inv.transpose();
 }
@@ -648,7 +582,7 @@ boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
 
 /* ************************************************************************* */
 Matrix expm(const Matrix& A, size_t K) {
-  Matrix E = eye(A.rows()), A_k = eye(A.rows());
+  Matrix E = Matrix::Identity(A.rows(),A.rows()), A_k = Matrix::Identity(A.rows(),A.rows());
   for(size_t k=1;k<=K;k++) {
     A_k = A_k*A/double(k);
     E = E + A_k;

gtsam/base/Matrix.h

@@ -16,6 +16,7 @@
  * @author  Kai Ni
  * @author  Frank Dellaert
  * @author  Alex Cunningham
+ * @author  Alex Hagiopol
  */
 
 // \callgraph
@@ -23,17 +24,17 @@
 #pragma once
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
-#include <gtsam/config.h>      // Configuration from CMake
+#include <gtsam/config.h>
-
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 #include <Eigen/Core>
 #include <Eigen/Cholesky>
 #include <Eigen/LU>
+#endif
 #include <boost/format.hpp>
 #include <boost/function.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/math/special_functions/fpclassify.hpp>
 
-
 /**
  * Matrix is a typedef in the gtsam namespace
  * TODO: make a version to work with matlab wrapping
@@ -74,40 +75,8 @@ GTSAM_MAKE_MATRIX_DEFS(9);
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
-// Matlab-like syntax
-
 /**
- * Creates an zeros matrix, with matlab-like syntax
+ * equals with a tolerance
- *
- * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros,
- * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error.
- */
-GTSAM_EXPORT Matrix zeros(size_t m, size_t n);
-
-/**
- * Creates an ones matrix, with matlab-like syntax
- */
-GTSAM_EXPORT Matrix ones(size_t m, size_t n);
-
-/**
- * Creates an identity matrix, with matlab-like syntax
- *
- * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
- * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error.
- */
-GTSAM_EXPORT Matrix eye(size_t m, size_t n);
-
-/**
- * Creates a square identity matrix, with matlab-like syntax
- *
- * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
- * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error.
- */
-inline Matrix eye( size_t m ) { return eye(m,m); }
-GTSAM_EXPORT Matrix diag(const Vector& v);
-
-/**
- * equals with an tolerance
  */
 template <class MATRIX>
 bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
@@ -166,37 +135,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to
  */
 GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
 
-/**
- * BLAS Level-2 style e <- e + alpha*A*x
- */
-GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e);
-
-/**
- * BLAS Level-2 style e <- e + A*x
- */
-GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e);
-
 /**
  * overload ^ for trans(A)*v
  * We transpose the vectors for speed.
  */
 GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
 
-/**
- * BLAS Level-2 style x <- x + alpha*A'*e
- */
-GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x);
-
-/**
- * BLAS Level-2 style x <- x + A'*e
- */
-GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x);
-
-/**
- * BLAS Level-2 style x <- x + alpha*A'*e
- */
-GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);
-
 /** products using old-style format to improve compatibility */
 template<class MATRIX>
 inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
@@ -281,19 +225,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
   return A.row(j);
 }
 
-/**
- * inserts a column into a matrix IN PLACE
- * NOTE: there is no size checking
- * Alternate form allows for vectors smaller than the whole column to be inserted
- * @param A matrix to be modified in place
- * @param col is the vector to be inserted
- * @param j is the index to insert the column
- */
-GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j);
-GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);
-
-GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
-
 /**
  * Zeros all of the elements below the diagonal of a matrix, in place
  * @param A is a matrix, to be modified in place
@@ -355,17 +286,6 @@ inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedTy
   return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
 }
 
-/**
- * solve AX=B via in-place Lu factorization and backsubstitution
- * After calling, A contains LU, B the solved RHS vectors
- */
-GTSAM_EXPORT void solve(Matrix& A, Matrix& B);
-
-/**
- * invert A
- */
-GTSAM_EXPORT Matrix inverse(const Matrix& A);
-
 /**
  * QR factorization, inefficient, best use imperative householder below
  * m*n matrix -> m*m Q, m*n R
@@ -492,12 +412,6 @@ inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
 /** Use Cholesky to calculate inverse square root of a matrix */
 GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
 
-/** Calculate the LL^t decomposition of a S.P.D matrix */
-GTSAM_EXPORT Matrix LLt(const Matrix& A);
-
-/** Calculate the R^tR decomposition of a S.P.D matrix */
-GTSAM_EXPORT Matrix RtR(const Matrix& A);
-
 /** Return the inverse of a S.P.D. matrix.  Inversion is done via Cholesky decomposition. */
 GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
 
@@ -603,6 +517,28 @@ struct MultiplyWithInverseFunction {
   const Operator phi_;
 };
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); }
+inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); }
+inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); }
+inline Matrix eye( size_t m ) { return eye(m,m); }
+inline Matrix diag(const Vector& v) { return v.asDiagonal(); }
+inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; }
+inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; }
+inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; }
+inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; }
+inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; }
+inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; }
+inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; }
+inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); }
+inline Matrix inverse(const Matrix& A) { return A.inverse(); }
+#endif
+
+GTSAM_EXPORT Matrix LLt(const Matrix& A);
+
+GTSAM_EXPORT Matrix RtR(const Matrix& A);
+
+GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>

gtsam/base/Vector.cpp

@@ -33,25 +33,6 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-bool zero(const Vector& v) {
-  bool result = true;
-  size_t n = v.size();
-  for( size_t j = 0 ; j < n ; j++)
-    result = result && (v(j) == 0.0);
-  return result;
-}
-
-/* ************************************************************************* */
-Vector repeat(size_t n, double value) {
-  return Vector::Constant(n, value);
-}
-
-/* ************************************************************************* */
-Vector delta(size_t n, size_t i, double value) {
-  return Vector::Unit(n, i) * value;
-}
-
 /* ************************************************************************* */
 //3 argument call
 void print(const Vector& v, const string& s, ostream& stream) {
@@ -176,28 +157,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
   return flag;
 }
 
-/* ************************************************************************* */
-ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
-  return v.segment(i1,i2-i1);
-}
-
-/* ************************************************************************* */
-void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
-  fullVector.segment(i, subVector.size()) = subVector;
-}
-
-/* ************************************************************************* */
-Vector emul(const Vector &a, const Vector &b) {
-  assert (b.size()==a.size());
-  return a.cwiseProduct(b);
-}
-
-/* ************************************************************************* */
-Vector ediv(const Vector &a, const Vector &b) {
-  assert (b.size()==a.size());
-  return a.cwiseQuotient(b);
-}
-
 /* ************************************************************************* */
 Vector ediv_(const Vector &a, const Vector &b) {
   size_t n = a.size();
@@ -210,36 +169,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
   return c;
 }
 
-/* ************************************************************************* */
-double sum(const Vector &a) {
-  return a.sum();
-}
-
-/* ************************************************************************* */
-double norm_2(const Vector& v) {
-  return v.norm();
-}
-
-/* ************************************************************************* */
-Vector reciprocal(const Vector &a) {
-  return a.array().inverse();
-}
-
-/* ************************************************************************* */
-Vector esqrt(const Vector& v) {
-  return v.cwiseSqrt();
-}
-
-/* ************************************************************************* */
-Vector abs(const Vector& v) {
-  return v.cwiseAbs();
-}
-
-/* ************************************************************************* */
-double max(const Vector &a) {
-  return a.maxCoeff();
-}
-
 /* ************************************************************************* */
 // imperative version, pass in x
 double houseInPlace(Vector &v) {
@@ -292,7 +221,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
       // Basically, instead of doing a normal QR step with the weighted
       // pseudoinverse, we enforce the constraint by turning
       // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
-      pseudo = delta(m, i, 1.0 / a[i]);
+      pseudo = Vector::Unit(m,i)*(1.0/a[i]);
       return inf;
     }
   }
@@ -363,6 +292,4 @@ Vector concatVectors(size_t nrVectors, ...)
   return concatVectors(vs);
 }
 
-/* ************************************************************************* */
-
 } // namespace gtsam

gtsam/base/Vector.h

@@ -14,13 +14,12 @@
  * @brief   typedef and functions to augment Eigen's VectorXd
  * @author  Kai Ni
  * @author  Frank Dellaert
+ * @author  Alex Hagiopol
  */
 
 // \callgraph
 
-
 #pragma once
-
 #ifndef MKL_BLAS
 #define MKL_BLAS MKL_DOMAIN_BLAS
 #endif
@@ -64,54 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12);
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 
-/**
- * Create vector initialized to a constant value
- * @param n is the size of the vector
- * @param value is a constant value to insert into the vector
- */
-GTSAM_EXPORT Vector repeat(size_t n, double value);
-
-/**
- * Create basis vector of dimension n,
- * with a constant in spot i
- * @param n is the size of the vector
- * @param i index of the one
- * @param value is the value to insert into the vector
- * @return delta vector
- */
-GTSAM_EXPORT Vector delta(size_t n, size_t i, double value);
-
-/**
- * Create basis vector of dimension n,
- * with one in spot i
- * @param n is the size of the vector
- * @param i index of the one
- * @return basis vector
- */
-inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
-
-/**
- * Create zero vector
- * @param n size
- */
-inline Vector zero(size_t n) { return Vector::Zero(n);}
-
-/**
- * Create vector initialized to ones
- * @param n size
- */
-inline Vector ones(size_t n) { return Vector::Ones(n); }
-
-/**
- * check if all zero
- */
-GTSAM_EXPORT bool zero(const Vector& v);
-
-/**
- * dimensionality == size
- */
-inline size_t dim(const Vector& v) { return v.size(); }
-
 /**
  * print without optional string, must specify cout yourself
  */
@@ -196,39 +147,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector&
  */
 GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
 
-/**
- * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
- * @param v Vector
- * @param i1 first row index
- * @param i2 last  row index + 1
- * @return subvector v(i1:i2)
- */
-GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
-
-/**
- * Inserts a subvector into a vector IN PLACE
- * @param fullVector is the vector to be changed
- * @param subVector is the vector to insert
- * @param i is the index where the subvector should be inserted
- */
-GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
-
-/**
- * elementwise multiplication
- * @param a first vector
- * @param b second vector
- * @return vector [a(i)*b(i)]
- */
-GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
-
-/**
- * elementwise division
- * @param a first vector
- * @param b second vector
- * @return vector [a(i)/b(i)]
- */
-GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
-
 /**
  * elementwise division, but 0/0 = 0, not inf
  * @param a first vector
@@ -237,49 +155,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
  */
 GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
 
-/**
- * sum vector elements
- * @param a vector
- * @return sum_i a(i)
- */
-GTSAM_EXPORT double sum(const Vector &a);
-
-/**
- * Calculates L2 norm for a vector
- * modeled after boost.ublas for compatibility
- * @param v vector
- * @return the L2 norm
- */
-GTSAM_EXPORT double norm_2(const Vector& v);
-
-/**
- * Elementwise reciprocal of vector elements
- * @param a vector
- * @return [1/a(i)]
- */
-GTSAM_EXPORT Vector reciprocal(const Vector &a);
-
-/**
- * Elementwise sqrt of vector elements
- * @param v is a vector
- * @return [sqrt(a(i))]
- */
-GTSAM_EXPORT Vector esqrt(const Vector& v);
-
-/**
- * Absolute values of vector elements
- * @param v is a vector
- * @return [abs(a(i))]
- */
-GTSAM_EXPORT Vector abs(const Vector& v);
-
-/**
- * Return the max element of a vector
- * @param a is a vector
- * @return max(a)
- */
-GTSAM_EXPORT double max(const Vector &a);
-
 /**
  * Dot product
  */
@@ -356,6 +231,25 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
  */
 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+inline Vector abs(const Vector& v){return v.cwiseAbs();}
+inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
+inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
+inline size_t dim(const Vector& v) { return v.size(); }
+inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
+inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
+inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
+inline double max(const Vector &a){return a.maxCoeff();}
+inline double norm_2(const Vector& v) {return v.norm();}
+inline Vector ones(size_t n) { return Vector::Ones(n); }
+inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
+inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
+inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
+inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
+inline double sum(const Vector &a){return a.sum();}
+inline bool zero(const Vector& v){ return v.isZero(); }
+inline Vector zero(size_t n) { return Vector::Zero(n); }
+#endif
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>

gtsam/base/numericalDerivative.h

@@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
   TangentX d;
   d.setZero();
 
-  Vector g = zero(N); // Can be fixed size
+  Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
   for (int j = 0; j < N; j++) {
     d(j) = delta;
     double hxplus = h(traits<X>::Retract(x, d));
@@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
   dx.setZero();
 
   // Fill in Jacobian H
-  Matrix H = zeros(m, N);
+  Matrix H = Matrix::Zero(m, N);
   const double factor = 1.0 / (2.0 * delta);
   for (int j = 0; j < N; j++) {
     dx(j) = delta;

gtsam/base/tests/testMatrix.cpp

@@ -156,8 +156,8 @@ TEST(Matrix, collect2 )
 TEST(Matrix, collect3 )
 {
   Matrix A, B;
-  A = eye(2, 3);
+  A = Matrix::Identity(2,3);
-  B = eye(2, 3);
+  B = Matrix::Identity(2,3);
   vector<const Matrix*> matrices;
   matrices.push_back(&A);
   matrices.push_back(&B);
@@ -211,48 +211,6 @@ TEST(Matrix, column )
   EXPECT(assert_equal(a3, exp3));
 }
 
-/* ************************************************************************* */
-TEST(Matrix, insert_column )
-{
-  Matrix big = zeros(5, 6);
-  Vector col = ones(5);
-  size_t j = 3;
-
-  insertColumn(big, col, j);
-
-  Matrix expected = (Matrix(5, 6) <<
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished();
-
-  EXPECT(assert_equal(expected, big));
-}
-
-/* ************************************************************************* */
-TEST(Matrix, insert_subcolumn )
-{
-  Matrix big = zeros(5, 6);
-  Vector col1 = ones(2);
-  size_t i = 1;
-  size_t j = 3;
-
-  insertColumn(big, col1, i, j); // check 1
-
-  Vector col2 = ones(1);
-  insertColumn(big, col2, 4, 5); // check 2
-
-  Matrix expected = (Matrix(5, 6) <<
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished();
-
-  EXPECT(assert_equal(expected, big));
-}
-
 /* ************************************************************************* */
 TEST(Matrix, row )
 {
@@ -272,26 +230,10 @@ TEST(Matrix, row )
   EXPECT(assert_equal(a3, exp3));
 }
 
-/* ************************************************************************* */
-TEST(Matrix, zeros )
-{
-  Matrix A(2, 3);
-  A(0, 0) = 0;
-  A(0, 1) = 0;
-  A(0, 2) = 0;
-  A(1, 0) = 0;
-  A(1, 1) = 0;
-  A(1, 2) = 0;
-
-  Matrix zero = zeros(2, 3);
-
-  EQUALITY(A , zero);
-}
-
 /* ************************************************************************* */
 TEST(Matrix, insert_sub )
 {
-  Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
+  Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
       1.0).finished();
 
   insertSub(big, small, 1, 2);
@@ -307,9 +249,9 @@ TEST(Matrix, insert_sub )
 TEST(Matrix, diagMatrices )
 {
   std::vector<Matrix> Hs;
-  Hs.push_back(ones(3,3));
+  Hs.push_back(Matrix::Ones(3,3));
-  Hs.push_back(ones(4,4)*2);
+  Hs.push_back(Matrix::Ones(4,4)*2);
-  Hs.push_back(ones(2,2)*3);
+  Hs.push_back(Matrix::Ones(2,2)*3);
 
   Matrix actual = diag(Hs);
 
@@ -723,9 +665,9 @@ TEST(Matrix, inverse )
   A(2, 1) = 0;
   A(2, 2) = 6;
 
-  Matrix Ainv = inverse(A);
+  Matrix Ainv = A.inverse();
-  EXPECT(assert_equal(eye(3), A*Ainv));
+  EXPECT(assert_equal((Matrix) I_3x3, A*Ainv));
-  EXPECT(assert_equal(eye(3), Ainv*A));
+  EXPECT(assert_equal((Matrix) I_3x3, Ainv*A));
 
   Matrix expected(3, 3);
   expected(0, 0) = 1.0909;
@@ -746,13 +688,13 @@ TEST(Matrix, inverse )
       0.0, -1.0, 1.0,
       1.0, 0.0, 2.0,
       0.0, 0.0, 1.0).finished(),
-      inverse(lMg)));
+      lMg.inverse()));
   Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
   EXPECT(assert_equal((Matrix(3, 3) <<
       0.0, 1.0,-2.0,
       -1.0, 0.0, 1.0,
       0.0, 0.0, 1.0).finished(),
-      inverse(gMl)));
+      gMl.inverse()));
 }
 
 /* ************************************************************************* */
@@ -769,7 +711,7 @@ TEST(Matrix, inverse2 )
   A(2, 1) = 0;
   A(2, 2) = 1;
 
-  Matrix Ainv = inverse(A);
+  Matrix Ainv = A.inverse();
 
   Matrix expected(3, 3);
   expected(0, 0) = 0;
@@ -996,7 +938,7 @@ TEST(Matrix, inverse_square_root )
       10.0).finished();
 
   EQUALITY(expected,actual);
-  EQUALITY(measurement_covariance,inverse(actual*actual));
+  EQUALITY(measurement_covariance,(actual*actual).inverse());
 
   // Randomly generated test.  This test really requires inverse to
   // be working well; if it's not, there's the possibility of a
@@ -1052,28 +994,6 @@ TEST(Matrix, cholesky_inverse )
   EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
 }
 
-/* ************************************************************************* */
-TEST(Matrix, multiplyAdd )
-{
-  Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
-  Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
-      expected = e + A * x;
-
-  multiplyAdd(1, A, x, e);
-  EXPECT(assert_equal(expected, e));
-}
-
-/* ************************************************************************* */
-TEST(Matrix, transposeMultiplyAdd )
-{
-  Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
-  Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
-      expected = x + trans(A) * e;
-
-  transposeMultiplyAdd(1, A, e, x);
-  EXPECT(assert_equal(expected, x));
-}
-
 /* ************************************************************************* */
 TEST(Matrix, linear_dependent )
 {
@@ -1102,12 +1022,12 @@ TEST(Matrix, linear_dependent3 )
 TEST(Matrix, svd1 )
 {
   Vector v = Vector3(2., 1., 0.);
-  Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
+  Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1)
       * Matrix(trans(V1));
   Matrix U, V;
   Vector s;
   svd(A, U, s, V);
-  Matrix S = diag(s);
+  Matrix S = s.asDiagonal();
   EXPECT(assert_equal(U*S*Matrix(trans(V)),A));
   EXPECT(assert_equal(S,S1));
 }
@@ -1158,7 +1078,7 @@ TEST(Matrix, svd3 )
     V = -V;
   }
 
-  Matrix S = diag(s);
+  Matrix S = s.asDiagonal();
   Matrix t = U * S;
   Matrix Vt = trans(V);
 
@@ -1202,7 +1122,7 @@ TEST(Matrix, svd4 )
     V.col(1) = -V.col(1);
   }
 
-  Matrix reconstructed = U * diag(s) * trans(V);
+  Matrix reconstructed = U * s.asDiagonal() * trans(V);
 
   EXPECT(assert_equal(A, reconstructed, 1e-4));
   EXPECT(assert_equal(expectedU,U, 1e-3));

gtsam/base/tests/testSymmetricBlockMatrix.cpp

@@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
     23, 29).finished();
   Matrix actual1 = testBlockMatrix(1, 1);
   // Test only writing the upper triangle for efficiency
-  Matrix actual1t = Matrix::Zero(2, 2);
+  Matrix actual1t = Z_2x2;
   actual1t.triangularView<Eigen::Upper>() = testBlockMatrix(1, 1).triangularView();
   EXPECT(assert_equal(expected1, actual1));
   EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));

gtsam/base/tests/testVector.cpp

@@ -79,22 +79,6 @@ TEST(Vector, copy )
   EXPECT(assert_equal(a, b));
 }
 
-/* ************************************************************************* */
-TEST(Vector, zero1 )
-{
-  Vector v = Vector::Zero(2);
-  EXPECT(zero(v));
-}
-
-/* ************************************************************************* */
-TEST(Vector, zero2 )
-{
-  Vector a = zero(2);
-  Vector b = Vector::Zero(2);
-  EXPECT(a==b);
-  EXPECT(assert_equal(a, b));
-}
-
 /* ************************************************************************* */
 TEST(Vector, scalar_multiply )
 {
@@ -119,36 +103,6 @@ TEST(Vector, negate )
   EXPECT(assert_equal(b, -a));
 }
 
-/* ************************************************************************* */
-TEST(Vector, sub )
-{
-  Vector a(6);
-  a(0) = 10; a(1) = 20; a(2) = 3;
-  a(3) = 34; a(4) = 11; a(5) = 2;
-
-  Vector result(sub(a,2,5));
-
-  Vector b(3);
-  b(0) = 3; b(1) = 34; b(2) =11;
-
-  EXPECT(b==result);
-  EXPECT(assert_equal(b, result));
-}
-
-/* ************************************************************************* */
-TEST(Vector, subInsert )
-{
-  Vector big = zero(6),
-       small = ones(3);
-
-  size_t i = 2;
-  subInsert(big, small, i);
-
-  Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
-
-  EXPECT(assert_equal(expected, big));
-}
-
 /* ************************************************************************* */
 TEST(Vector, householder )
 {
@@ -198,7 +152,7 @@ TEST(Vector, weightedPseudoinverse )
   // create sigmas
   Vector sigmas(2);
   sigmas(0) = 0.1; sigmas(1) = 0.2;
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
 
   // perform solve
   Vector actual; double precision;
@@ -224,8 +178,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
   // create sigmas
   Vector sigmas(2);
   sigmas(0) = 0.0; sigmas(1) = 0.2;
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
-
   // perform solve
   Vector actual; double precision;
   boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
@@ -244,7 +197,7 @@ TEST(Vector, weightedPseudoinverse_nan )
 {
   Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
   Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
   Vector pseudo; double precision;
   boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
 
@@ -253,17 +206,6 @@ TEST(Vector, weightedPseudoinverse_nan )
   DOUBLES_EQUAL(100, precision, 1e-5);
 }
 
-/* ************************************************************************* */
-TEST(Vector, ediv )
-{
-  Vector a = Vector3(10., 20., 30.);
-  Vector b = Vector3(2.0, 5.0, 6.0);
-  Vector actual(ediv(a,b));
-
-  Vector c = Vector3(5.0, 4.0, 5.0);
-  EXPECT(assert_equal(c,actual));
-}
-
 /* ************************************************************************* */
 TEST(Vector, dot )
 {
@@ -298,18 +240,11 @@ TEST(Vector, equals )
 TEST(Vector, greater_than )
 {
   Vector v1 = Vector3(1.0, 2.0, 3.0),
-       v2 = zero(3);
+       v2 = Z_3x1;
   EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
   EXPECT(greaterThanOrEqual(v1, v2)); // test equals
 }
 
-/* ************************************************************************* */
-TEST(Vector, reciprocal )
-{
-  Vector v = Vector3(1.0, 2.0, 4.0);
-  EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
-}
-
 /* ************************************************************************* */
 TEST(Vector, linear_dependent )
 {

gtsam/discrete/tests/testDiscreteMarginals.cpp

@@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) {
   // Calculate the marginals by brute force
   vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
       key[0] & key[1] & key[2] & key[3] & key[4]);
-  Vector T = zero(5), F = zero(5);
+  Vector T = Z_5x1, F = Z_5x1;
   for (size_t i = 0; i < allPosbValues.size(); ++i) {
     DiscreteFactor::Values x = allPosbValues[i];
     double px = graph(x);

gtsam/geometry/OrientedPlane3.cpp

@@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
   double pred_d = n_.unitVector().dot(xr.translation()) + d_;
 
   if (Hr) {
-    *Hr = zeros(3, 6);
+    *Hr = Matrix::Zero(3,6);
     Hr->block<2, 3>(0, 0) = D_rotated_plane;
     Hr->block<1, 3>(2, 3) = unit_vec;
   }

gtsam/geometry/Pose2.cpp

@@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const {
 /* ************************************************************************* */
 Matrix3 Pose2::adjointMap(const Vector3& v) {
   // See Chirikjian12book2, vol.2, pg. 36
-  Matrix3 ad = zeros(3,3);
+  Matrix3 ad = Z_3x3;
   ad(0,1) = -v[2];
   ad(1,0) = v[2];
   ad(0,2) = v[1];

gtsam/geometry/Rot2.cpp

@@ -63,7 +63,7 @@ Rot2& Rot2::normalize() {
 Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
   if (H)
     *H = I_1x1;
-  if (zero(v))
+  if (v.isZero())
     return (Rot2());
   else
     return Rot2::fromAngle(v(0));

gtsam/geometry/Rot2.h

@@ -119,12 +119,12 @@ namespace gtsam {
 
     /// Left-trivialized derivative of the exponential map
     static Matrix ExpmapDerivative(const Vector& /*v*/) {
-      return ones(1);
+      return I_1x1;
     }
 
     /// Left-trivialized derivative inverse of the exponential map
     static Matrix LogmapDerivative(const Vector& /*v*/) {
-      return ones(1);
+      return I_1x1;
     }
 
     // Chart at origin simply uses exponential map and its inverse

gtsam/geometry/tests/testCal3_S2.cpp

@@ -122,8 +122,8 @@ TEST(Cal3_S2, between) {
   Matrix H1, H2;
 
   EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
-  EXPECT(assert_equal(-eye(5), H1));
+  EXPECT(assert_equal(-I_5x5, H1));
-  EXPECT(assert_equal(eye(5), H2));
+  EXPECT(assert_equal(I_5x5, H2));
 
 }
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -62,7 +62,7 @@ TEST (EssentialMatrix, FromPose3) {
 //*******************************************************************************
 TEST(EssentialMatrix, localCoordinates0) {
   EssentialMatrix E;
-  Vector expected = zero(5);
+  Vector expected = Z_5x1;
   Vector actual = E.localCoordinates(E);
   EXPECT(assert_equal(expected, actual, 1e-8));
 }
@@ -74,7 +74,7 @@ TEST (EssentialMatrix, localCoordinates) {
   Pose3 pose(trueRotation, trueTranslation);
   EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
   Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
-  EXPECT(assert_equal(zero(5), actual, 1e-8));
+  EXPECT(assert_equal(Z_5x1, actual, 1e-8));
 
   Vector6 d;
   d << 0.1, 0.2, 0.3, 0, 0, 0;
@@ -85,7 +85,7 @@ TEST (EssentialMatrix, localCoordinates) {
 
 //*************************************************************************
 TEST (EssentialMatrix, retract0) {
-  EssentialMatrix actual = trueE.retract(zero(5));
+  EssentialMatrix actual = trueE.retract(Z_5x1);
   EXPECT(assert_equal(trueE, actual));
 }
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -96,8 +96,8 @@ inline static Vector randomVector(const Vector& minLimits,
     const Vector& maxLimits) {
 
   // Get the number of dimensions and create the return vector
-  size_t numDims = dim(minLimits);
+  size_t numDims = minLimits.size();
-  Vector vector = zero(numDims);
+  Vector vector = Vector::Zero(numDims);
 
   // Create the random vector
   for (size_t i = 0; i < numDims; i++) {
@@ -145,7 +145,7 @@ TEST (OrientedPlane3, error2) {
   OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
 
   // Hard-coded regression values, to ensure the result doesn't change.
-  EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8));
+  EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8));
   EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
 
   // Test the jacobians of transform

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat)
 
   Matrix R = camera2.pose().rotation().matrix();
   Matrix I = trans(R)*R;
-  EXPECT(assert_equal(I, eye(3)));
+  EXPECT(assert_equal(I, I_3x3));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholePose.cpp

@@ -87,7 +87,7 @@ TEST( PinholePose, lookat)
 
   Matrix R = camera2.pose().rotation().matrix();
   Matrix I = trans(R)*R;
-  EXPECT(assert_equal(I, eye(3)));
+  EXPECT(assert_equal(I, I_3x3));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPoint2.cpp

@@ -73,12 +73,12 @@ TEST(Point2, Lie) {
   Matrix H1, H2;
 
   EXPECT(assert_equal(Point2(5,7), traits<Point2>::Compose(p1, p2, H1, H2)));
-  EXPECT(assert_equal(eye(2), H1));
+  EXPECT(assert_equal(I_2x2, H1));
-  EXPECT(assert_equal(eye(2), H2));
+  EXPECT(assert_equal(I_2x2, H2));
 
   EXPECT(assert_equal(Point2(3,3), traits<Point2>::Between(p1, p2, H1, H2)));
-  EXPECT(assert_equal(-eye(2), H1));
+  EXPECT(assert_equal(-I_2x2, H1));
-  EXPECT(assert_equal(eye(2), H2));
+  EXPECT(assert_equal(I_2x2, H2));
 
   EXPECT(assert_equal(Point2(5,7), traits<Point2>::Retract(p1, Vector2(4., 5.))));
   EXPECT(assert_equal(Vector2(3.,3.), traits<Point2>::Local(p1,p2)));

gtsam/geometry/tests/testPoint3.cpp

@@ -47,12 +47,12 @@ TEST(Point3, Lie) {
   Matrix H1, H2;
 
   EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
-  EXPECT(assert_equal(eye(3), H1));
+  EXPECT(assert_equal(I_3x3, H1));
-  EXPECT(assert_equal(eye(3), H2));
+  EXPECT(assert_equal(I_3x3, H2));
 
   EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
-  EXPECT(assert_equal(-eye(3), H1));
+  EXPECT(assert_equal(-I_3x3, H1));
-  EXPECT(assert_equal(eye(3), H2));
+  EXPECT(assert_equal(I_3x3, H2));
 
   EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
   EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));

gtsam/geometry/tests/testPose2.cpp

@@ -102,7 +102,7 @@ TEST(Pose2, expmap3) {
       0.99,  0.0, -0.015,
       0.0,   0.0,  0.0).finished();
   Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
-  Matrix expected = eye(3) + A + A2 + A3 + A4;
+  Matrix expected = I_3x3 + A + A2 + A3 + A4;
 
   Vector v = Vector3(0.01, -0.015, 0.99);
   Pose2 pose = Pose2::Expmap(v);
@@ -311,7 +311,7 @@ TEST(Pose2, compose_a)
        -1.0, 0.0, 2.0,
       0.0, 0.0, 1.0
   ).finished();
-  Matrix expectedH2 = eye(3);
+  Matrix expectedH2 = I_3x3;
   Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
   Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
   EXPECT(assert_equal(expectedH1,actualDcompose1));

gtsam/geometry/tests/testPose3.cpp

@@ -61,7 +61,7 @@ TEST( Pose3, constructors)
 TEST( Pose3, retract_first_order)
 {
   Pose3 id;
-  Vector v = zero(6);
+  Vector v = Z_6x1;
   v(0) = 0.3;
   EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
   v(3)=0.2;v(4)=0.7;v(5)=-2;
@@ -71,7 +71,7 @@ TEST( Pose3, retract_first_order)
 /* ************************************************************************* */
 TEST( Pose3, retract_expmap)
 {
-  Vector v = zero(6); v(0) = 0.3;
+  Vector v = Z_6x1; v(0) = 0.3;
   Pose3 pose = Pose3::Expmap(v);
   EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
   EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
@@ -81,7 +81,7 @@ TEST( Pose3, retract_expmap)
 TEST( Pose3, expmap_a_full)
 {
   Pose3 id;
-  Vector v = zero(6);
+  Vector v = Z_6x1;
   v(0) = 0.3;
   EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
   v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@@ -92,7 +92,7 @@ TEST( Pose3, expmap_a_full)
 TEST( Pose3, expmap_a_full2)
 {
   Pose3 id;
-  Vector v = zero(6);
+  Vector v = Z_6x1;
   v(0) = 0.3;
   EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
   v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@@ -148,12 +148,12 @@ TEST(Pose3, Adjoint_full)
 Pose3 Agrawal06iros(const Vector& xi) {
   Vector w = xi.head(3);
   Vector v = xi.tail(3);
-  double t = norm_2(w);
+  double t = w.norm();
   if (t < 1e-5)
     return Pose3(Rot3(), Point3(v));
   else {
     Matrix W = skewSymmetric(w/t);
-    Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
+    Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
     return Pose3(Rot3::Expmap (w), Point3(A * v));
   }
 }
@@ -267,7 +267,7 @@ TEST( Pose3, inverse)
 {
   Matrix actualDinverse;
   Matrix actual = T.inverse(actualDinverse).matrix();
-  Matrix expected = inverse(T.matrix());
+  Matrix expected = T.matrix().inverse();
   EXPECT(assert_equal(actual,expected,1e-8));
 
   Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
@@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2)
 TEST( Pose3, compose_inverse)
 {
   Matrix actual = (T*T.inverse()).matrix();
-  Matrix expected = eye(4,4);
+  Matrix expected = I_4x4;
   EXPECT(assert_equal(actual,expected,1e-8));
 }
 
@@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates)
 /* ************************************************************************* */
 TEST(Pose3, expmap_logmap)
 {
-  Vector d12 = repeat(6,0.1);
+  Vector d12 = Vector6::Constant(0.1);
   Pose3 t1 = T, t2 = t1.expmap(d12);
   EXPECT(assert_equal(d12, t1.logmap(t2)));
 }
@@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) {
 TEST( Pose3, unicycle )
 {
   // velocity in X should be X in inertial frame, rather than global frame
-  Vector x_step = delta(6,3,1.0);
+  Vector x_step = Vector::Unit(6,3)*1.0;
   EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
   EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
   EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
@@ -723,9 +723,8 @@ TEST( Pose3, adjointMap) {
   Matrix res = Pose3::adjointMap(screwPose3::xi);
   Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
   Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
-  Matrix Z3 = zeros(3,3);
   Matrix6 expected;
-  expected << wh, Z3, vh, wh;
+  expected << wh, Z_3x3, vh, wh;
   EXPECT(assert_equal(expected,res,1e-5));
 }
 

gtsam/geometry/tests/testRot2.cpp

@@ -62,8 +62,8 @@ TEST( Rot2, compose)
 
   Matrix H1, H2;
   (void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2);
-  EXPECT(assert_equal(eye(1), H1));
+  EXPECT(assert_equal(I_1x1, H1));
-  EXPECT(assert_equal(eye(1), H2));
+  EXPECT(assert_equal(I_1x1, H2));
 }
 
 /* ************************************************************************* */
@@ -74,8 +74,8 @@ TEST( Rot2, between)
 
   Matrix H1, H2;
   (void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2);
-  EXPECT(assert_equal(-eye(1), H1));
+  EXPECT(assert_equal(-I_1x1, H1));
-  EXPECT(assert_equal(eye(1), H2));
+  EXPECT(assert_equal(I_1x1, H2));
 }
 
 /* ************************************************************************* */
@@ -89,7 +89,7 @@ TEST( Rot2, equals)
 /* ************************************************************************* */
 TEST( Rot2, expmap)
 {
-  Vector v = zero(1);
+  Vector v = Z_1x1;
   CHECK(assert_equal(R.retract(v), R));
 }
 

gtsam/geometry/tests/testRot3.cpp

@@ -96,7 +96,7 @@ TEST( Rot3, equals)
 /* ************************************************************************* */
 // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
 Rot3 slow_but_correct_Rodrigues(const Vector& w) {
-  double t = norm_2(w);
+  double t = w.norm();
   Matrix3 J = skewSymmetric(w / t);
   if (t < 1e-5) return Rot3();
   Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
@@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2)
 TEST( Rot3, Rodrigues3)
 {
   Vector w = Vector3(0.1, 0.2, 0.3);
-  Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w));
+  Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm());
   Rot3 R2 = slow_but_correct_Rodrigues(w);
   CHECK(assert_equal(R2,R1));
 }
@@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4)
 /* ************************************************************************* */
 TEST( Rot3, retract)
 {
-  Vector v = zero(3);
+  Vector v = Z_3x1;
   CHECK(assert_equal(R, R.retract(v)));
 
 //  // test Canonical coordinates
@@ -213,7 +213,7 @@ TEST(Rot3, log)
 #define CHECK_OMEGA_ZERO(X,Y,Z) \
   w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
   R = Rot3::Rodrigues(w); \
-  EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
+  EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
 
   CHECK_OMEGA_ZERO( 2.0*PI,      0,      0)
   CHECK_OMEGA_ZERO(      0, 2.0*PI,      0)
@@ -224,16 +224,16 @@ TEST(Rot3, log)
 /* ************************************************************************* */
 TEST(Rot3, retract_localCoordinates)
 {
-  Vector3 d12 = repeat(3,0.1);
+  Vector3 d12 = Vector3::Constant(0.1);
   Rot3 R2 = R.retract(d12);
   EXPECT(assert_equal(d12, R.localCoordinates(R2)));
 }
 /* ************************************************************************* */
 TEST(Rot3, expmap_logmap)
 {
-  Vector3 d12 = repeat(3,0.1);
+  Vector3 d12 = Vector3::Constant(0.1);
   Rot3 R2 = R.expmap(d12);
-  EXPECT(assert_equal(d12, R.logmap(R2)));
+  EXPECT(assert_equal(d12, (Vector) R.logmap(R2)));
 }
 
 /* ************************************************************************* */
@@ -555,8 +555,8 @@ TEST(Rot3, quaternion) {
 /* ************************************************************************* */
 Matrix Cayley(const Matrix& A) {
   Matrix::Index n = A.cols();
-  const Matrix I = eye(n);
+  const Matrix I = Matrix::Identity(n,n);
-  return (I-A)*inverse(I+A);
+  return (I-A)*(I+A).inverse();
 }
 
 TEST( Rot3, Cayley ) {

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat)
 
   Matrix R = camera2.pose().rotation().matrix();
   Matrix I = trans(R)*R;
-  CHECK(assert_equal(I, eye(3)));
+  CHECK(assert_equal(I, I_3x3));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testUnit3.cpp

@@ -237,7 +237,7 @@ TEST(Unit3, distance) {
 TEST(Unit3, localCoordinates0) {
   Unit3 p;
   Vector actual = p.localCoordinates(p);
-  EXPECT(assert_equal(zero(2), actual, 1e-8));
+  EXPECT(assert_equal(Z_2x1, actual, 1e-8));
 }
 
 TEST(Unit3, localCoordinates) {
@@ -245,14 +245,14 @@ TEST(Unit3, localCoordinates) {
     Unit3 p, q;
     Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
     EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {
     Unit3 p, q(1, 6.12385e-21, 0);
     Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
     EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {

gtsam/geometry/triangulation.cpp

@@ -31,7 +31,7 @@ Vector4 triangulateHomogeneousDLT(
   size_t m = projection_matrices.size();
 
   // Allocate DLT matrix
-  Matrix A = zeros(m * 2, 4);
+  Matrix A = Matrix::Zero(m * 2, 4);
 
   for (size_t i = 0; i < m; i++) {
     size_t row = i * 2;

gtsam/geometry/triangulation.h

@@ -363,6 +363,18 @@ struct TriangulationParameters {
         << p.dynamicOutlierRejectionThreshold << std::endl;
     return os;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(rankTolerance);
+    ar & BOOST_SERIALIZATION_NVP(enableEPI);
+    ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
+    ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
+  }
 };
 
 /**
@@ -411,6 +423,15 @@ public:
       os << "no point, status = " << result.status_ << std::endl;
     return os;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(status_);
+  }
 };
 
 /// triangulateSafe: extensive checking of the outcome

gtsam/linear/GaussianConditional.cpp

@@ -183,7 +183,7 @@ namespace gtsam {
     if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
 
     for (const_iterator it = beginParents(); it!= endParents(); it++)
-      gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
+      gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
 
     // Scale by sigmas
     if(model_)

gtsam/linear/HessianFactor.cpp

@@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
   vector<Vector> y;
   y.reserve(size());
   for (const_iterator it = begin(); it != end(); it++)
-    y.push_back(zero(getDim(it)));
+    y.push_back(Vector::Zero(getDim(it)));
 
   // Accessing the VectorValues one by one is expensive
   // So we will loop over columns to access x only once per column
@@ -427,7 +427,7 @@ void HessianFactor::gradientAtZero(double* d) const {
 Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
   Factor::const_iterator i = find(key);
   // Sum over G_ij*xj for all xj connecting to xi
-  Vector b = zero(x.at(key).size());
+  Vector b = Vector::Zero(x.at(key).size());
   for (Factor::const_iterator j = begin(); j != end(); ++j) {
     // Obtain Gij from the Hessian factor
     // Hessian factor only stores an upper triangular matrix, so be careful when i>j

gtsam/linear/JacobianFactor.cpp

@@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
 
 /* ************************************************************************* */
 Vector JacobianFactor::operator*(const VectorValues& x) const {
-  Vector Ax = zero(Ab_.rows());
+  Vector Ax = Vector::Zero(Ab_.rows());
   if (empty())
     return Ax;
 
@@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
     pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
     if (xi.second)
       xi.first->second = Vector::Zero(getDim(begin() + pos));
-    gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second);
+    xi.first->second += Ab_(pos).transpose()*E;
-
   }
 }
 
@@ -595,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y
 
   if (empty())
     return;
-  Vector Ax = zero(Ab_.rows());
+  Vector Ax = Vector::Zero(Ab_.rows());
 
   /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
   /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'

gtsam/linear/KalmanFilter.cpp

@@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
   // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
   // TODO: starts to seem more elaborate than straight-up KF equations?
-  Matrix M = inverse(Q), Ft = trans(F);
+  Matrix M = Q.inverse(), Ft = trans(F);
   Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
   Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
   double f = dot(b, g2);
@@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
     const Vector& z, const Matrix& Q) const {
   Key k = step(p);
-  Matrix M = inverse(Q), Ht = trans(H);
+  Matrix M = Q.inverse(), Ht = trans(H);
   Matrix G = Ht * M * H;
   Vector g = Ht * M * z;
   double f = dot(z, M * z);

gtsam/linear/KalmanFilter.h

@@ -69,7 +69,7 @@ public:
   // Constructor
   KalmanFilter(size_t n, Factorization method =
       KALMANFILTER_DEFAULT_FACTORIZATION) :
-      n_(n), I_(eye(n_, n_)), function_(
+      n_(n), I_(Matrix::Identity(n_, n_)), function_(
           method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
               GaussianFactorGraph::Eliminate(EliminateCholesky)) {
   }

gtsam/linear/NoiseModel.cpp

@@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
       if (variances(j) != variances(0)) goto full;
     return Isotropic::Variance(n, variances(0), true);
   }
-  full: return shared_ptr(new Diagonal(esqrt(variances)));
+  full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
 }
 
 /* ************************************************************************* */
@@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
 
 /* ************************************************************************* */
 Constrained::Constrained(const Vector& sigmas)
-  : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
+  : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
   internal::fix(sigmas, precisions_, invsigmas_);
 }
 
@@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
 
 /* ************************************************************************* */
 Constrained::shared_ptr Constrained::unit() const {
-  Vector sigmas = ones(dim());
+  Vector sigmas = Vector::Ones(dim());
   for (size_t i=0; i<dim(); ++i)
     if (constrained(i))
       sigmas(i) = 0.0;

gtsam/linear/NoiseModel.h

@@ -309,7 +309,7 @@ namespace gtsam {
        * i.e. the diagonal of the information matrix, i.e., weights
        */
       static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
-        return Variances(reciprocal(precisions), smart);
+        return Variances(precisions.array().inverse(), smart);
       }
 
       virtual void print(const std::string& name) const;
@@ -341,7 +341,7 @@ namespace gtsam {
        * Return R itself, but note that Whiten(H) is cheaper than R*H
        */
       virtual Matrix R() const {
-        return diag(invsigmas());
+        return invsigmas().asDiagonal();
       }
 
     private:
@@ -381,7 +381,7 @@ namespace gtsam {
        * from appearing in invsigmas or precisions.
        * mu set to large default value (1000.0)
        */
-      Constrained(const Vector& sigmas = zero(1));
+      Constrained(const Vector& sigmas = Z_1x1);
 
       /**
        * Constructor that prevents any inf values
@@ -416,7 +416,7 @@ namespace gtsam {
        * standard devations, some of which might be zero
        */
       static shared_ptr MixedSigmas(const Vector& sigmas) {
-        return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas);
+        return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
       }
 
       /**
@@ -424,7 +424,7 @@ namespace gtsam {
        * standard devations, some of which might be zero
        */
       static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
-        return MixedSigmas(repeat(sigmas.size(), m), sigmas);
+        return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
       }
 
       /**
@@ -432,10 +432,10 @@ namespace gtsam {
        * standard devations, some of which might be zero
        */
       static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
-        return shared_ptr(new Constrained(mu, esqrt(variances)));
+        return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
       }
       static shared_ptr MixedVariances(const Vector& variances) {
-        return shared_ptr(new Constrained(esqrt(variances)));
+        return shared_ptr(new Constrained(variances.cwiseSqrt()));
       }
 
       /**
@@ -443,10 +443,10 @@ namespace gtsam {
        * precisions, some of which might be inf
        */
       static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
-        return MixedVariances(mu, reciprocal(precisions));
+        return MixedVariances(mu, precisions.array().inverse());
       }
       static shared_ptr MixedPrecisions(const Vector& precisions) {
-        return MixedVariances(reciprocal(precisions));
+        return MixedVariances(precisions.array().inverse());
       }
 
       /**
@@ -458,17 +458,17 @@ namespace gtsam {
 
       /** Fully constrained variations */
       static shared_ptr All(size_t dim) {
-        return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0)));
+        return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
       }
 
       /** Fully constrained variations */
       static shared_ptr All(size_t dim, const Vector& mu) {
-        return shared_ptr(new Constrained(mu, repeat(dim,0)));
+        return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
       }
 
       /** Fully constrained variations with a mu parameter */
       static shared_ptr All(size_t dim, double mu) {
-        return shared_ptr(new Constrained(repeat(dim, mu), repeat(dim,0)));
+        return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
       }
 
       virtual void print(const std::string& name) const;
@@ -522,10 +522,10 @@ namespace gtsam {
 
       /** protected constructor takes sigma */
       Isotropic(size_t dim, double sigma) :
-        Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
+        Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
 
       /* dummy constructor to allow for serialization */
-      Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {}
+      Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
 
     public:
 

gtsam/linear/RegularJacobianFactor.h

@@ -83,7 +83,7 @@ public:
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
     if (empty())
       return;
-    Vector Ax = zero(Ab_.rows());
+    Vector Ax = Vector::Zero(Ab_.rows());
 
     // Just iterate over all A matrices and multiply in correct config part
     for (size_t pos = 0; pos < size(); ++pos)
@@ -173,7 +173,7 @@ public:
    * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
    */
   Vector operator*(const double* x) const {
-    Vector Ax = zero(Ab_.rows());
+    Vector Ax = Vector::Zero(Ab_.rows());
     if (empty())
       return Ax;
 

gtsam/linear/VectorValues.cpp

@@ -52,7 +52,7 @@ namespace gtsam {
       Key key;
       size_t n;
       boost::tie(key, n) = v;
-      values_.insert(make_pair(key, sub(x, j, j + n)));
+      values_.insert(make_pair(key, x.segment(j,n)));
       j += n;
     }
   }

gtsam/linear/iterative.h

@@ -86,7 +86,7 @@ namespace gtsam {
 
     /** x += alpha* A'*e */
     void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
-      gtsam::transposeMultiplyAdd(alpha, A(), e, x);
+      x += alpha * A().transpose() * e;
     }
   };
 

gtsam/linear/tests/testGaussianBayesTree.cpp

@@ -170,7 +170,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
   LONGS_EQUAL(1, (long)actualJacobianQR.size());
   LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]);
   Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin());
-  Matrix actualCov = inverse(actualA.transpose() * actualA);
+  Matrix actualCov = (actualA.transpose() * actualA).inverse();
   EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
 
   // Marginal on 6
@@ -187,7 +187,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
   LONGS_EQUAL(1, (long)actualJacobianQR.size());
   LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]);
   actualA = actualJacobianQR.getA(actualJacobianQR.begin());
-  actualCov = inverse(actualA.transpose() * actualA);
+  actualCov = (actualA.transpose() * actualA).inverse();
   EXPECT(assert_equal(expectedCov, actualCov, 1e1));
 }
 

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -47,10 +47,10 @@ TEST(GaussianFactorGraph, initialization) {
   SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
   fg +=
-    JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
+    JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2),
-    JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2),
+    JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
-    JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2),
+    JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
-    JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2);
+    JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
 
   EXPECT_LONGS_EQUAL(4, (long)fg.size());
 
@@ -166,13 +166,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
   GaussianFactorGraph fg;
   SharedDiagonal unit2 = noiseModel::Unit::Create(2);
   // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
-  fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
+  fg += JacobianFactor(2, 10*I_2x2, -1.0*Vector::Ones(2), unit2);
   // odometry between x1 and x2: x2-x1=[0.2;-0.1]
-  fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2);
+  fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2);
   // measurement between x1 and l1: l1-x1=[0.0;0.2]
-  fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(0.0, 1.0), unit2);
+  fg += JacobianFactor(1, 5*I_2x2, 2, -5*I_2x2, Vector2(0.0, 1.0), unit2);
   // measurement between x2 and l1: l1-x2=[-0.2;0.3]
-  fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2);
+  fg += JacobianFactor(0, -5*I_2x2, 1, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
   return fg;
 }
 
@@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
 /* ************************************************************************* */
 static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
   GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
-  gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2),   Vector2(0.0, 1.0),
+  gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2,   Vector2(0.0, 1.0),
-                                           400*eye(2,2), Vector2(1.0, 1.0), 3.0);
+                                           400*I_2x2, Vector2(1.0, 1.0), 3.0);
   return gfg;
 }
 

gtsam/linear/tests/testHessianFactor.cpp

@@ -54,7 +54,7 @@ TEST(HessianFactor, emptyConstructor)
   HessianFactor f;
   DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9);        // Constant term should be zero
   EXPECT(assert_equal(Vector(), f.linearTerm()));    // Linear term should be empty
-  EXPECT(assert_equal(zeros(1,1), f.info()));        // Full matrix should be 1-by-1 zero matrix
+  EXPECT(assert_equal((Matrix) Z_1x1, f.info()));        // Full matrix should be 1-by-1 zero matrix
   DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
 }
 
@@ -123,11 +123,11 @@ TEST(HessianFactor, Constructor1)
 TEST(HessianFactor, Constructor1b)
 {
   Vector mu = Vector2(1.0,2.0);
-  Matrix Sigma = eye(2,2);
+  Matrix Sigma = I_2x2;
 
   HessianFactor factor(0, mu, Sigma);
 
-  Matrix G = eye(2,2);
+  Matrix G = I_2x2;
   Vector g = G*mu;
   double f = dot(g,mu);
 
@@ -484,7 +484,7 @@ TEST(HessianFactor, combine) {
   -8.94427191,      0.0,
          0.0, -8.94427191).finished();
   Vector b = Vector2(2.23606798,-1.56524758);
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2));
   GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
   GaussianFactorGraph factors = list_of(f);
 

gtsam/linear/tests/testJacobianFactor.cpp

@@ -168,19 +168,19 @@ namespace simple_graph {
 Key keyX(10), keyY(8), keyZ(12);
 
 double sigma1 = 0.1;
-Matrix A11 = Matrix::Identity(2, 2);
+Matrix A11 = I_2x2;
 Vector2 b1(2, -1);
 JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));
 
 double sigma2 = 0.5;
-Matrix A21 = -2 * Matrix::Identity(2, 2);
+Matrix A21 = -2 * I_2x2;
-Matrix A22 = 3 * Matrix::Identity(2, 2);
+Matrix A22 = 3 * I_2x2;
 Vector2 b2(4, -5);
 JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));
 
 double sigma3 = 1.0;
-Matrix A32 = -4 * Matrix::Identity(2, 2);
+Matrix A32 = -4 * I_2x2;
-Matrix A33 = 5 * Matrix::Identity(2, 2);
+Matrix A33 = 5 * I_2x2;
 Vector2 b3(3, -6);
 JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));
 
@@ -193,8 +193,8 @@ TEST( JacobianFactor, construct_from_graph)
 {
   using namespace simple_graph;
 
-  Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2);
+  Matrix A1(6,2); A1 << A11, A21, Z_2x2;
-  Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32;
+  Matrix A2(6,2); A2 << Z_2x2, A22, A32;
   Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
   Vector b(6); b << b1, b2, b3;
   Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
@@ -260,17 +260,17 @@ TEST(JacobianFactor, matrices_NULL)
 
   // hessianDiagonal
   VectorValues expectDiagonal;
-  expectDiagonal.insert(5, ones(3));
+  expectDiagonal.insert(5, Vector::Ones(3));
-  expectDiagonal.insert(10, 4*ones(3));
+  expectDiagonal.insert(10, 4*Vector::Ones(3));
-  expectDiagonal.insert(15, 9*ones(3));
+  expectDiagonal.insert(15, 9*Vector::Ones(3));
   EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
 
   // hessianBlockDiagonal
   map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
   LONGS_EQUAL(3,actualBD.size());
-  EXPECT(assert_equal(1*eye(3),actualBD[5]));
+  EXPECT(assert_equal(1*I_3x3,actualBD[5]));
-  EXPECT(assert_equal(4*eye(3),actualBD[10]));
+  EXPECT(assert_equal(4*I_3x3,actualBD[10]));
-  EXPECT(assert_equal(9*eye(3),actualBD[15]));
+  EXPECT(assert_equal(9*I_3x3,actualBD[15]));
 }
 
 /* ************************************************************************* */
@@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices)
   // hessianBlockDiagonal
   map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
   LONGS_EQUAL(3,actualBD.size());
-  EXPECT(assert_equal(4*eye(3),actualBD[5]));
+  EXPECT(assert_equal(4*I_3x3,actualBD[5]));
-  EXPECT(assert_equal(16*eye(3),actualBD[10]));
+  EXPECT(assert_equal(16*I_3x3,actualBD[10]));
-  EXPECT(assert_equal(36*eye(3),actualBD[15]));
+  EXPECT(assert_equal(36*I_3x3,actualBD[15]));
 }
 
 /* ************************************************************************* */
@@ -324,7 +324,7 @@ TEST(JacobianFactor, operators )
 {
   SharedDiagonal  sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
 
-  Matrix I = eye(2);
+  Matrix I = I_2x2;
   Vector b = Vector2(0.2,-0.1);
   JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
 
@@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate)
   gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
   gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
 
-  Matrix zero3x3 = zeros(3,3);
+  Matrix zero3x3 = Matrix::Zero(3,3);
   Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
   Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
   Vector9 b; b << b1, b0, b2;
@@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
 {
   // construct a linear constraint
   Vector v(2); v(0)=1.2; v(1)=3.4;
-  JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2));
+  JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2));
 
   // eliminate it
   pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
@@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
 
   // verify conditional Gaussian
   Vector sigmas = Vector2(0.0, 0.0);
-  GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
+  GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas));
   EXPECT(assert_equal(expCG, *actual.first));
 }
 

gtsam/linear/tests/testKalmanFilter.cpp

@@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) {
   EXPECT(assert_equal(x_initial, p1->mean()));
   Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished();
   EXPECT(assert_equal(Sigma, p1->covariance()));
-  EXPECT(assert_equal(inverse(Sigma), p1->information()));
+  EXPECT(assert_equal(Sigma.inverse(), p1->information()));
 
   // Create one with a sharedGaussian
   KalmanFilter::State p2 = kf1.init(x_initial, Sigma);
@@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) {
 TEST( KalmanFilter, linear1 ) {
 
   // Create the controls and measurement properties for our example
-  Matrix F = eye(2, 2);
+  Matrix F = I_2x2;
-  Matrix B = eye(2, 2);
+  Matrix B = I_2x2;
   Vector u = Vector2(1.0, 0.0);
   SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
-  Matrix Q = 0.01*eye(2, 2);
+  Matrix Q = 0.01*I_2x2;
-  Matrix H = eye(2, 2);
+  Matrix H = I_2x2;
   State z1(1.0, 0.0);
   State z2(2.0, 0.0);
   State z3(3.0, 0.0);
   SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
-  Matrix R = 0.01*eye(2, 2);
+  Matrix R = 0.01*I_2x2;
 
   // Create the set of expected output TestValues
   State expected0(0.0, 0.0);
-  Matrix P00 = 0.01*eye(2, 2);
+  Matrix P00 = 0.01*I_2x2;
 
   State expected1(1.0, 0.0);
   Matrix P01 = P00 + Q;
-  Matrix I11 = inverse(P01) + inverse(R);
+  Matrix I11 = P01.inverse() + R.inverse();
 
   State expected2(2.0, 0.0);
-  Matrix P12 = inverse(I11) + Q;
+  Matrix P12 = I11.inverse() + Q;
-  Matrix I22 = inverse(P12) + inverse(R);
+  Matrix I22 = P12.inverse() + R.inverse();
 
   State expected3(3.0, 0.0);
-  Matrix P23 = inverse(I22) + Q;
+  Matrix P23 = I22.inverse() + Q;
-  Matrix I33 = inverse(P23) + inverse(R);
+  Matrix I33 = P23.inverse() + R.inverse();
 
   // Create a Kalman filter of dimension 2
   KalmanFilter kf(2);
@@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) {
   Vector u = Vector3(1.0, 0.0, 2.0);
   Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished();
   Matrix M = trans(R)*R;
-  Matrix Q = inverse(M);
+  Matrix Q = M.inverse();
 
   // Create a Kalman filter of dimension 2
   KalmanFilter kf(2);
@@ -167,7 +167,7 @@ TEST( KalmanFilter, predict ) {
 // Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update
 TEST( KalmanFilter, QRvsCholesky ) {
 
-  Vector mean = ones(9);
+  Vector mean = Vector::Ones(9);
   Matrix covariance = 1e-6 * (Matrix(9, 9) <<
       15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6,
       -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1,
@@ -197,8 +197,8 @@ TEST( KalmanFilter, QRvsCholesky ) {
       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished();
-  Matrix B = zeros(9, 1);
+  Matrix B = Matrix::Zero(9, 1);
-  Vector u = zero(1);
+  Vector u = Z_1x1;
   Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
       33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
       3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
   EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
 
   // do the above update again, this time with a full Matrix Q
-  Matrix modelQ = diag(emul(sigmas,sigmas));
+  Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal();
   KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ);
   KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -112,7 +112,7 @@ TEST(NoiseModel, Unit)
 TEST(NoiseModel, equals)
 {
   Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
-                       g2 = Gaussian::SqrtInformation(eye(3,3));
+                       g2 = Gaussian::SqrtInformation(I_3x3);
   Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
                        d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
   Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
@@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors )
   Vector3 mu(200.0, 300.0, 400.0);
   actual = Constrained::All(d);
   // TODO: why should this be a thousand ??? Dummy variable?
-  EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
+  EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
-  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
+  EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
-  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value
+  EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
-  EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
+  EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value
 
   actual = Constrained::All(d, m);
-  EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
+  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
 
   actual = Constrained::All(d, mu);
   EXPECT(assert_equal(mu, actual->mu()));
@@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors )
   EXPECT(assert_equal(mu, actual->mu()));
 
   actual = Constrained::MixedSigmas(m, sigmas);
-  EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
+  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
 }
 
 /* ************************************************************************* */
@@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation )
 {
   bool smart = true;
   gtsam::SharedGaussian expected = Unit::Create(3);
-  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart);
+  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart);
   EXPECT(assert_equal(*expected,*actual));
 }
 
@@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 )
 {
   bool smart = true;
   gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
-  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart);
+  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart);
   EXPECT(assert_equal(*expected,*actual));
 }
 
@@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation )
 {
   bool smart = true;
   gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
-  Matrix M = 0.5*eye(3);
+  Matrix M = 0.5*I_3x3;
   EXPECT(checkIfDiagonal(M));
   gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
   EXPECT(assert_equal(*expected,*actual));
@@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance )
 {
   bool smart = true;
   gtsam::SharedGaussian expected = Unit::Create(3);
-  gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+  gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
   EXPECT(assert_equal(*expected,*actual));
 }
 
@@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector )
 {
   bool smart = true;
   SharedGaussian expected = Unit::Create(3);
-  SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
+  SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
   EXPECT(assert_equal(*expected,*actual));
 }
 
@@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace)
 {
   Vector sigmas = Vector3(0.1, 0.1, 0.1);
   SharedDiagonal model = Diagonal::Sigmas(sigmas);
-  Matrix A = eye(3);
+  Matrix A = I_3x3;
   model->WhitenInPlace(A);
-  Matrix expected = eye(3) * 10;
+  Matrix expected = I_3x3 * 10;
   EXPECT(assert_equal(expected, A));
 }
 

gtsam/linear/tests/testSerializationLinear.cpp

@@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 /* ************************************************************************* */
 // example noise models
 static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
-static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
+static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3);
 static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
 static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1));
 static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
@@ -144,8 +144,8 @@ TEST (Serialization, linear_factors) {
   EXPECT(equalsBinary<VectorValues>(values));
 
   Key i1 = 4, i2 = 7;
-  Matrix A1 = eye(3), A2 = -1.0 * eye(3);
+  Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
-  Vector b = ones(3);
+  Vector b = Vector::Ones(3);
   SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
   JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
   EXPECT(equalsObj(jacobianfactor));
@@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) {
 
   {
     Key i1 = 4, i2 = 7;
-    Matrix A1 = eye(3), A2 = -1.0 * eye(3);
+    Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
-    Vector b = ones(3);
+    Vector b = Vector::Ones(3);
     SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
     JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
     HessianFactor hessianfactor(jacobianfactor);

gtsam/navigation/AttitudeFactor.h

@@ -189,7 +189,7 @@ public:
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
-      *H = Matrix::Zero(2, 6);
+      *H = Matrix::Zero(2,6);
       H->block<2,3>(0,0) = H23;
     }
     return e;

gtsam/navigation/MagFactor.h

@@ -154,7 +154,7 @@ public:
     // measured bM = nRb<52> * nM + b, where b is unknown bias
     Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
     if (H2)
-      *H2 = eye(3);
+      *H2 = I_3x3;
     return (hx - measured_);
   }
 };
@@ -205,7 +205,7 @@ public:
       *H2 = scale * H * (*H2);
     }
     if (H3)
-      *H3 = eye(3);
+      *H3 = I_3x3;
     return (hx - measured_);
   }
 };

gtsam/navigation/PreintegratedRotation.h

@@ -26,6 +26,38 @@
 
 namespace gtsam {
 
+/// Parameters for pre-integration:
+/// Usage: Create just a single Params and pass a shared pointer to the constructor
+struct PreintegratedRotationParams {
+  Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
+  boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
+  boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
+
+  PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
+
+  virtual void print(const std::string& s) const;
+  virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
+
+  void setGyroscopeCovariance(const Matrix3& cov)   { gyroscopeCovariance = cov;  }
+  void setOmegaCoriolis(const Vector3& omega)       { omegaCoriolis.reset(omega); }
+  void setBodyPSensor(const Pose3& pose)            { body_P_sensor.reset(pose);  }
+
+  const Matrix3& getGyroscopeCovariance()     const { return gyroscopeCovariance; }
+  boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
+  boost::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
+    ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
+    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
+  }
+};
+
 /**
  * PreintegratedRotation is the base class for all PreintegratedMeasurements
  * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
@@ -33,29 +65,7 @@ namespace gtsam {
  */
 class PreintegratedRotation {
  public:
-  /// Parameters for pre-integration:
+  typedef PreintegratedRotationParams Params;
-  /// Usage: Create just a single Params and pass a shared pointer to the constructor
-  struct Params {
-    Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
-    boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
-    boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
-
-    Params() : gyroscopeCovariance(I_3x3) {}
-
-    virtual void print(const std::string& s) const;
-    virtual bool equals(const Params& other, double tol=1e-9) const;
-
-   private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      namespace bs = ::boost::serialization;
-      ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
-      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
-      ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
-    }
-  };
 
  protected:
   /// Parameters

gtsam/navigation/PreintegrationBase.cpp

@@ -105,7 +105,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
     // Update derivative: centrifugal causes the correlation between acc and omega!!!
     if (correctedAcc_H_unbiasedOmega) {
       double wdp = correctedOmega.dot(b_arm);
-      *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp))
+      *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
           + correctedOmega * b_arm.transpose()) * bRs.matrix()
           + 2 * b_arm * unbiasedOmega.transpose();
     }

gtsam/navigation/PreintegrationParams.h

@@ -23,7 +23,7 @@ namespace gtsam {
 
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
-struct PreintegrationParams: PreintegratedRotation::Params {
+struct PreintegrationParams: PreintegratedRotationParams {
   Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
   Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
@@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params {
   void print(const std::string& s) const;
   bool equals(const PreintegratedRotation::Params& other, double tol) const;
 
+  void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
+  void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }
+  void setUse2ndOrderCoriolis(bool flag)              { use2ndOrderCoriolis = flag; }
+
+  const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
+  const Matrix3& getIntegrationCovariance()   const { return integrationCovariance; }
+  bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }
+
 protected:
   /// Default constructor for serialization only: uninitialized!
   PreintegrationParams() {}
@@ -60,7 +68,7 @@ protected:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
     ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
-         boost::serialization::base_object<PreintegratedRotation::Params>(*this));
+         boost::serialization::base_object<PreintegratedRotationParams>(*this));
     ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
     ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
     ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
 
   const Vector3 x = thetahat; // parametrization of so(3)
   const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-  double normx = norm_2(x);
+  double normx = x.norm();
   const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
       + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
           * X;

gtsam/navigation/tests/testAttitudeFactor.cpp

@@ -42,7 +42,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
 
   // Create a linearization point at the zero-error point
   Rot3 nRb;
-  EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
+  EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Rot3>(
@@ -75,7 +75,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
 
   // Create a linearization point at the zero-error point
   Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
-  EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
+  EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Pose3>(

gtsam/navigation/tests/testGPSFactor.cpp

@@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) {
 
   // Create a linearization point at zero error
   Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
-  EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
+  EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,Pose3>(
@@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) {
 
   // Create a linearization point at zero error
   NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
-  EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
+  EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
   Matrix expectedH = numericalDerivative11<Vector,NavState>(

gtsam/navigation/tests/testMagFactor.cpp

@@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) {
 
   // MagFactor
   MagFactor f(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
+  EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5));
   EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
       (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
 
 // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
+  EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
       (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
 
 // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
-  EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
+  EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5));
   EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
       (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
       H1, 1e-7));
@@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) {
 
 // MagFactor2
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
-  EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
+  EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
       (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
       H1, 1e-7));

gtsam/nonlinear/NonlinearEquality.h

@@ -142,18 +142,18 @@ public:
     const size_t nj = traits<T>::GetDimension(feasible_);
     if (allow_error_) {
       if (H)
-        *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
+        *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
       return traits<T>::Local(xj,feasible_);
     } else if (compare_(feasible_, xj)) {
       if (H)
-        *H = eye(nj);
+        *H = Matrix::Identity(nj,nj);
-      return zero(nj); // set error to zero if equal
+      return Vector::Zero(nj); // set error to zero if equal
     } else {
       if (H)
         throw std::invalid_argument(
             "Linearization point not feasible for "
                 + DefaultKeyFormatter(this->key()) + "!");
-      return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
+      return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
     }
   }
 
@@ -249,7 +249,7 @@ public:
   Vector evaluateError(const X& x1,
       boost::optional<Matrix&> H = boost::none) const {
     if (H)
-      (*H) = eye(traits<X>::GetDimension(x1));
+      (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
     // manifold equivalent of h(x)-z -> log(z,h(x))
     return traits<X>::Local(value_,x1);
   }
@@ -322,8 +322,8 @@ public:
   Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
       boost::none, boost::optional<Matrix&> H2 = boost::none) const {
     static const size_t p = traits<X>::dimension;
-    if (H1) *H1 = -eye(p);
+    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 =  eye(p);
+    if (H2) *H2 =  Matrix::Identity(p,p);
     return traits<X>::Local(x1,x2);
   }
 

gtsam/nonlinear/NonlinearFactor.h

@@ -313,7 +313,7 @@ public:
         return evaluateError(x1);
       }
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 
@@ -388,7 +388,7 @@ public:
         return evaluateError(x1, x2);
       }
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 
@@ -463,7 +463,7 @@ public:
       else
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 
@@ -543,7 +543,7 @@ public:
       else
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 
@@ -627,7 +627,7 @@ public:
       else
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 
@@ -715,7 +715,7 @@ public:
       else
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
     } else {
-      return zero(this->dim());
+      return Vector::Zero(this->dim());
     }
   }
 

gtsam/nonlinear/Values-inl.h

@@ -269,25 +269,114 @@ namespace gtsam {
      return filter(key_value.key);
    }
 
-  /* ************************************************************************* */
+   /* ************************************************************************* */
-  template<typename ValueType>
-  const ValueType& Values::at(Key j) const {
-    // Find the item
-    KeyValueMap::const_iterator item = values_.find(j);
 
-    // Throw exception if it does not exist
+    namespace internal {
-    if(item == values_.end())
-      throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    const Value& value = *item->second;
+    // Generic version, partially specialized below for various Eigen Matrix types
-    try {
+    template<typename ValueType>
-      return dynamic_cast<const GenericValue<ValueType>&>(value).value();
+    struct handle {
-    } catch (std::bad_cast &) {
+      ValueType operator()(Key j, const gtsam::Value * const pointer) {
-      // NOTE(abe): clang warns about potential side effects if done in typeid
+        try {
-      const Value* value = item->second;
+          // value returns a const ValueType&, and the return makes a copy !!!!!
-      throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
+          return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
-    }
+        } catch (std::bad_cast &) {
+          throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
+        }
+      }
+    };
+
+    // Handle dynamic vectors
+    template<>
+    struct handle<Eigen::Matrix<double, -1, 1> > {
+       Eigen::Matrix<double, -1, 1> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          // value returns a const Vector&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // If a fixed vector was stored, we end up here as well.
+          throw ValuesIncorrectType(j, typeid(*pointer),
+              typeid(Eigen::Matrix<double, -1, 1>));
+        }
+      }
+    };
+
+    // Handle dynamic matrices
+    template<int N>
+    struct handle<Eigen::Matrix<double, -1, N> > {
+       Eigen::Matrix<double, -1, N> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          // value returns a const Matrix&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // If a fixed matrix was stored, we end up here as well.
+          throw ValuesIncorrectType(j, typeid(*pointer),
+              typeid(Eigen::Matrix<double, -1, N>));
+        }
+      }
+    };
+
+    // Request for a fixed vector
+    // TODO(jing): is this piece of code really needed ???
+    template<int M>
+    struct handle<Eigen::Matrix<double, M, 1> > {
+      Eigen::Matrix<double, M, 1> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          // value returns a const VectorM&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // Check if a dynamic vector was stored
+          Matrix A = handle<Eigen::VectorXd>()(j, pointer); // will throw if not....
+          // Yes: check size, and throw if not a match
+          if (A.rows() != M || A.cols() != 1)
+            throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols());
+          else
+            // This is not a copy because of RVO
+            return A;
+        }
+      }
+    };
+
+    // Request for a fixed matrix
+    template<int M, int N>
+    struct handle<Eigen::Matrix<double, M, N> > {
+      Eigen::Matrix<double, M, N> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          // value returns a const MatrixMN&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // Check if a dynamic matrix was stored
+          Matrix A = handle<Eigen::MatrixXd>()(j, pointer); // will throw if not....
+          // Yes: check size, and throw if not a match
+          if (A.rows() != M || A.cols() != N)
+            throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
+          else
+            // This is not a copy because of RVO
+            return A;
+        }
+      }
+    };
+
+
+    } // internal
+
+   /* ************************************************************************* */
+   template<typename ValueType>
+   ValueType Values::at(Key j) const {
+     // Find the item
+     KeyValueMap::const_iterator item = values_.find(j);
+
+     // Throw exception if it does not exist
+     if(item == values_.end())
+       throw ValuesKeyDoesNotExist("at", j);
+
+    // Check the type and throw exception if incorrect
+    return internal::handle<ValueType>()(j,item->second);
   }
 
   /* ************************************************************************* */
@@ -312,16 +401,48 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+
+  // wrap all fixed in dynamics when insert and update
+  namespace internal {
+
+  // general type
+  template<typename ValueType>
+  struct handle_wrap {
+    inline gtsam::GenericValue<ValueType> operator()(Key j, const ValueType& val) {
+      return gtsam::GenericValue<ValueType>(val);
+    }
+  };
+
+  // when insert/update a fixed size vector: convert to dynamic size
+  template<int M>
+  struct handle_wrap<Eigen::Matrix<double, M, 1> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, 1> > operator()(
+        Key j, const Eigen::Matrix<double, M, 1>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, 1> >(val);
+    }
+  };
+
+  // when insert/update a fixed size matrix: convert to dynamic size
+  template<int M, int N>
+  struct handle_wrap<Eigen::Matrix<double, M, N> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, -1> > operator()(
+        Key j, const Eigen::Matrix<double, M, N>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, -1> >(val);
+    }
+  };
+
+  }
+
   // insert a templated value
   template<typename ValueType>
-   void Values::insert(Key j, const ValueType& val) {
+  void Values::insert(Key j, const ValueType& val) {
-     insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
+    insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
-   }
+  }
 
   // update with templated value
   template <typename ValueType>
   void Values::update(Key j, const ValueType& val) {
-    update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
+    update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
   }
 
 }

gtsam/nonlinear/Values.cpp

@@ -25,8 +25,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <list>
-
 #include <boost/foreach.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
@@ -38,6 +36,9 @@
 #endif
 #include <boost/iterator/transform_iterator.hpp>
 
+#include <list>
+#include <sstream>
+
 using namespace std;
 
 namespace gtsam {
@@ -112,24 +113,6 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
-  Vector Values::atFixed(Key j,  size_t n) {
-    switch (n) {
-    case 1: return at<Vector1>(j);
-    case 2: return at<Vector2>(j);
-    case 3: return at<Vector3>(j);
-    case 4: return at<Vector4>(j);
-    case 5: return at<Vector5>(j);
-    case 6: return at<Vector6>(j);
-    case 7: return at<Vector7>(j);
-    case 8: return at<Vector8>(j);
-    case 9: return at<Vector9>(j);
-    default:
-      throw runtime_error(
-          "Values::at fixed size can only handle n in 1..9");
-    }
-  }
-
   /* ************************************************************************* */
   const Value& Values::at(Key j) const {
     // Find the item
@@ -148,24 +131,6 @@ namespace gtsam {
       throw ValuesKeyAlreadyExists(j);
   }
 
-  /* ************************************************************************* */
-  void Values::insertFixed(Key j, const Vector& v, size_t n) {
-    switch (n) {
-    case 1: insert<Vector1>(j,v); break;
-    case 2: insert<Vector2>(j,v); break;
-    case 3: insert<Vector3>(j,v); break;
-    case 4: insert<Vector4>(j,v); break;
-    case 5: insert<Vector5>(j,v); break;
-    case 6: insert<Vector6>(j,v); break;
-    case 7: insert<Vector7>(j,v); break;
-    case 8: insert<Vector8>(j,v); break;
-    case 9: insert<Vector9>(j,v); break;
-    default:
-      throw runtime_error(
-          "Values::insert fixed size can only handle n in 1..9");
-    }
-  }
-
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
     for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@@ -269,4 +234,18 @@ namespace gtsam {
     return message_.c_str();
   }
 
+  /* ************************************************************************* */
+  const char* NoMatchFoundForFixed::what() const throw() {
+    if(message_.empty()) {
+      ostringstream oss;
+    oss
+        << "Attempting to retrieve fixed-size matrix with dimensions " //
+        << M1_ << "x" << N1_
+        << ", but found dynamic Matrix with mismatched dimensions " //
+        << M2_ << "x" << N2_;
+      message_ = oss.str();
+    }
+    return message_.c_str();
+  }
+
 }

gtsam/nonlinear/Values.h

@@ -168,16 +168,13 @@ namespace gtsam {
     /** Retrieve a variable by key \c j.  The type of the value associated with
      * this key is supplied as a template argument to this function.
      * @param j Retrieve the value associated with this key
-     * @tparam Value The type of the value stored with this key, this method
+     * @tparam ValueType The type of the value stored with this key, this method
-     * throws DynamicValuesIncorrectType if this requested type is not correct.
+     * Throws DynamicValuesIncorrectType if this requested type is not correct.
-     * @return A const reference to the stored value
+     * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
+     * @return The stored value
      */
     template<typename ValueType>
-    const ValueType& at(Key j) const;
+    ValueType at(Key j) const;
-
-    /// Special version for small fixed size vectors, for matlab/python
-    /// throws truntime error if n<1 || n>9
-    Vector atFixed(Key j, size_t n);
 
     /// version for double
     double atDouble(size_t key) const { return at<double>(key);}
@@ -259,10 +256,6 @@ namespace gtsam {
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
 
-    /// Special version for small fixed size vectors, for matlab/python
-    /// throws truntime error if n<1 || n>9
-    void insertFixed(Key j, const Vector& v, size_t n);
-
     /// version for double
     void insertDouble(Key j, double c) { insert<double>(j,c); }
 
@@ -500,6 +493,28 @@ namespace gtsam {
     }
   };
 
+  /* ************************************************************************* */
+  class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception {
+
+  protected:
+    const size_t M1_, N1_;
+    const size_t M2_, N2_;
+
+  private:
+    mutable std::string message_;
+
+  public:
+    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
+        M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
+    }
+
+    virtual ~NoMatchFoundForFixed() throw () {
+    }
+
+    virtual const char* what() const throw ();
+  };
+
+  /* ************************************************************************* */
   /// traits
   template<>
   struct traits<Values> : public Testable<Values> {

gtsam/nonlinear/expressions.h

@@ -25,7 +25,7 @@ Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
 }
 
 // Some typedefs
-typedef Expression<double> double_;
+typedef Expression<double> Double_;
 typedef Expression<Vector1> Vector1_;
 typedef Expression<Vector2> Vector2_;
 typedef Expression<Vector3> Vector3_;

gtsam/nonlinear/tests/testExpression.cpp

@@ -17,7 +17,7 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam/nonlinear/Expression.h>
+#include <gtsam/nonlinear/expressions.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point3.h>
@@ -32,9 +32,7 @@ using boost::assign::map_list_of;
 using namespace std;
 using namespace gtsam;
 
-typedef Expression<double> double_;
 typedef Expression<Point3> Point3_;
-typedef Expression<Vector3> Vector3_;
 typedef Expression<Pose3> Pose3_;
 typedef Expression<Rot3> Rot3_;
 
@@ -101,7 +99,7 @@ TEST(Expression, Unary1) {
 }
 TEST(Expression, Unary2) {
   using namespace unary;
-  double_ e(f2, p);
+  Double_ e(f2, p);
   EXPECT(expected == e.keys());
 }
 
@@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p);
 // Check that creating an expression to double compiles
 TEST(Expression, BinaryToDouble) {
   using namespace binary;
-  double_ p_cam(doubleF, x, p);
+  Double_ p_cam(doubleF, x, p);
 }
 
 /* ************************************************************************* */
@@ -269,11 +267,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi
                   OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
   // return dummy derivatives (not correct, but that's ok for testing here)
   if (H1)
-    *H1 = eye(3);
+    *H1 = I_3x3;
   if (H2)
-    *H2 = eye(3);
+    *H2 = I_3x3;
   if (H3)
-    *H3 = eye(3);
+    *H3 = I_3x3;
   return R1 * (R2 * R3);
 }
 
@@ -372,8 +370,8 @@ TEST(Expression, TripleSum) {
 /* ************************************************************************* */
 TEST(Expression, SumOfUnaries) {
   const Key key(67);
-  const double_ norm_(&gtsam::norm, Point3_(key));
+  const Double_ norm_(&gtsam::norm, Point3_(key));
-  const double_ sum_ = norm_ + norm_;
+  const Double_ sum_ = norm_ + norm_;
 
   Values values;
   values.insert<Point3>(key, Point3(6, 0, 0));
@@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) {
 TEST(Expression, UnaryOfSum) {
   const Key key1(42), key2(67);
   const Point3_ sum_ = Point3_(key1) + Point3_(key2);
-  const double_ norm_(&gtsam::norm, sum_);
+  const Double_ norm_(&gtsam::norm, sum_);
 
   map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
   norm_.dims(actual_dims);

gtsam/nonlinear/tests/testLinearContainerFactor.cpp

@@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) {
   // create a linear factor
   SharedDiagonal model = noiseModel::Unit::Create(2);
   JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
-      l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model));
+      l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model));
 
   // create a set of values - build with full set of values
   gtsam::Values full_values, exp_values;

gtsam/nonlinear/tests/testValues.cpp

@@ -477,13 +477,59 @@ TEST(Values, Destructors) {
 }
 
 /* ************************************************************************* */
-TEST(Values, FixedSize) {
+TEST(Values, VectorDynamicInsertFixedRead) {
   Values values;
   Vector v(3); v << 5.0, 6.0, 7.0;
-  values.insertFixed(key1, v, 3);
+  values.insert(key1, v);
   Vector3 expected(5.0, 6.0, 7.0);
-  CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
+  Vector3 actual = values.at<Vector3>(key1);
-  CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
+  CHECK(assert_equal(expected, actual));
+  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorDynamicInsertDynamicRead) {
+  Values values;
+  Vector v(3); v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector expected(3); expected << 5.0, 6.0, 7.0;
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorFixedInsertFixedRead) {
+  Values values;
+  Vector3 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector3 expected; expected << 5.0, 6.0, 7.0;
+  Vector3 actual = values.at<Vector3>(key1);
+  CHECK(assert_equal(expected, actual));
+  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorFixedInsertDynamicRead) {
+  Values values;
+  Vector3 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector expected(3); expected << 5.0, 6.0, 7.0;
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Values, MatrixDynamicInsertFixedRead) {
+  Values values;
+  Matrix v(1,3); v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector3 expected(5.0, 6.0, 7.0);
+  CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
+  CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
 }
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

gtsam/slam/GeneralSFMFactor.h

@@ -129,7 +129,7 @@ public:
       if (H1) *H1 = JacobianC::Zero();
       if (H2) *H2 = JacobianL::Zero();
       // TODO warn if verbose output asked for
-      return zero(2);
+      return Z_2x1;
     }
   }
 
@@ -266,13 +266,13 @@ public:
       return reprojError.vector();
     }
     catch( CheiralityException& e) {
-      if (H1) *H1 = zeros(2, 6);
+      if (H1) *H1 = Matrix::Zero(2, 6);
-      if (H2) *H2 = zeros(2, 3);
+      if (H2) *H2 = Matrix::Zero(2, 3);
-      if (H3) *H3 = zeros(2, DimK);
+      if (H3) *H3 = Matrix::Zero(2, DimK);
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
       << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
     }
-    return zero(2);
+    return Z_2x1;
   }
 
   /** return the measured */

gtsam/slam/InitializePose3.cpp

@@ -31,10 +31,9 @@ using namespace std;
 namespace gtsam {
 namespace InitializePose3 {
 
-//static const Matrix I = eye(1);
+static const Matrix I9 = I_9x9;
-static const Matrix I9 = eye(9);
 static const Vector zero9 = Vector::Zero(9);
-static const Matrix zero33= Matrix::Zero(3,3);
+static const Matrix zero33 = Z_3x3;
 
 static const Key keyAnchor = symbol('Z', 9999999);
 
@@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
     else
       std::cout << "Error in buildLinearOrientationGraph" << std::endl;
 
-    // std::cout << "Rij \n" << Rij << std::endl;
-
     const FastVector<Key>& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
-    Matrix M9 = Matrix::Zero(9,9);
+    Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
@@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
-  Matrix ppm = Matrix::Zero(3,3); // plus plus minus
+  Matrix ppm = Z_3x3; // plus plus minus
   ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
 
   Values validRot3;

gtsam/slam/JacobianFactorQ.h

@@ -56,7 +56,7 @@ public:
       Base() {
     size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
     // Calculate projector Q
-    Matrix Q = eye(m2) - E * P * E.transpose();
+    Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
     // Calculate pre-computed Jacobian matrices
     // TODO: can we do better ?
     std::vector<KeyMatrix> QF;

gtsam/slam/OrientedPlane3Factor.cpp

@@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
     Vector e = n_hat_p.error(n_hat_q, H_p);
     H->resize(2, 3);
     H->block<2, 2>(0, 0) << H_p;
-    H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
+    H->block<2, 1>(0, 2) << Z_2x1;
     return e;
   } else {
     Unit3 n_hat_p = measured_p_.normal();

gtsam/slam/PoseRotationPrior.h

@@ -75,7 +75,7 @@ public:
   Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
     const Rotation& newR = pose.rotation();
     if (H) {
-      *H = gtsam::zeros(rDim, xDim);
+      *H = Matrix::Zero(rDim, xDim);
       std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
       (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
     }

gtsam/slam/PoseTranslationPrior.h

@@ -65,7 +65,7 @@ public:
     const int tDim = traits<Translation>::GetDimension(newTrans);
     const int xDim = traits<Pose>::GetDimension(pose);
     if (H) {
-      *H = gtsam::zeros(tDim, xDim);
+      *H = Matrix::Zero(tDim, xDim);
       std::pair<size_t, size_t> transInterval = POSE::translationInterval();
       (*H).middleCols(transInterval.first, tDim) = R.matrix();
     }

gtsam/slam/PriorFactor.h

@@ -86,7 +86,7 @@ namespace gtsam {
 
     /** vector of errors */
     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
-      if (H) (*H) = eye(traits<T>::GetDimension(x));
+      if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
       // TODO(ASL) Add Jacobians.
       return -traits<T>::Local(x, prior_);

gtsam/slam/ProjectionFactor.h

@@ -146,15 +146,15 @@ namespace gtsam {
           return reprojectionError.vector();
         }
       } catch( CheiralityException& e) {
-        if (H1) *H1 = zeros(2,6);
+        if (H1) *H1 = Matrix::Zero(2,6);
-        if (H2) *H2 = zeros(2,3);
+        if (H2) *H2 = Matrix::Zero(2,3);
         if (verboseCheirality_)
           std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
               " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
         if (throwCheirality_)
           throw e;
       }
-      return ones(2) * 2.0 * K_->fx();
+      return Vector2::Constant(2.0 * K_->fx());
     }
 
     /** return the measurement */

gtsam/slam/ReferenceFrameFactor.h

@@ -100,7 +100,7 @@ public:
         boost::optional<Matrix&> Dlocal = boost::none) const  {
     Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
     if (Dlocal)
-      *Dlocal = -1* gtsam::eye(Point::dimension);
+      *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension);
     return traits<Point>::Local(local,newlocal);
   }
 

gtsam/slam/SmartFactorBase.h

@@ -29,6 +29,7 @@
 #include <gtsam/geometry/CameraSet.h>
 
 #include <boost/optional.hpp>
+#include <boost/serialization/optional.hpp>
 #include <boost/make_shared.hpp>
 #include <vector>
 
@@ -73,7 +74,7 @@ protected:
   std::vector<Z> measured_;
 
   /// @name Pose of the camera in the body frame
-  const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
+  boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
   /// @}
 
   // Cache for Fblocks, to avoid a malloc ever time we re-linearize
@@ -385,7 +386,9 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar & BOOST_SERIALIZATION_NVP(noiseModel_);
     ar & BOOST_SERIALIZATION_NVP(measured_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
 };
 // end class SmartFactorBase

gtsam/slam/SmartProjectionFactor.h

@@ -112,6 +112,20 @@ struct GTSAM_EXPORT SmartProjectionParams {
   void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
     triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
   }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(linearizationMode);
+    ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
+    ar & BOOST_SERIALIZATION_NVP(triangulation);
+    ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
+    ar & BOOST_SERIALIZATION_NVP(throwCheirality);
+    ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
+  }
 };
 
 /**
@@ -277,9 +291,9 @@ public:
     if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
       // failed: return"empty" Hessian
       BOOST_FOREACH(Matrix& m, Gs)
-        m = zeros(Base::Dim, Base::Dim);
+        m = Matrix::Zero(Base::Dim, Base::Dim);
       BOOST_FOREACH(Vector& v, gs)
-        v = zero(Base::Dim);
+        v = Vector::Zero(Base::Dim);
       return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
           Gs, gs, 0.0);
     }
@@ -463,7 +477,7 @@ public:
     if (nonDegenerate)
       return Base::unwhitenedError(cameras, *result_);
     else
-      return zero(cameras.size() * 2);
+      return Vector::Zero(cameras.size() * 2);
   }
 
   /**
@@ -535,8 +549,9 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
+    ar & BOOST_SERIALIZATION_NVP(params_);
-    ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
+    ar & BOOST_SERIALIZATION_NVP(result_);
+    ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
   }
 }
 ;

gtsam/slam/StereoFactor.h

@@ -138,15 +138,15 @@ public:
         return (stereoCam.project(point, H1, H2) - measured_).vector();
       }
     } catch(StereoCheiralityException& e) {
-      if (H1) *H1 = zeros(3,6);
+      if (H1) *H1 = Matrix::Zero(3,6);
-      if (H2) *H2 = zeros(3,3);
+      if (H2) *H2 = Z_3x3;
       if (verboseCheirality_)
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       if (throwCheirality_)
         throw e;
     }
-    return ones(3) * 2.0 * K_->fx();
+    return Vector3::Constant(2.0 * K_->fx());
   }
 
   /** return the measured */

gtsam/slam/TriangulationFactor.h

@@ -124,14 +124,14 @@ public:
       return error.vector();
     } catch (CheiralityException& e) {
       if (H2)
-        *H2 = zeros(Measurement::dimension, 3);
+        *H2 = Matrix::Zero(Measurement::dimension, 3);
       if (verboseCheirality_)
         std::cout << e.what() << ": Landmark "
             << DefaultKeyFormatter(this->key()) << " moved behind camera"
             << std::endl;
       if (throwCheirality_)
         throw e;
-      return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
+      return Eigen::Matrix<double,Measurement::dimension,1>::Constant(2.0 * camera_.calibration().fx());
     }
   }
 

gtsam/slam/dataset.cpp

@@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
           << p.x() << " "  << p.y() << " " << p.z()  << " " << R.toQuaternion().x()
           << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()  << " " << R.toQuaternion().w();
 
-      Matrix InfoG2o = eye(6);
+      Matrix InfoG2o = I_6x6;
       InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
       InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
       InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
@@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) {
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
       Rot3 R = Rot3::Ypr(yaw,pitch,roll);
       Point3 t = Point3(x, y, z);
-      Matrix m = eye(6);
+      Matrix m = I_6x6;
       for (int i = 0; i < 6; i++)
         for (int j = i; j < 6; j++)
           ls >> m(i, j);
@@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) {
       graph->push_back(factor);
     }
     if (tag == "EDGE_SE3:QUAT") {
-      Matrix m = eye(6);
+      Matrix m = I_6x6;
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
@@ -563,11 +563,13 @@ GraphAndValues load3D(const string& filename) {
           m(j, i) = mij;
         }
       }
-      Matrix mgtsam = eye(6);
+      Matrix mgtsam = I_6x6;
-      mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
+
-      mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
+      mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
-      mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal
+      mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
-      mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal
+      mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
+      mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
+
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
       NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
       graph->push_back(factor);

gtsam/slam/lago.cpp

@@ -30,8 +30,8 @@ using namespace std;
 namespace gtsam {
 namespace lago {
 
-static const Matrix I = eye(1);
+static const Matrix I = I_1x1;
-static const Matrix I3 = eye(3);
+static const Matrix I3 = I_3x3;
 
 static const Key keyAnchor = symbol('Z', 9999999);
 static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =

gtsam/slam/tests/testEssentialMatrixConstraint.cpp

@@ -45,7 +45,7 @@ TEST( EssentialMatrixConstraint, test ) {
       Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
       Point3(-3.37493895, 6.14660244, -8.93650986));
 
-  Vector expected = zero(5);
+  Vector expected = Z_5x1;
   Vector actual = factor.evaluateError(pose1,pose2);
   CHECK(assert_equal(expected, actual, 1e-8));
 

gtsam/slam/tests/testPoseRotationPrior.cpp

@@ -52,7 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
   Pose3 pose1(rot3A, point3A);
   Pose3RotationPrior factor(poseKey, rot3A, model3);
   Matrix actH1;
-  EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }
@@ -78,7 +78,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
   Pose2 pose1(rot2A, point2A);
   Pose2RotationPrior factor(poseKey, rot2A, model1);
   Matrix actH1;
-  EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector1,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }

gtsam/slam/tests/testPoseTranslationPrior.cpp

@@ -48,7 +48,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
   Pose3 pose1(rot3A, point3A);
   Pose3TranslationPrior factor(poseKey, point3A, model3);
   Matrix actH1;
-  EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }
@@ -68,7 +68,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
   Pose3 pose1(rot3B, point3A);
   Pose3TranslationPrior factor(poseKey, point3A, model3);
   Matrix actH1;
-  EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }
@@ -88,7 +88,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
   Pose3 pose1(rot3C, point3A);
   Pose3TranslationPrior factor(poseKey, point3A, model3);
   Matrix actH1;
-  EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }
@@ -108,7 +108,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
   Pose2 pose1(rot2A, point2A);
   Pose2TranslationPrior factor(poseKey, point2A, model2);
   Matrix actH1;
-  EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
+  EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1)));
   Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
   EXPECT(assert_equal(expH1, actH1, tol));
 }

gtsam/slam/tests/testReferenceFrameFactor.cpp

@@ -93,7 +93,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
 
   PointReferenceFrameFactor tc(lA1, tA1, lB1);
   Vector actCost = tc.evaluateError(global, trans, local),
-      expCost = zero(2);
+      expCost = Z_2x1;
   EXPECT(assert_equal(expCost, actCost, 1e-5));
 
   Matrix actualDT, actualDL, actualDF;

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
 //*************************************************************************************
 TEST( regularImplicitSchurFactor, creation ) {
   // Matrix E = Matrix::Ones(6,3);
-  Matrix E = zeros(6, 3);
+  Matrix E = Matrix::Zero(6, 3);
-  E.block<2,2>(0, 0) = eye(2);
+  E.block<2,2>(0, 0) = I_2x2;
-  E.block<2,3>(2, 0) = 2 * ones(2, 3);
+  E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
   Matrix3 P = (E.transpose() * E).inverse();
   RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
   Matrix expectedP = expected.getPointCovariance();
@@ -61,35 +61,35 @@ TEST( regularImplicitSchurFactor, creation ) {
 /* ************************************************************************* */
 TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
-  Matrix E = zeros(6, 3);
+  Matrix E = Matrix::Zero(6, 3);
-  E.block<2,2>(0, 0) = eye(2);
+  E.block<2,2>(0, 0) = I_2x2;
-  E.block<2,3>(2, 0) = 2 * ones(2, 3);
+  E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
-  E.block<2,2>(4, 1) = eye(2);
+  E.block<2,2>(4, 1) = I_2x2;
   Matrix3 P = (E.transpose() * E).inverse();
 
   double alpha = 0.5;
   VectorValues xvalues = map_list_of //
-  (0, gtsam::repeat(6, 2))//
+  (0, Vector::Constant(6, 2))//
-  (1, gtsam::repeat(6, 4))//
+  (1, Vector::Constant(6, 4))//
-  (2, gtsam::repeat(6, 0))// distractor
+  (2, Vector::Constant(6, 0))// distractor
-  (3, gtsam::repeat(6, 8));
+  (3, Vector::Constant(6, 8));
 
   VectorValues yExpected = map_list_of//
-  (0, gtsam::repeat(6, 27))//
+  (0, Vector::Constant(6, 27))//
-  (1, gtsam::repeat(6, -40))//
+  (1, Vector::Constant(6, -40))//
-  (2, gtsam::repeat(6, 0))// distractor
+  (2, Vector::Constant(6, 0))// distractor
-  (3, gtsam::repeat(6, 279));
+  (3, Vector::Constant(6, 279));
 
   // Create full F
   size_t M=4, m = 3, d = 6;
   Matrix F(2 * m, d * M);
-  F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
+  F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
 
   // Calculate expected result F'*alpha*(I - E*P*E')*F*x
   FastVector<Key> keys2;
   keys2 += 0,1,2,3;
   Vector x = xvalues.vector(keys2);
-  Vector expected = zero(24);
+  Vector expected = Vector::Zero(24);
   RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
   EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
 
@@ -170,9 +170,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   }
 
   VectorValues expectedVV;
-  expectedVV.insert(0,-3.5*ones(6));
+  expectedVV.insert(0,-3.5*Vector::Ones(6));
-  expectedVV.insert(1,10*ones(6)/3);
+  expectedVV.insert(1,10*Vector::Ones(6)/3);
-  expectedVV.insert(3,-19.5*ones(6));
+  expectedVV.insert(3,-19.5*Vector::Ones(6));
   { // Check gradientAtZero
     VectorValues actual = implicitFactor.gradientAtZero();
     EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8));
@@ -210,9 +210,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 TEST(regularImplicitSchurFactor, hessianDiagonal)
 {
   /* TESTED AGAINST MATLAB
-   *  F = [ones(2,6) zeros(2,6) zeros(2,6)
+   *  F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6)
-        zeros(2,6) 2*ones(2,6) zeros(2,6)
+        zeros(2,6) 2*Vector::Ones(2,6) zeros(2,6)
-        zeros(2,6) zeros(2,6) 3*ones(2,6)]
+        zeros(2,6) zeros(2,6) 3*Vector::Ones(2,6)]
       E = [[1:6] [1:6] [0.5 1:5]];
       E = reshape(E',3,6)'
       P = inv(E' * E)
@@ -228,9 +228,9 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
 
   // hessianDiagonal
   VectorValues expected;
-  expected.insert(0, 1.195652*ones(6));
+  expected.insert(0, 1.195652*Vector::Ones(6));
-  expected.insert(1, 4.782608*ones(6));
+  expected.insert(1, 4.782608*Vector::Ones(6));
-  expected.insert(3, 7.043478*ones(6));
+  expected.insert(3, 7.043478*Vector::Ones(6));
   EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));
 
   // hessianBlockDiagonal
@@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
   EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
 
   // variant two
-  Matrix I2 = eye(2);
+  Matrix I2 = I_2x2;
   Matrix E0 = E.block<2,3>(0, 0);
   Matrix F0t = F0.transpose();
   EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));

gtsam/slam/tests/testRotateFactor.cpp

@@ -58,7 +58,7 @@ TEST (RotateFactor, checkMath) {
 TEST (RotateFactor, test) {
   Model model = noiseModel::Isotropic::Sigma(3, 0.01);
   RotateFactor f(1, i1Ri2, c1Zc2, model);
-  EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
+  EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8));
 
   Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
@@ -127,7 +127,7 @@ TEST (RotateFactor, minimization) {
 TEST (RotateDirectionsFactor, test) {
   Model model = noiseModel::Isotropic::Sigma(2, 0.01);
   RotateDirectionsFactor f(1, p1, z1, model);
-  EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
+  EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8));
 
   Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
 

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
   double expectedError = 0.0;
   DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
   CHECK(
-      assert_equal(zero(4),
+      assert_equal(Z_4x1,
           factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
 }
 
@@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
   Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
   Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
   double actualError = 0.5
-      * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
+      * (e1.norm() * e1.norm() + e2.norm() * e2.norm());
   double actualErrorGraph = generalGraph.error(values);
 
   DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
   Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
   EXPECT(assert_equal(expectedE, E, 1e-7));
 
-  EXPECT(assert_equal(zero(4), actualErrors, 1e-7));
+  EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
 
   // Calculate using computeJacobians
   Vector b;
@@ -1409,6 +1409,26 @@ TEST(SmartProjectionPoseFactor, serialize) {
   EXPECT(equalsBinary(factor));
 }
 
+TEST(SmartProjectionPoseFactor, serialize2) {
+  using namespace vanillaPose;
+  using namespace gtsam::serializationTestHelpers;
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  Pose3 bts;
+  SmartFactor factor(model, sharedK, bts, params);
+
+  // insert some measurments
+  vector<Key> key_view;
+  vector<Point2> meas_view;
+  key_view.push_back(Symbol('x', 1));
+  meas_view.push_back(Point2(10, 10));
+  factor.add(meas_view, key_view);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/dynamics/DynamicsPriors.h

@@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
     this->mask_[0] = 6;
     this->mask_[1] = 7;
     this->mask_[2] = 8;
-    this->H_ = zeros(3, 9);
+    this->H_ = Matrix::Zero(3, 9);
     this->fillH();
   }
 };
@@ -75,13 +75,13 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
    */
   DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
   : Base(key, model) {
-    this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch]
+    this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch]
     this->mask_.resize(4);
     this->mask_[0] = 5; // z = height
     this->mask_[1] = 8; // vz
     this->mask_[2] = 0; // roll
     this->mask_[3] = 1; // pitch
-    this->H_ = zeros(3, 9);
+    this->H_ = Matrix::Zero(3, 9);
     this->fillH();
   }
 
@@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
     this->mask_[1] = 8; // vz
     this->mask_[2] = 0; // roll
     this->mask_[3] = 1; // pitch
-    this->H_ = zeros(3, 9);
+    this->H_ = Matrix::Zero(3, 9);
     this->fillH();
   }
 };

gtsam_unstable/dynamics/Pendulum.h

@@ -50,9 +50,9 @@ public:
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
     const size_t p = 1;
-    if (H1) *H1 = -eye(p);
+    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 = eye(p);
+    if (H2) *H2 = Matrix::Identity(p,p);
-    if (H3) *H3 = eye(p)*h_;
+    if (H3) *H3 = Matrix::Identity(p,p)*h_;
     return (Vector(1) << qk+v*h_-qk1).finished();
   }
 
@@ -98,9 +98,9 @@ public:
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
     const size_t p = 1;
-    if (H1) *H1 = -eye(p);
+    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 = eye(p);
+    if (H2) *H2 = Matrix::Identity(p,p);
-    if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
+    if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
     return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished();
   }
 
@@ -154,9 +154,9 @@ public:
     double mr2_h = 1/h_*m_*r_*r_;
     double mgrh  = m_*g_*r_*h_;
 
-    if (H1) *H1 = -eye(p);
+    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
+    if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
-    if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
+    if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
 
     return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished();
   }
@@ -210,9 +210,9 @@ public:
     double mr2_h = 1/h_*m_*r_*r_;
     double mgrh  = m_*g_*r_*h_;
 
-    if (H1) *H1 = -eye(p);
+    if (H1) *H1 = -Matrix::Identity(p,p);
-    if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
+    if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
-    if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
+    if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
 
     return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
   }

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -11,7 +11,7 @@ namespace gtsam {
 
 using namespace std;
 
-static const Vector kGravity = delta(3, 2, 9.81);
+static const Vector kGravity = Vector::Unit(3,2)*9.81;
 
 /* ************************************************************************* */
 double bound(double a, double min, double max) {
@@ -105,7 +105,7 @@ PoseRTV PoseRTV::flyingDynamics(
   Vector Acc_n =
       yaw_correction_bn.rotate(forward)              // applies locally forward force in the global frame
       - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished()  // drag term dependent on v1
-      + delta(3, 2, loss_lift - lift_control);                // falling due to lift lost from pitch
+      + Vector::Unit(3,2)*(loss_lift - lift_control);                // falling due to lift lost from pitch
 
   // Update Vehicle Position and Velocity
   Velocity3 v2 = v1 + Velocity3(Acc_n * dt);

gtsam_unstable/dynamics/SimpleHelicopter.h

@@ -140,7 +140,7 @@ public:
     }
 
     if (H3) {
-      *H3 = zeros(6,6);
+      *H3 = Z_6x6;
       insertSub(*H3, -h_*D_gravityBody_gk, 3, 0);
     }
 

gtsam_unstable/dynamics/VelocityConstraint3.h

@@ -41,9 +41,9 @@ public:
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
     const size_t p = 1;
-    if (H1) *H1 = eye(p);
+    if (H1) *H1 = Matrix::Identity(p,p);
-    if (H2) *H2 = -eye(p);
+    if (H2) *H2 = -Matrix::Identity(p,p);
-    if (H3) *H3 = eye(p)*dt_;
+    if (H3) *H3 = Matrix::Identity(p,p)*dt_;
     return (Vector(1) << x1+v*dt_-x2).finished();
   }
 

gtsam_unstable/dynamics/tests/testIMUSystem.cpp

@@ -26,7 +26,7 @@ using namespace gtsam;
 const double tol=1e-5;
 
 static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4;
-static const Vector g = delta(3, 2, -9.81);
+static const Vector g = Vector::Unit(3,2)*(-9.81);
 
 /* ************************************************************************* */
 TEST(testIMUSystem, instantiations) {
@@ -38,7 +38,7 @@ TEST(testIMUSystem, instantiations) {
   gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6);
   gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9);
 
-  Vector accel = ones(3), gyro = ones(3);
+  Vector accel = Vector::Ones(3), gyro = Vector::Ones(3);
 
   IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
   FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
@@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) {
   VelocityConstraint constraint(x1, x2, 0.1, 10000);
   PriorFactor<gtsam::PoseRTV> posePrior(x1, x1_v, model9);
   DHeightPrior heightPrior(x1, 0.1, model1);
-  VelocityPrior velPrior(x1, ones(3), model3);
+  VelocityPrior velPrior(x1, Vector::Ones(3), model3);
 }
 
 /* ************************************************************************* */
@@ -149,8 +149,8 @@ TEST( testIMUSystem, linear_trajectory) {
   const double dt = 1.0;
 
   PoseRTV start;
-  Vector accel = delta(3, 0, 0.5); // forward force
+  Vector accel = Vector::Unit(3,0)*0.5; // forward force
-  Vector gyro = delta(3, 0, 0.1); // constant rotation
+  Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation
   SharedDiagonal model = noiseModel::Unit::Create(9);
 
   Values true_traj, init_traj;

gtsam_unstable/dynamics/tests/testPendulumFactors.cpp

@@ -29,7 +29,7 @@ TEST( testPendulumFactor1, evaluateError) {
   PendulumFactor1 constraint(Q(2), Q(1), V(1), h);
 
   // verify error function
-  EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol));
+  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol));
 }
 
 /* ************************************************************************* */
@@ -38,7 +38,7 @@ TEST( testPendulumFactor2, evaluateError) {
   PendulumFactor2 constraint(V(2), V(1), Q(1), h);
 
   // verify error function
-  EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol));
+  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol));
 }
 
 /* ************************************************************************* */
@@ -49,7 +49,7 @@ TEST( testPendulumFactorPk, evaluateError) {
   double pk( 1/h * (q2-q1) + h*g*sin(q1) );
 
   // verify error function
-  EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
+  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol));
 }
 
 /* ************************************************************************* */
@@ -60,7 +60,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
   double pk1( 1/h * (q2-q1) );
 
   // verify error function
-  EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));
+  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol));
 }
 
 

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -76,12 +76,12 @@ TEST( testPoseRTV, equals ) {
 TEST( testPoseRTV, Lie ) {
   // origin and zero deltas
   PoseRTV identity;
-  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9))));
+  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1)));
-  EXPECT(assert_equal(zero(9), identity.localCoordinates(identity)));
+  EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity)));
 
   PoseRTV state1(pt, rot, vel);
-  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9))));
+  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1)));
-  EXPECT(assert_equal(zero(9), state1.localCoordinates(state1)));
+  EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1)));
 
   Vector delta(9);
   delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
@@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) {
 
   const double dt = 0.1;
   Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2);
-  Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6);
+  Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1;
 
   x1 = x0.generalDynamics(accel, gyro, dt);
   x2 = x1.generalDynamics(accel, gyro, dt);
@@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) {
 
 /* ************************************************************************* */
 TEST(testPoseRTV, RRTMbn) {
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
+  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3)));
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
+  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3())));
   EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
 }
 
 /* ************************************************************************* */
 TEST(testPoseRTV, RRTMnb) {
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
+  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3)));
-  EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
+  EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3())));
   EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
 }
 

gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp

@@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0);  // no shape
 Vector u2 = Vector2(0.0, 0.0); // no control at time 2
 double distT = 1.0; // distance from the body-centered x axis to the big top motor
 double distR = 5.0; // distance from the body-centered z axis to the small motor
-Matrix Mass = diag((Vector(3) << mass, mass, mass).finished());
+Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal();
-Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished());
+Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal();
 
 Vector computeFu(const Vector& gamma, const Vector& control) {
   double gamma_r = gamma(0), gamma_p = gamma(1);
@@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) {
   // verify error function
   Matrix H1, H2, H3;
   EXPECT(
-      assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
+      assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
 
   Matrix numericalH1 = numericalDerivative31(
       boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
@@ -89,8 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
 
 TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
   Vector Fu = computeFu(gamma2, u2);
-  Vector fGravity_g1 = zero(6);
+  Vector fGravity_g1 = Z_6x1;
-  subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
+  fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81));  // gravity force in g1 frame
   Vector Fb = Fu+fGravity_g1;
 
   Vector dV = newtonEuler(V1_g1, Fb, Inertia);
@@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
 
   // verify error function
   Matrix H1, H2, H3;
-  EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
+  EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
 
   Matrix numericalH1 = numericalDerivative31(
       boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(

gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp

@@ -25,10 +25,10 @@ TEST( testVelocityConstraint, trapezoidal ) {
   VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt);
 
   // verify error function
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol));
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
-  EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol));
-  EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
 }
 
 /* ************************************************************************* */
@@ -37,10 +37,10 @@ TEST( testEulerVelocityConstraint, euler_start ) {
   VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt);
 
   // verify error function
-  EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol));
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
-  EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
 }
 
 /* ************************************************************************* */
@@ -49,10 +49,10 @@ TEST( testEulerVelocityConstraint, euler_end ) {
   VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt);
 
   // verify error function
-  EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol));
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
-  EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
+  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
-  EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
+  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
 }
 
 /* ************************************************************************* */

gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp

@@ -23,7 +23,7 @@ TEST( testVelocityConstraint3, evaluateError) {
   VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
 
   // verify error function
-  EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
+  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol));
 }
 
 /* ************************************************************************* */

gtsam_unstable/geometry/InvDepthCamera3.h

@@ -102,7 +102,7 @@ public:
       gtsam::Matrix J2;
       gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none);
       if (H1) {
-        *H1 = (*H1) * gtsam::eye(6);
+        *H1 = (*H1) * I_6x6;
       }
 
       double cos_theta = cos(theta);

gtsam_unstable/geometry/Pose3Upright.cpp

@@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const {
 Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
   Pose3Upright result(T_.inverse(H1), -z_);
   if (H1) {
-    Matrix H1_ = -eye(4,4);
+    Matrix H1_ = -I_4x4;
     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
     *H1 = H1_;
@@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
     return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
   Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
   if (H1) {
-    Matrix H1_ = eye(4,4);
+    Matrix H1_ = I_4x4;
     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
     *H1 = H1_;
   }
-  if (H2) *H2 = eye(4,4);
+  if (H2) *H2 = I_4x4;
   return result;
 }
 
@@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
     return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
   Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
   if (H1) {
-    Matrix H1_ = -eye(4,4);
+    Matrix H1_ = -I_4x4;
     H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
     H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
     *H1 = H1_;
   }
-  if (H2) *H2 = eye(4,4);
+  if (H2) *H2 = I_4x4;
   return result;
 }