diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1e5da7d86..ff509b41e 100644 --- a/gtsam/base/Matrix.cpp +++ b/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& 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) { 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 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 qr(const Matrix& A) { v(k) = k llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(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 llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(inv); return inv.transpose(); } @@ -648,7 +582,7 @@ boost::tuple 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; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index b0b292c56..84f201c51 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -33,7 +33,6 @@ #include #include - /** * 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 SubMatrix; typedef Eigen::Block 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& 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 diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index aa023a09a..514328483 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/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; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 1aa8f060a..383c94dd1 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/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]; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 7ec9edbb3..a1dc3269a 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/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; diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 63271401c..7db29d861 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/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)) { } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 15fad5a78..d0c7a58db 100644 --- a/gtsam/linear/NoiseModel.h +++ b/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: diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b93fe1fc1..7d7a86405 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/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); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8305fce12..9a3a4a47a 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -86,7 +86,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::GetDimension(x)); + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 14192fcaa..c1c919a58 100644 --- a/gtsam/slam/dataset.cpp +++ b/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 diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index e46b8ee71..a29d3e8b1 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { namespace lago { -static const Matrix I = eye(1); -static const Matrix I3 = eye(3); +static const Matrix I = Matrix::Identity(1,1); +static const Matrix I3 = Matrix::Identity(3,3); static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =