Work in progress: deprecating inline functions.

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

@@ -33,7 +33,6 @@
 #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,38 +73,6 @@ 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
- *
- * 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
  */
@@ -292,8 +259,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
 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
@@ -492,12 +457,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 +562,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/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 = Matrix::Zero(3,3);
   ad(0,1) = -v[2];
   ad(1,0) = v[2];
   ad(0,2) = v[1];

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/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.h

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

gtsam/slam/InitializePose3.cpp

@@ -32,7 +32,7 @@ namespace gtsam {
 namespace InitializePose3 {
 
 //static const Matrix I = eye(1);
-static const Matrix I9 = eye(9);
+static const Matrix I9 = Matrix::Identity(9,9);
 static const Vector zero9 = Vector::Zero(9);
 static const Matrix zero33= Matrix::Zero(3,3);
 

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/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 = Matrix::Identity(6,6);
       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 = Matrix::Identity(6,6);
       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 = Matrix::Identity(6,6);
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
@@ -563,7 +563,7 @@ GraphAndValues load3D(const string& filename) {
           m(j, i) = mij;
         }
       }
-      Matrix mgtsam = eye(6);
+      Matrix mgtsam = Matrix::Identity(6,6);
       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(0,3,3,3) = m.block(0,3,3,3); // off diagonal

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 = Matrix::Identity(1,1);
-static const Matrix I3 = eye(3);
+static const Matrix I3 = Matrix::Identity(3,3);
 
 static const Key keyAnchor = symbol('Z', 9999999);
 static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =