Merged in fix/DeprecateVectorFunctions (pull request #244)

fix/DeprecateVectorFunctions

gtsam/base/Matrix.cpp

@@ -290,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));
 

gtsam/base/Matrix.h

@@ -16,6 +16,7 @@
  * @author  Kai Ni
  * @author  Frank Dellaert
  * @author  Alex Cunningham
+ * @author  Alex Hagiopol
  */
 
 // \callgraph

gtsam/base/Vector.cpp

@@ -33,20 +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 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) {
@@ -235,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;
     }
   }

gtsam/base/Vector.h

@@ -14,11 +14,11 @@
  * @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
@@ -63,47 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12);
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 
-/**
- * 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
  */
@@ -272,21 +231,25 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
  */
 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
 
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();}
+inline Vector abs(const Vector& v){return v.cwiseAbs();}
-GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
+inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
-GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
+inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
-GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
+inline size_t dim(const Vector& v) { return v.size(); }
-GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();}
+inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
-GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();}
+inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
-GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
+inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
-GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
+inline double max(const Vector &a){return a.maxCoeff();}
-GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
+inline double norm_2(const Vector& v) {return v.norm();}
-GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
+inline Vector ones(size_t n) { return Vector::Ones(n); }
-GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();}
+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));

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 )
 {
@@ -256,7 +240,7 @@ 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
 }

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/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/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/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;
@@ -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));

gtsam/geometry/tests/testRot2.cpp

@@ -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

@@ -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)

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/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;
 
@@ -594,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/NoiseModel.cpp

@@ -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

@@ -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

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/tests/testGaussianFactorGraph.cpp

@@ -47,7 +47,7 @@ TEST(GaussianFactorGraph, initialization) {
   SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
   fg +=
-    JacobianFactor(0, 10*I_2x2, -1.0*ones(2), unit2),
+    JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2),
     JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
     JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
     JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
@@ -166,7 +166,7 @@ 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*I_2x2, -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*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2);
   // measurement between x1 and l1: l1-x1=[0.0;0.2]

gtsam/linear/tests/testHessianFactor.cpp

@@ -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

@@ -260,9 +260,9 @@ 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

gtsam/linear/tests/testKalmanFilter.cpp

@@ -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,
@@ -198,7 +198,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
       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 = 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,

gtsam/linear/tests/testSerializationLinear.cpp

@@ -145,7 +145,7 @@ TEST (Serialization, linear_factors) {
 
   Key i1 = 4, i2 = 7;
   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));
@@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) {
   {
     Key i1 = 4, i2 = 7;
     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/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

@@ -147,7 +147,7 @@ public:
     } else if (compare_(feasible_, xj)) {
       if (H)
         *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(

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/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, I_2x2, l5, 2.0 * I_2x2, 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/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;
     }
   }
 
@@ -272,7 +272,7 @@ public:
       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/ProjectionFactor.h

@@ -154,7 +154,7 @@ namespace gtsam {
         if (throwCheirality_)
           throw e;
       }
-      return ones(2) * 2.0 * K_->fx();
+      return Vector2::Constant(2.0 * K_->fx());
     }
 
     /** return the measurement */

gtsam/slam/SmartProjectionFactor.h

@@ -293,7 +293,7 @@ public:
       BOOST_FOREACH(Matrix& m, Gs)
         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);
     }
@@ -477,7 +477,7 @@ public:
     if (nonDegenerate)
       return Base::unwhitenedError(cameras, *result_);
     else
-      return zero(cameras.size() * 2);
+      return Vector::Zero(cameras.size() * 2);
   }
 
   /**

gtsam/slam/StereoFactor.h

@@ -146,7 +146,7 @@ public:
       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

@@ -131,7 +131,7 @@ public:
             << 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/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

@@ -89,7 +89,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   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

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));
 }
 

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;

gtsam_unstable/dynamics/DynamicsPriors.h

@@ -75,7 +75,7 @@ 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

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/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);

gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp

@@ -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,7 +89,7 @@ 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;
   fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81));  // gravity force in g1 frame
   Vector Fb = Fu+fGravity_g1;
 
@@ -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/SimPolygon2D.cpp

@@ -149,7 +149,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
 
     // extend from B to find C
     double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
-    Pose2 xC = xB.retract(delta(3, 0, dBC));
+    Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);
 
     // use triangle equality to verify non-degenerate triangle
     double dAC = xA.t().distance(xC.t());

gtsam_unstable/geometry/tests/testBearingS2.cpp

@@ -48,11 +48,11 @@ TEST( testBearingS2, manifold ) {
   BearingS2 origin, b1(0.2, 0.3);
   EXPECT_LONGS_EQUAL(2, origin.dim());
 
-  EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol));
+  EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol));
-  EXPECT(assert_equal(origin, origin.retract(zero(2)), tol));
+  EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol));
 
-  EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol));
+  EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol));
-  EXPECT(assert_equal(b1, b1.retract(zero(2)), tol));
+  EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol));
 }
 
 /* ************************************************************************* */

gtsam_unstable/geometry/tests/testPose3Upright.cpp

@@ -68,9 +68,9 @@ TEST( testPose3Upright, manifold ) {
   Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0);
   EXPECT_LONGS_EQUAL(4, origin.dim());
 
-  EXPECT(assert_equal(origin, origin.retract(zero(4)), tol));
+  EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol));
-  EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
+  EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol));
-  EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
+  EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol));
 
   Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12;
   EXPECT(assert_equal(x2, x1.retract(delta12), tol));
@@ -83,8 +83,8 @@ TEST( testPose3Upright, manifold ) {
 /* ************************************************************************* */
 TEST( testPose3Upright, lie ) {
   Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1);
-  EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol));
+  EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol));
-  EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol));
+  EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol));
 
   EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol));
 }

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -156,7 +156,7 @@ TEST(Similarity3, Manifold) {
 //******************************************************************************
 TEST( Similarity3, retract_first_order) {
   Similarity3 id;
-  Vector v = zero(7);
+  Vector v = Z_7x1;
   v(0) = 0.3;
   EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
 //  v(3) = 0.2;

gtsam_unstable/linear/QPSolver.cpp

@@ -51,7 +51,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
 
   // Collect the gradients of unconstrained cost factors to the b vector
   if (Aterms.size() > 0) {
-    Vector b = zero(delta.at(key).size());
+    Vector b = Vector::Zero(delta.at(key).size());
     if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
       BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
         GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -26,7 +26,7 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 
-const Matrix One = Matrix::Ones(1,1);
+const Matrix One = I_1x1;
 
 /* ************************************************************************* */
 // Create test graph according to Forst10book_pg171Ex5
@@ -38,14 +38,14 @@ QP createTestCase() {
   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
   // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1),
+      HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1,
-          2.0 * Matrix::Ones(1, 1), zero(1), 10.0));
+          2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0));
 
   // Inequality constraints
-  qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
+  qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
-  qp.inequalities.push_back(LinearInequality(X(1), -Matrix::Ones(1,1), 0, 1));                 // -x1     <= 0
+  qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1));                 // -x1     <= 0
-  qp.inequalities.push_back(LinearInequality(X(2), -Matrix::Ones(1,1), 0, 2));                 //    -x2  <= 0
+  qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2));                 //    -x2  <= 0
-  qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), 1.5, 3));                // x1      <= 3/2
+  qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3));                // x1      <= 3/2
 
   return qp;
 }
@@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() {
   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
   // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1),
+      HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1,
-          2.0 * Matrix::Ones(1, 1), zero(1), 0.0));
+          2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0));
 
   // Equality constraints
   // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@@ -110,8 +110,8 @@ TEST(QPSolver, dual) {
 
   // Initials values
   VectorValues initialValues;
-  initialValues.insert(X(1), ones(1));
+  initialValues.insert(X(1), I_1x1);
-  initialValues.insert(X(2), ones(1));
+  initialValues.insert(X(2), I_1x1);
 
   QPSolver solver(qp);
 
@@ -129,8 +129,8 @@ TEST(QPSolver, indentifyActiveConstraints) {
   QPSolver solver(qp);
 
   VectorValues currentSolution;
-  currentSolution.insert(X(1), zero(1));
+  currentSolution.insert(X(1), Z_1x1);
-  currentSolution.insert(X(2), zero(1));
+  currentSolution.insert(X(2), Z_1x1);
 
   LinearInequalityFactorGraph workingSet =
       solver.identifyActiveConstraints(qp.inequalities, currentSolution);
@@ -154,8 +154,8 @@ TEST(QPSolver, iterate) {
   QPSolver solver(qp);
 
   VectorValues currentSolution;
-  currentSolution.insert(X(1), zero(1));
+  currentSolution.insert(X(1), Z_1x1);
-  currentSolution.insert(X(2), zero(1));
+  currentSolution.insert(X(2), Z_1x1);
 
   std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
   expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished());
@@ -199,8 +199,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
   QP qp = createTestCase();
   QPSolver solver(qp);
   VectorValues initialValues;
-  initialValues.insert(X(1), zero(1));
+  initialValues.insert(X(1), Z_1x1);
-  initialValues.insert(X(2), zero(1));
+  initialValues.insert(X(2), Z_1x1);
   VectorValues solution;
   boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
   VectorValues expectedSolution;
@@ -219,8 +219,8 @@ QP createTestMatlabQPEx() {
   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
   // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * ones(1),
+      HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1,
-          2.0 * Matrix::Ones(1, 1), 6 * ones(1), 1000.0));
+          2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0));
 
   // Inequality constraints
   qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0));      // x1 + x2 <= 2
@@ -236,8 +236,8 @@ TEST(QPSolver, optimizeMatlabEx) {
   QP qp = createTestMatlabQPEx();
   QPSolver solver(qp);
   VectorValues initialValues;
-  initialValues.insert(X(1), zero(1));
+  initialValues.insert(X(1), Z_1x1);
-  initialValues.insert(X(2), zero(1));
+  initialValues.insert(X(2), Z_1x1);
   VectorValues solution;
   boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
   VectorValues expectedSolution;
@@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) {
 QP createTestNocedal06bookEx16_4() {
   QP qp;
 
-  qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), ones(1)));
+  qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1));
-  qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * ones(1)));
+  qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1));
 
   // Inequality constraints
   qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
@@ -269,7 +269,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
   QPSolver solver(qp);
   VectorValues initialValues;
   initialValues.insert(X(1), (Vector(1) << 2.0).finished());
-  initialValues.insert(X(2), zero(1));
+  initialValues.insert(X(2), Z_1x1);
 
   VectorValues solution;
   boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
@@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
 
 TEST(QPSolver, failedSubproblem) {
   QP qp;
-  qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2)));
+  qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
-  qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0));
+  qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
   qp.inequalities.push_back(
       LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0));
 

gtsam_unstable/nonlinear/LinearizedFactor.cpp

@@ -194,7 +194,7 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol
 double LinearizedHessianFactor::error(const Values& c) const {
 
   // Construct an error vector in key-order from the Values
-  Vector dx = zero(dim());
+  Vector dx = Vector::Zero(dim());
   size_t index = 0;
   for(unsigned int i = 0; i < this->size(); ++i){
     Key key = this->keys()[i];
@@ -217,7 +217,7 @@ boost::shared_ptr<GaussianFactor>
 LinearizedHessianFactor::linearize(const Values& c) const {
 
   // Construct an error vector in key-order from the Values
-  Vector dx = zero(dim());
+  Vector dx = Vector::Zero(dim());
   size_t index = 0;
   for(unsigned int i = 0; i < this->size(); ++i){
     Key key = this->keys()[i];

gtsam_unstable/slam/AHRS.cpp

@@ -45,9 +45,9 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
 
   F_g_ = -I_3x3 / tau_g;
   F_a_ = -I_3x3 / tau_a;
-  Vector3 var_omega_w = 0 * ones(3); // TODO
+  Vector3 var_omega_w = 0 * Vector::Ones(3); // TODO
-  Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
+  Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(3);
-  Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
+  Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(3);
   Vector3 sigmas_v_g_sq = sigmas_v_g.array().square();
   var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
 
@@ -101,7 +101,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
   P_plus_k2.block<3,3>(6, 3) = Z_3x3;
   P_plus_k2.block<3,3>(6, 6) = Pa;
 
-  Vector dx = zero(9);
+  Vector dx = Z_9x1;
   KalmanFilter::State state = KF_.init(dx, P_plus_k2);
   return std::make_pair(mech0_, state);
 }

gtsam_unstable/slam/DummyFactor.cpp

@@ -55,7 +55,7 @@ DummyFactor::linearize(const Values& c) const {
 
   noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
   return GaussianFactor::shared_ptr(
-      new JacobianFactor(terms, zero(rowDim_), model));
+      new JacobianFactor(terms, Vector::Zero(rowDim_), model));
 }
 
 /* ************************************************************************* */

gtsam_unstable/slam/InvDepthFactor3.h

@@ -94,7 +94,7 @@ public:
       if (H3) *H2 = Matrix::Zero(2,1);
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
-      return gtsam::ones(2) * 2.0 * K_->fx();
+      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (gtsam::Vector(1) << 0.0).finished();
   }

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -93,7 +93,7 @@ public:
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
           << std::endl;
-      return gtsam::ones(2) * 2.0 * K_->fx();
+      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (gtsam::Vector(1) << 0.0).finished();
   }

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -96,7 +96,7 @@ public:
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
           << std::endl;
-      return gtsam::ones(2) * 2.0 * K_->fx();
+      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (gtsam::Vector(1) << 0.0).finished();
   }

gtsam_unstable/slam/InvDepthFactorVariant3.h

@@ -96,7 +96,7 @@ public:
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
           << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
           << std::endl;
-      return gtsam::ones(2) * 2.0 * K_->fx();
+      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (Vector(1) << 0.0).finished();
   }
@@ -215,7 +215,7 @@ public:
           << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
           << " moved behind camera " << DefaultKeyFormatter(this->key2())
           << std::endl;
-      return gtsam::ones(2) * 2.0 * K_->fx();
+      return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
     }
     return (Vector(1) << 0.0).finished();
   }

gtsam_unstable/slam/Mechanization_bRn2.h

@@ -30,7 +30,7 @@ public:
    * @param r3 Z-axis of rotated frame
    */
   Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
-      const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) :
+      const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) :
       bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
   }
 

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -192,7 +192,7 @@ namespace gtsam {
         if (throwCheirality_)
           throw e;
       }
-      return ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
 
     /** return the measurements */

gtsam_unstable/slam/PartialPriorFactor.h

@@ -113,7 +113,7 @@ namespace gtsam {
       // FIXME: this was originally the generic retraction - may not produce same results
       Vector full_logmap = T::Logmap(p);
 //      Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation
-      Vector masked_logmap = zero(this->dim());
+      Vector masked_logmap = Vector::Zero(this->dim());
       for (size_t i=0; i<mask_.size(); ++i)
         masked_logmap(i) = full_logmap(mask_[i]);
       return masked_logmap - prior_;

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -146,7 +146,7 @@ namespace gtsam {
         if (throwCheirality_)
           throw e;
       }
-      return ones(2) * 2.0 * K_->fx();
+      return Vector::Ones(2) * 2.0 * K_->fx();
     }
 
     /** return the measurement */

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -142,7 +142,7 @@ namespace gtsam {
         if (throwCheirality_)
           throw e;
       }
-      return ones(2) * 2.0 * K.fx();
+      return Vector::Ones(2) * 2.0 * K.fx();
     }
 
     /** return the measurement */

gtsam_unstable/slam/SmartRangeFactor.h

@@ -142,9 +142,9 @@ public:
         // set Jacobians to zero for n<3
         for (size_t j = 0; j < n; j++)
           (*H)[j] = Matrix::Zero(3, 1);
-      return zero(1);
+      return Z_1x1;
     } else {
-      Vector error = zero(1);
+      Vector error = Z_1x1;
 
       // triangulate to get the optimized point
       // TODO: Should we have a (better?) variant that does this in relative coordinates ?

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -342,7 +342,7 @@ public:
       BOOST_FOREACH(Matrix& m, Gs)
         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);
     }
@@ -528,7 +528,7 @@ public:
     if (nonDegenerate)
       return Base::unwhitenedError(cameras, *result_);
     else
-      return zero(cameras.size() * Base::ZDim);
+      return Vector::Zero(cameras.size() * Base::ZDim);
   }
 
   /**

gtsam_unstable/slam/tests/testDummyFactor.cpp

@@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) {
   CHECK(actLinearization);
   noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
   GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
-      key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3));
+      key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3));
   EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
 }
 

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -152,7 +152,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
@@ -185,7 +185,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
@@ -225,7 +225,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   // TODO: Expected values need to be updated for global velocity version
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
@@ -488,7 +488,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
@@ -529,7 +529,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
@@ -579,7 +579,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
-  Vector ExpectedErr(zero(9));
+  Vector ExpectedErr(Z_9x1);
 
   // TODO: Expected values need to be updated for global velocity version
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));

gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp

@@ -34,7 +34,7 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
   double measured = 2.0;
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
-  EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
+  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
   Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
       boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
   Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
@@ -79,7 +79,7 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
   double measured = 2.0;
   RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
   Matrix actH1, actH2;
-  EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
+  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
   Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
       boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
   Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(

gtsam_unstable/slam/tests/testSerialization.cpp

@@ -48,9 +48,9 @@ Values exampleValues() {
 
 NonlinearFactorGraph exampleGraph() {
   NonlinearFactorGraph graph;
-  graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
+  graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
-  graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
+  graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
-  graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2))));
+  graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
   return graph;
 }
 

gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp

@@ -97,7 +97,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError)
   Vector err = g.unwhitenedError(values);
 
   // Equals
-  CHECK(assert_equal(err, zero(3), 1e-5));
+  CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -131,7 +131,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2)
   Vector err = g.unwhitenedError(values);
 
   // Equals
-  CHECK(assert_equal(err, zero(3), 1e-5));
+  CHECK(assert_equal(err, Z_3x1, 1e-5));
 }
 
 /* ************************************************************************* */

gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp

@@ -108,7 +108,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
   Vector err = g.unwhitenedError(values);
 
   // Equals
-  CHECK(assert_equal(err, zero(3), 1e-5));
+  CHECK(assert_equal(err, Z_3x1, 1e-5));
 }
 
 /* ************************************************************************* */
@@ -147,7 +147,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
   Vector err = g.unwhitenedError(values);
 
   // Equals
-  CHECK(assert_equal(err, zero(3), 1e-5));
+  CHECK(assert_equal(err, Z_3x1, 1e-5));
 }
 
 /* ************************************************************************* */

tests/smallExample.h

@@ -260,9 +260,9 @@ inline VectorValues createZeroDelta() {
   using symbol_shorthand::X;
   using symbol_shorthand::L;
   VectorValues c;
-  c.insert(L(1), zero(2));
+  c.insert(L(1), Z_2x1);
-  c.insert(X(1), zero(2));
+  c.insert(X(1), Z_2x1);
-  c.insert(X(2), zero(2));
+  c.insert(X(2), Z_2x1);
   return c;
 }
 
@@ -274,7 +274,7 @@ inline GaussianFactorGraph createGaussianFactorGraph() {
   GaussianFactorGraph fg;
 
   // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
-  fg += JacobianFactor(X(1), 10*I_2x2, -1.0*ones(2));
+  fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2));
 
   // odometry between x1 and x2: x2-x1=[0.2;-0.1]
   fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));

tests/testBoundingConstraint.cpp

@@ -54,10 +54,10 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) {
   EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
   EXPECT(constraint1.isGreaterThan());
   EXPECT(constraint2.isGreaterThan());
-  EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol));
+  EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol));
-  EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol));
+  EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol));
-  EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol));
+  EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol));
-  EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol));
+  EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol));
   EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
   EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
 }
@@ -75,8 +75,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) {
   EXPECT(!constraint4.isGreaterThan());
   EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol));
   EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol));
-  EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol));
+  EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol));
-  EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol));
+  EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol));
   EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
   EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol);
 }
@@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
   config.insert(key, pt1);
   EXPECT(constraint3.active(config));
   EXPECT(constraint4.active(config));
-  EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol));
+  EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
-  EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
+  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
-  EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
+  EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
-  EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
+  EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
   EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
   EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
 }
@@ -188,32 +188,32 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
   EXPECT(rangeBound.dim() == 1);
 
   EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1)));
-  EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
+  EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2)));
-  EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
+  EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3)));
-  EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
+  EXPECT(assert_equal(-1.0*I_1x1, rangeBound.evaluateError(pt1, pt4)));
 
   Values config1;
   config1.insert(key1, pt1);
   config1.insert(key2, pt1);
   EXPECT(!rangeBound.active(config1));
-  EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
+  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
   EXPECT(!rangeBound.linearize(config1));
   EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
 
   config1.update(key2, pt2);
   EXPECT(!rangeBound.active(config1));
-  EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
+  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
   EXPECT(!rangeBound.linearize(config1));
   EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
 
   config1.update(key2, pt3);
   EXPECT(rangeBound.active(config1));
-  EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
+  EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
   EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
 
   config1.update(key2, pt4);
   EXPECT(rangeBound.active(config1));
-  EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
+  EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1)));
   EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
 }
 

tests/testExpressionFactor.cpp

@@ -338,7 +338,7 @@ TEST(ExpressionFactor, Compose1) {
   EXPECT( assert_equal(I_3x3, H[1],1e-9));
 
   // Check linearization
-  JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3));
+  JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
@@ -367,7 +367,7 @@ TEST(ExpressionFactor, compose2) {
   EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
 
   // Check linearization
-  JacobianFactor expected(1, 2 * I_3x3, zero(3));
+  JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
@@ -396,7 +396,7 @@ TEST(ExpressionFactor, compose3) {
   EXPECT( assert_equal(I_3x3, H[0],1e-9));
 
   // Check linearization
-  JacobianFactor expected(3, I_3x3, zero(3));
+  JacobianFactor expected(3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
@@ -441,7 +441,7 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(I_3x3, H[2],1e-9));
 
   // Check linearization
-  JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3));
+  JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);

tests/testGaussianBayesTreeB.cpp

@@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
   double sigma3 = 0.61808;
   Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
   GaussianBayesNet expected3;
-  expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3);
+  expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
   GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
   GaussianBayesNet actual3 = C3->shortcut(R);
   EXPECT(assert_equal(expected3,actual3,tol));
@@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
   double sigma4 = 0.661968;
   Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
   GaussianBayesNet expected4;
-  expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4);
+  expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
   GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
   GaussianBayesNet actual4 = C4->shortcut(R);
   EXPECT(assert_equal(expected4,actual4,tol));
@@ -132,7 +132,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
   double tol=1e-5;
 
   // Check marginal on x1
-  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1);
+  JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
   JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
   Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
   Matrix actualCovarianceX1;
@@ -143,22 +143,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
 
   // Check marginal on x2
   double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
-  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2);
+  JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
   JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
   EXPECT(assert_equal(expected2,actual2,tol));
 
   // Check marginal on x3
-  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3);
+  JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
   JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
   EXPECT(assert_equal(expected3,actual3,tol));
 
   // Check marginal on x4
-  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4);
+  JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
   JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
   EXPECT(assert_equal(expected4,actual4,tol));
 
   // Check marginal on x7 (should be equal to x1)
-  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7);
+  JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
   JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
   EXPECT(assert_equal(expected7,actual7,tol));
 }
@@ -212,8 +212,8 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts )
 //
 //  // Check the clique marginal P(C3)
 //  double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
-//  GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
+//  GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt);
-//  push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
+//  push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
 //  GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
 //  GaussianFactorGraph marginal = C3->marginal(R);
 //  GaussianVariableIndex varIndex(marginal);
@@ -248,17 +248,17 @@ TEST( GaussianBayesTree, balanced_smoother_joint )
   // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
   GaussianBayesNet expected1 = list_of
     // Why does the sign get flipped on the prior?
-    (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7))
+    (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7))
-    (GaussianConditional(X(7), zero(2), -1*I/sigmax7));
+    (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7));
   GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
   EXPECT(assert_equal(expected1, actual1, tol));
 
   //  // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
   //  GaussianBayesNet expected2;
   //  GaussianConditional::shared_ptr
-  //      parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2)));
+  //      parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2)));
   //    expected2.push_front(parent2);
-  //  push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma);
+  //  push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma);
   //  GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
   //  EXPECT(assert_equal(expected2,actual2,tol));
 
@@ -266,19 +266,19 @@ TEST( GaussianBayesTree, balanced_smoother_joint )
   double sig14 = 0.784465;
   Matrix A14 = -0.0769231*I;
   GaussianBayesNet expected3 = list_of
-    (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14))
+    (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14))
-    (GaussianConditional(X(4), zero(2), I/sigmax4));
+    (GaussianConditional(X(4), Z_2x1, I/sigmax4));
   GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
   EXPECT(assert_equal(expected3,actual3,tol));
 
   //  // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
   //  GaussianBayesNet expected4;
   //  GaussianConditional::shared_ptr
-  //      parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2)));
+  //      parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2)));
   //    expected4.push_front(parent4);
   //  double sig41 = 0.668096;
   //  Matrix A41 = -0.055794*I;
-  //  push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma);
+  //  push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma);
   //  GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
   //  EXPECT(assert_equal(expected4,actual4,tol));
 }

tests/testGaussianFactorGraphB.cpp

@@ -97,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
   // create expected Conditional Gaussian
   double sig = 0.0894427;
   Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-  Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
+  Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2);
   GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
 
   EXPECT(assert_equal(expected,*actual,tol));
@@ -113,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
   // create expected Conditional Gaussian
   double sig = sqrt(2.0)/10.;
   Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
-  Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
+  Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2);
   GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
 
   EXPECT(assert_equal(expected,*actual,tol));
@@ -130,7 +130,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
 
   // create expected Conditional Gaussian
   Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
-  Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2);
+  Vector d = Vector2(-0.133333, -0.0222222), sigma = Vector::Ones(2);
   GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
 
   // Create expected remaining new factor
@@ -160,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast )
   // create expected Conditional Gaussian
   double sig = 0.0894427;
   Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-  Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
+  Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2);
   GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
 
   EXPECT(assert_equal(expected,*actual,tol));
@@ -176,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
   // create expected Conditional Gaussian
   double sig = sqrt(2.0)/10.;
   Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
-  Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
+  Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2);
   GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
 
   EXPECT(assert_equal(expected,*actual,tol));
@@ -195,11 +195,11 @@ TEST( GaussianFactorGraph, eliminateAll )
   GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);
 
   double sig1 = 0.149071;
-  Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = ones(2);
+  Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2);
   push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2);
 
   double sig2 = 0.0894427;
-  Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = ones(2);
+  Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2);
   push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);
 
   // Check one ordering
@@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace)
   SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
 
   GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
-      ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise));
+      ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise));
   GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
-      ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise));
+      ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise));
   GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
-      ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise));
+      ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise));
   GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
-      ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise));
+      ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise));
 
   GaussianFactorGraph actual;
   actual.push_back(f1);
@@ -588,7 +588,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
   BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) {
     GaussianConditional::shared_ptr conditional = clique->conditional();
     //size_t dim = conditional->rows();
-    //EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol));
+    //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol));
     EXPECT(!conditional->get_model());
   }
 }

tests/testIterative.cpp

@@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent )
   // get matrices
   Matrix A;
   Vector b;
-  Vector x0 = gtsam::zero(6);
+  Vector x0 = Z_6x1;
   boost::tie(A, b) = fg.jacobian();
   Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished();
 
@@ -104,7 +104,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
   VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
 
   VectorValues expected;
-  expected.insert(X(1), zero(3));
+  expected.insert(X(1), Z_3x1);
   expected.insert(X(2), Vector3(-0.5,0.,0.));
   CHECK(assert_equal(expected, actual));
 }
@@ -131,7 +131,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
   VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
 
   VectorValues expected;
-  expected.insert(X(1), zero(3));
+  expected.insert(X(1), Z_3x1);
   expected.insert(X(2), Vector3(-0.5,0.,0.));
   CHECK(assert_equal(expected, actual));
 }

tests/testManifold.cpp

@@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) {
   EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
 
   // Dynamic does not work yet !
-  Vector z = zero(2), v(2);
+  Vector z = Z_2x1, v(2);
   v << 1, 0;
   //DefaultChart<Vector> chart4;
 //  EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
@@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) {
   EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
   // Check zero vector
   //DefaultChart<Rot3> chart6;
-  EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
+  EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
 }
 
 //******************************************************************************

tests/testNonlinearEquality.cpp

@@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) {
 
   // check linearize
   SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
-  JacobianFactor expLF(key, I_3x3, zero(3), constraintModel);
+  JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel);
   GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
   EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
 }
@@ -133,7 +133,7 @@ TEST ( NonlinearEquality, error ) {
 
   // check error function outputs
   Vector actual = nle->unwhitenedError(feasible);
-  EXPECT(assert_equal(actual, zero(3)));
+  EXPECT(assert_equal(actual, Z_3x1));
 
   actual = nle->unwhitenedError(bad_linearize);
   EXPECT(
@@ -263,8 +263,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
   Values config1;
   config1.insert(key, pt);
   EXPECT(constraint.active(config1));
-  EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol));
+  EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol));
-  EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
+  EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
   EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
 
   Values config2;
@@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
   config1.insert(key, pt);
   GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
   GaussianFactor::shared_ptr expected1(
-      new JacobianFactor(key, I_2x2, zero(2), hard_model));
+      new JacobianFactor(key, I_2x2, Z_2x1, hard_model));
   EXPECT(assert_equal(*expected1, *actual1, tol));
 
   Values config2;
@@ -345,8 +345,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
   config1.insert(key1, x1);
   config1.insert(key2, x2);
   EXPECT(constraint.active(config1));
-  EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol));
+  EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol));
-  EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
+  EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
   EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
 
   Values config2;
@@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
   config1.insert(key2, x2);
   GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
   GaussianFactor::shared_ptr expected1(
-      new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2),
+      new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1,
           hard_model));
   EXPECT(assert_equal(*expected1, *actual1, tol));
 

tests/testNonlinearFactor.cpp

@@ -91,7 +91,7 @@ TEST( NonlinearFactor, NonlinearFactor )
   // calculate the error_vector from the factor "f1"
   // error_vector = [0.1 0.1]
   Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
-  CHECK(assert_equal(0.1*ones(2),actual_e));
+  CHECK(assert_equal(0.1*Vector::Ones(2),actual_e));
 
   // error = 0.5 * [1 1] * [1;1] = 1
   double expected = 1.0;

tests/testNonlinearOptimizer.cpp

@@ -257,9 +257,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
   expected.insert(2, Pose2(1, 1, M_PI));
 
   VectorValues expectedGradient;
-  expectedGradient.insert(0,zero(3));
+  expectedGradient.insert(0,Z_3x1);
-  expectedGradient.insert(1,zero(3));
+  expectedGradient.insert(1,Z_3x1);
-  expectedGradient.insert(2,zero(3));
+  expectedGradient.insert(2,Z_3x1);
 
   // Try LM and Dogleg
   LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults();

timing/timeSchurFactors.cpp

@@ -109,7 +109,7 @@ void timeAll(size_t m, size_t N) {
     double* xdata = x.data();
 
     // create a y
-    Vector y = zero(m * D);
+    Vector y = Vector::Zero(m * D);
     TIME(RawImplicit, implicitFactor, xdata, y.data())
     TIME(RawJacobianQ, jf, xdata, y.data())
     TIME(RawJacobianQR, jqr, xdata, y.data())