diff --git a/gtsam.h b/gtsam.h index c5424ef80..c12055916 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1798,9 +1798,6 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -1818,12 +1815,6 @@ class Values { template T at(size_t j); - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// Fixed size versions, for n in 1..9 - Vector atFixed(size_t j, size_t n); - /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2489,10 +2480,30 @@ class NavState { gtsam::Pose3 pose() const; }; +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; + #include -class PreintegrationParams { +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); - // TODO(frank): add setters/getters or make this MATLAB wrapper feature + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; }; #include diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d92be12f5..a9d521a7f 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 > results; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once + Vector weights = sigmas.array().square().inverse(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -356,7 +290,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { if (precision < 1e-8) continue; // create solution and copy into r - Vector r(basis(n, j)); + Vector r(Vector::Unit(n,j)); for (size_t j2=j+1; j2 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..235cd30f3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -16,6 +16,7 @@ * @author Kai Ni * @author Frank Dellaert * @author Alex Cunningham + * @author Alex Hagiopol */ // \callgraph @@ -23,17 +24,17 @@ #pragma once #include #include -#include // Configuration from CMake - +#include +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #include #include #include +#endif #include #include #include #include - /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping @@ -74,40 +75,8 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block 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 + * equals with a tolerance */ template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& B, double tol = 1e-9) { @@ -166,37 +135,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to */ GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9); -/** - * BLAS Level-2 style e <- e + alpha*A*x - */ -GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e); - -/** - * BLAS Level-2 style e <- e + A*x - */ -GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e); - /** * overload ^ for trans(A)*v * We transpose the vectors for speed. */ GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v); -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x); - /** products using old-style format to improve compatibility */ template inline MATRIX prod(const MATRIX& A, const MATRIX&B) { @@ -281,19 +225,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { return A.row(j); } -/** - * inserts a column into a matrix IN PLACE - * NOTE: there is no size checking - * Alternate form allows for vectors smaller than the whole column to be inserted - * @param A matrix to be modified in place - * @param col is the vector to be inserted - * @param j is the index to insert the column - */ -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j); -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); - -GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); - /** * Zeros all of the elements below the diagonal of a matrix, in place * @param A is a matrix, to be modified in place @@ -355,17 +286,6 @@ inline typename Reshape::ReshapedTy return Reshape::reshape(m); } -/** - * solve AX=B via in-place Lu factorization and backsubstitution - * After calling, A contains LU, B the solved RHS vectors - */ -GTSAM_EXPORT void solve(Matrix& A, Matrix& B); - -/** - * invert A - */ -GTSAM_EXPORT Matrix inverse(const Matrix& A); - /** * QR factorization, inefficient, best use imperative householder below * m*n matrix -> m*m Q, m*n R @@ -492,12 +412,6 @@ inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); -/** Calculate the LL^t decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix LLt(const Matrix& A); - -/** Calculate the R^tR decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix RtR(const Matrix& A); - /** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */ GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A); @@ -603,6 +517,28 @@ struct MultiplyWithInverseFunction { const Operator phi_; }; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); } +inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); } +inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); } +inline Matrix eye( size_t m ) { return eye(m,m); } +inline Matrix diag(const Vector& v) { return v.asDiagonal(); } +inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; } +inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; } +inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; } +inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; } +inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; } +inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); } +inline Matrix inverse(const Matrix& A) { return A.inverse(); } +#endif + +GTSAM_EXPORT Matrix LLt(const Matrix& A); + +GTSAM_EXPORT Matrix RtR(const Matrix& A); + +GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); } // namespace gtsam #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 61a42fead..40d819de9 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -33,25 +33,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -bool zero(const Vector& v) { - bool result = true; - size_t n = v.size(); - for( size_t j = 0 ; j < n ; j++) - result = result && (v(j) == 0.0); - return result; -} - -/* ************************************************************************* */ -Vector repeat(size_t n, double value) { - return Vector::Constant(n, value); -} - -/* ************************************************************************* */ -Vector delta(size_t n, size_t i, double value) { - return Vector::Unit(n, i) * value; -} - /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { @@ -176,28 +157,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { return flag; } -/* ************************************************************************* */ -ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { - return v.segment(i1,i2-i1); -} - -/* ************************************************************************* */ -void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { - fullVector.segment(i, subVector.size()) = subVector; -} - -/* ************************************************************************* */ -Vector emul(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseProduct(b); -} - -/* ************************************************************************* */ -Vector ediv(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseQuotient(b); -} - /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { size_t n = a.size(); @@ -210,36 +169,6 @@ Vector ediv_(const Vector &a, const Vector &b) { return c; } -/* ************************************************************************* */ -double sum(const Vector &a) { - return a.sum(); -} - -/* ************************************************************************* */ -double norm_2(const Vector& v) { - return v.norm(); -} - -/* ************************************************************************* */ -Vector reciprocal(const Vector &a) { - return a.array().inverse(); -} - -/* ************************************************************************* */ -Vector esqrt(const Vector& v) { - return v.cwiseSqrt(); -} - -/* ************************************************************************* */ -Vector abs(const Vector& v) { - return v.cwiseAbs(); -} - -/* ************************************************************************* */ -double max(const Vector &a) { - return a.maxCoeff(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { @@ -292,7 +221,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Basically, instead of doing a normal QR step with the weighted // pseudoinverse, we enforce the constraint by turning // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 - pseudo = delta(m, i, 1.0 / a[i]); + pseudo = Vector::Unit(m,i)*(1.0/a[i]); return inf; } } @@ -363,6 +292,4 @@ Vector concatVectors(size_t nrVectors, ...) return concatVectors(vs); } -/* ************************************************************************* */ - } // namespace gtsam diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f87703e2b..91aec85b8 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -14,13 +14,12 @@ * @brief typedef and functions to augment Eigen's VectorXd * @author Kai Ni * @author Frank Dellaert + * @author Alex Hagiopol */ // \callgraph - #pragma once - #ifndef MKL_BLAS #define MKL_BLAS MKL_DOMAIN_BLAS #endif @@ -64,54 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * Create vector initialized to a constant value - * @param n is the size of the vector - * @param value is a constant value to insert into the vector - */ -GTSAM_EXPORT Vector repeat(size_t n, double value); - -/** - * Create basis vector of dimension n, - * with a constant in spot i - * @param n is the size of the vector - * @param i index of the one - * @param value is the value to insert into the vector - * @return delta vector - */ -GTSAM_EXPORT Vector delta(size_t n, size_t i, double value); - -/** - * Create basis vector of dimension n, - * with one in spot i - * @param n is the size of the vector - * @param i index of the one - * @return basis vector - */ -inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } - -/** - * Create zero vector - * @param n size - */ -inline Vector zero(size_t n) { return Vector::Zero(n);} - -/** - * Create vector initialized to ones - * @param n size - */ -inline Vector ones(size_t n) { return Vector::Ones(n); } - -/** - * check if all zero - */ -GTSAM_EXPORT bool zero(const Vector& v); - -/** - * dimensionality == size - */ -inline size_t dim(const Vector& v) { return v.size(); } - /** * print without optional string, must specify cout yourself */ @@ -196,39 +147,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& */ GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9); -/** - * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2 - * @param v Vector - * @param i1 first row index - * @param i2 last row index + 1 - * @return subvector v(i1:i2) - */ -GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); - -/** - * Inserts a subvector into a vector IN PLACE - * @param fullVector is the vector to be changed - * @param subVector is the vector to insert - * @param i is the index where the subvector should be inserted - */ -GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); - -/** - * elementwise multiplication - * @param a first vector - * @param b second vector - * @return vector [a(i)*b(i)] - */ -GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b); - -/** - * elementwise division - * @param a first vector - * @param b second vector - * @return vector [a(i)/b(i)] - */ -GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); - /** * elementwise division, but 0/0 = 0, not inf * @param a first vector @@ -237,49 +155,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); */ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); -/** - * sum vector elements - * @param a vector - * @return sum_i a(i) - */ -GTSAM_EXPORT double sum(const Vector &a); - -/** - * Calculates L2 norm for a vector - * modeled after boost.ublas for compatibility - * @param v vector - * @return the L2 norm - */ -GTSAM_EXPORT double norm_2(const Vector& v); - -/** - * Elementwise reciprocal of vector elements - * @param a vector - * @return [1/a(i)] - */ -GTSAM_EXPORT Vector reciprocal(const Vector &a); - -/** - * Elementwise sqrt of vector elements - * @param v is a vector - * @return [sqrt(a(i))] - */ -GTSAM_EXPORT Vector esqrt(const Vector& v); - -/** - * Absolute values of vector elements - * @param v is a vector - * @return [abs(a(i))] - */ -GTSAM_EXPORT Vector abs(const Vector& v); - -/** - * Return the max element of a vector - * @param a is a vector - * @return max(a) - */ -GTSAM_EXPORT double max(const Vector &a); - /** * Dot product */ @@ -356,6 +231,25 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +inline Vector abs(const Vector& v){return v.cwiseAbs();} +inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); } +inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} +inline size_t dim(const Vector& v) { return v.size(); } +inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} +inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} +inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +inline double max(const Vector &a){return a.maxCoeff();} +inline double norm_2(const Vector& v) {return v.norm();} +inline Vector ones(size_t n) { return Vector::Ones(n); } +inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} +inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} +inline double sum(const Vector &a){return a.sum();} +inline bool zero(const Vector& v){ return v.isZero(); } +inline Vector zero(size_t n) { return Vector::Zero(n); } +#endif } // namespace gtsam #include diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9a42db3ce..6cd28b951 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function g; g.setZero(); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(traits::Retract(x, d)); @@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct dx.setZero(); // Fill in Jacobian H - Matrix H = zeros(m, N); + Matrix H = Matrix::Zero(m, N); const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 13d32794e..093f22961 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -156,8 +156,8 @@ TEST(Matrix, collect2 ) TEST(Matrix, collect3 ) { Matrix A, B; - A = eye(2, 3); - B = eye(2, 3); + A = Matrix::Identity(2,3); + B = Matrix::Identity(2,3); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -211,48 +211,6 @@ TEST(Matrix, column ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, insert_column ) -{ - Matrix big = zeros(5, 6); - Vector col = ones(5); - size_t j = 3; - - insertColumn(big, col, j); - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - -/* ************************************************************************* */ -TEST(Matrix, insert_subcolumn ) -{ - Matrix big = zeros(5, 6); - Vector col1 = ones(2); - size_t i = 1; - size_t j = 3; - - insertColumn(big, col1, i, j); // check 1 - - Vector col2 = ones(1); - insertColumn(big, col2, 4, 5); // check 2 - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - /* ************************************************************************* */ TEST(Matrix, row ) { @@ -272,26 +230,10 @@ TEST(Matrix, row ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, zeros ) -{ - Matrix A(2, 3); - A(0, 0) = 0; - A(0, 1) = 0; - A(0, 2) = 0; - A(1, 0) = 0; - A(1, 1) = 0; - A(1, 2) = 0; - - Matrix zero = zeros(2, 3); - - EQUALITY(A , zero); -} - /* ************************************************************************* */ TEST(Matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, + Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); insertSub(big, small, 1, 2); @@ -307,9 +249,9 @@ TEST(Matrix, insert_sub ) TEST(Matrix, diagMatrices ) { std::vector Hs; - Hs.push_back(ones(3,3)); - Hs.push_back(ones(4,4)*2); - Hs.push_back(ones(2,2)*3); + Hs.push_back(Matrix::Ones(3,3)); + Hs.push_back(Matrix::Ones(4,4)*2); + Hs.push_back(Matrix::Ones(2,2)*3); Matrix actual = diag(Hs); @@ -723,9 +665,9 @@ TEST(Matrix, inverse ) A(2, 1) = 0; A(2, 2) = 6; - Matrix Ainv = inverse(A); - EXPECT(assert_equal(eye(3), A*Ainv)); - EXPECT(assert_equal(eye(3), Ainv*A)); + Matrix Ainv = A.inverse(); + EXPECT(assert_equal((Matrix) I_3x3, A*Ainv)); + EXPECT(assert_equal((Matrix) I_3x3, Ainv*A)); Matrix expected(3, 3); expected(0, 0) = 1.0909; @@ -746,13 +688,13 @@ TEST(Matrix, inverse ) 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished(), - inverse(lMg))); + lMg.inverse())); Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished()); EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished(), - inverse(gMl))); + gMl.inverse())); } /* ************************************************************************* */ @@ -769,7 +711,7 @@ TEST(Matrix, inverse2 ) A(2, 1) = 0; A(2, 2) = 1; - Matrix Ainv = inverse(A); + Matrix Ainv = A.inverse(); Matrix expected(3, 3); expected(0, 0) = 0; @@ -996,7 +938,7 @@ TEST(Matrix, inverse_square_root ) 10.0).finished(); EQUALITY(expected,actual); - EQUALITY(measurement_covariance,inverse(actual*actual)); + EQUALITY(measurement_covariance,(actual*actual).inverse()); // Randomly generated test. This test really requires inverse to // be working well; if it's not, there's the possibility of a @@ -1052,28 +994,6 @@ TEST(Matrix, cholesky_inverse ) EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } -/* ************************************************************************* */ -TEST(Matrix, multiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = e + A * x; - - multiplyAdd(1, A, x, e); - EXPECT(assert_equal(expected, e)); -} - -/* ************************************************************************* */ -TEST(Matrix, transposeMultiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = x + trans(A) * e; - - transposeMultiplyAdd(1, A, e, x); - EXPECT(assert_equal(expected, x)); -} - /* ************************************************************************* */ TEST(Matrix, linear_dependent ) { @@ -1102,12 +1022,12 @@ TEST(Matrix, linear_dependent3 ) TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); - Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) + Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; Vector s; svd(A, U, s, V); - Matrix S = diag(s); + Matrix S = s.asDiagonal(); EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); EXPECT(assert_equal(S,S1)); } @@ -1158,7 +1078,7 @@ TEST(Matrix, svd3 ) V = -V; } - Matrix S = diag(s); + Matrix S = s.asDiagonal(); Matrix t = U * S; Matrix Vt = trans(V); @@ -1202,7 +1122,7 @@ TEST(Matrix, svd4 ) V.col(1) = -V.col(1); } - Matrix reconstructed = U * diag(s) * trans(V); + Matrix reconstructed = U * s.asDiagonal() * trans(V); EXPECT(assert_equal(A, reconstructed, 1e-4)); EXPECT(assert_equal(expectedU,U, 1e-3)); diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index df3403cf4..da193aec5 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) 23, 29).finished(); Matrix actual1 = testBlockMatrix(1, 1); // Test only writing the upper triangle for efficiency - Matrix actual1t = Matrix::Zero(2, 2); + Matrix actual1t = Z_2x2; actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 00e40039b..8a54d5469 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -79,22 +79,6 @@ TEST(Vector, copy ) EXPECT(assert_equal(a, b)); } -/* ************************************************************************* */ -TEST(Vector, zero1 ) -{ - Vector v = Vector::Zero(2); - EXPECT(zero(v)); -} - -/* ************************************************************************* */ -TEST(Vector, zero2 ) -{ - Vector a = zero(2); - Vector b = Vector::Zero(2); - EXPECT(a==b); - EXPECT(assert_equal(a, b)); -} - /* ************************************************************************* */ TEST(Vector, scalar_multiply ) { @@ -119,36 +103,6 @@ TEST(Vector, negate ) EXPECT(assert_equal(b, -a)); } -/* ************************************************************************* */ -TEST(Vector, sub ) -{ - Vector a(6); - a(0) = 10; a(1) = 20; a(2) = 3; - a(3) = 34; a(4) = 11; a(5) = 2; - - Vector result(sub(a,2,5)); - - Vector b(3); - b(0) = 3; b(1) = 34; b(2) =11; - - EXPECT(b==result); - EXPECT(assert_equal(b, result)); -} - -/* ************************************************************************* */ -TEST(Vector, subInsert ) -{ - Vector big = zero(6), - small = ones(3); - - size_t i = 2; - subInsert(big, small, i); - - Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - /* ************************************************************************* */ TEST(Vector, householder ) { @@ -198,7 +152,7 @@ TEST(Vector, weightedPseudoinverse ) // create sigmas Vector sigmas(2); sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector weights = sigmas.array().square().inverse(); // perform solve Vector actual; double precision; @@ -224,8 +178,7 @@ TEST(Vector, weightedPseudoinverse_constraint ) // create sigmas Vector sigmas(2); sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); - + Vector weights = sigmas.array().square().inverse(); // perform solve Vector actual; double precision; boost::tie(actual, precision) = weightedPseudoinverse(x, weights); @@ -244,7 +197,7 @@ TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); - Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector weights = sigmas.array().square().inverse(); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); @@ -253,17 +206,6 @@ TEST(Vector, weightedPseudoinverse_nan ) DOUBLES_EQUAL(100, precision, 1e-5); } -/* ************************************************************************* */ -TEST(Vector, ediv ) -{ - Vector a = Vector3(10., 20., 30.); - Vector b = Vector3(2.0, 5.0, 6.0); - Vector actual(ediv(a,b)); - - Vector c = Vector3(5.0, 4.0, 5.0); - EXPECT(assert_equal(c,actual)); -} - /* ************************************************************************* */ TEST(Vector, dot ) { @@ -298,18 +240,11 @@ TEST(Vector, equals ) TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), - v2 = zero(3); + v2 = Z_3x1; EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals } -/* ************************************************************************* */ -TEST(Vector, reciprocal ) -{ - Vector v = Vector3(1.0, 2.0, 4.0); - EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); -} - /* ************************************************************************* */ TEST(Vector, linear_dependent ) { diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 24bec4fa1..4e9f956b6 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { // Calculate the marginals by brute force vector 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); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index aa023a09a..4af0257ec 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..8a0e8fbf5 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 = Z_3x3; ad(0,1) = -v[2]; ad(1,0) = v[2]; ad(0,2) = v[1]; diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 41d56b6e3..9780cb49a 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/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)); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 198cd5862..65dd5f609 100644 --- a/gtsam/geometry/Rot2.h +++ b/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 diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 3e93dedc1..4ccb6075b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -122,8 +122,8 @@ TEST(Cal3_S2, between) { Matrix H1, H2; EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); - EXPECT(assert_equal(-eye(5), H1)); - EXPECT(assert_equal(eye(5), H2)); + EXPECT(assert_equal(-I_5x5, H1)); + EXPECT(assert_equal(I_5x5, H2)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 79759678d..75ee9427d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/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)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index d3a107570..b3d87f18c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/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); - Vector vector = zero(numDims); + size_t numDims = minLimits.size(); + 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 diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4fbdbcbe1..99dcb95bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index c5137e3b3..0d840de7e 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -87,7 +87,7 @@ TEST( PinholePose, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 6f8d45b3e..3a636b9bf 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -73,12 +73,12 @@ TEST(Point2, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(-I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index da7696d0d..9b6e53323 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -47,12 +47,12 @@ TEST(Point3, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(-I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5b835200a..d1f68fe28 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -102,7 +102,7 @@ TEST(Pose2, expmap3) { 0.99, 0.0, -0.015, 0.0, 0.0, 0.0).finished(); Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; - Matrix expected = eye(3) + A + A2 + A3 + A4; + Matrix expected = I_3x3 + A + A2 + A3 + A4; Vector v = Vector3(0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); @@ -311,7 +311,7 @@ TEST(Pose2, compose_a) -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 ).finished(); - Matrix expectedH2 = eye(3); + Matrix expectedH2 = I_3x3; Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(expectedH1,actualDcompose1)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1aba34bd6..97ccbcb34 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/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(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(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; @@ -148,12 +148,12 @@ TEST(Pose3, Adjoint_full) Pose3 Agrawal06iros(const Vector& xi) { Vector w = xi.head(3); Vector v = xi.tail(3); - double t = norm_2(w); + double t = w.norm(); if (t < 1e-5) return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); + Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); return Pose3(Rot3::Expmap (w), Point3(A * v)); } } @@ -267,7 +267,7 @@ TEST( Pose3, inverse) { Matrix actualDinverse; Matrix actual = T.inverse(actualDinverse).matrix(); - Matrix expected = inverse(T.matrix()); + Matrix expected = T.matrix().inverse(); EXPECT(assert_equal(actual,expected,1e-8)); Matrix numericalH = numericalDerivative11(testing::inverse, T); @@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2) TEST( Pose3, compose_inverse) { Matrix actual = (T*T.inverse()).matrix(); - Matrix expected = eye(4,4); + Matrix expected = I_4x4; EXPECT(assert_equal(actual,expected,1e-8)); } @@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates) /* ************************************************************************* */ TEST(Pose3, expmap_logmap) { - Vector d12 = repeat(6,0.1); + Vector d12 = Vector6::Constant(0.1); Pose3 t1 = T, t2 = t1.expmap(d12); EXPECT(assert_equal(d12, t1.logmap(t2))); } @@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) { TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame - Vector x_step = delta(6,3,1.0); + Vector x_step = Vector::Unit(6,3)*1.0; EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); @@ -723,9 +723,8 @@ TEST( Pose3, adjointMap) { Matrix res = Pose3::adjointMap(screwPose3::xi); Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); - Matrix Z3 = zeros(3,3); Matrix6 expected; - expected << wh, Z3, vh, wh; + expected << wh, Z_3x3, vh, wh; EXPECT(assert_equal(expected,res,1e-5)); } diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6ead22860..d6d1f3149 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -62,8 +62,8 @@ TEST( Rot2, compose) Matrix H1, H2; (void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ @@ -74,8 +74,8 @@ TEST( Rot2, between) Matrix H1, H2; (void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(-eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(-I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ @@ -89,7 +89,7 @@ TEST( Rot2, equals) /* ************************************************************************* */ TEST( Rot2, expmap) { - Vector v = zero(1); + Vector v = Z_1x1; CHECK(assert_equal(R.retract(v), R)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5364d72f6..5e72d4c5b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -96,7 +96,7 @@ TEST( Rot3, equals) /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { - double t = norm_2(w); + double t = w.norm(); Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); @@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2) TEST( Rot3, Rodrigues3) { Vector w = Vector3(0.1, 0.2, 0.3); - Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w)); + Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm()); Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } @@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4) /* ************************************************************************* */ TEST( Rot3, retract) { - Vector v = zero(3); + Vector v = Z_3x1; CHECK(assert_equal(R, R.retract(v))); // // test Canonical coordinates @@ -213,7 +213,7 @@ TEST(Rot3, log) #define CHECK_OMEGA_ZERO(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) @@ -224,16 +224,16 @@ TEST(Rot3, log) /* ************************************************************************* */ TEST(Rot3, retract_localCoordinates) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector3::Constant(0.1); Rot3 R2 = R.retract(d12); EXPECT(assert_equal(d12, R.localCoordinates(R2))); } /* ************************************************************************* */ TEST(Rot3, expmap_logmap) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector3::Constant(0.1); Rot3 R2 = R.expmap(d12); - EXPECT(assert_equal(d12, R.logmap(R2))); + EXPECT(assert_equal(d12, (Vector) R.logmap(R2))); } /* ************************************************************************* */ @@ -555,8 +555,8 @@ TEST(Rot3, quaternion) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { Matrix::Index n = A.cols(); - const Matrix I = eye(n); - return (I-A)*inverse(I+A); + const Matrix I = Matrix::Identity(n,n); + return (I-A)*(I+A).inverse(); } TEST( Rot3, Cayley ) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71dcabd4e..70b3069f2 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + CHECK(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 396c8b0f3..0e99268ee 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/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)); } { 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/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 15029b8ad..4e8ae2f17 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -363,6 +363,18 @@ struct TriangulationParameters { << p.dynamicOutlierRejectionThreshold << std::endl; return os; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(rankTolerance); + ar & BOOST_SERIALIZATION_NVP(enableEPI); + ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + } }; /** @@ -411,6 +423,15 @@ public: os << "no point, status = " << result.status_ << std::endl; return os; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(status_); + } }; /// triangulateSafe: extensive checking of the outcome diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index feb477a4e..eefb6302f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -183,7 +183,7 @@ namespace gtsam { if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); + gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas if(model_) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9277f3903..1c55d293b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, 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 diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fc1a7c841..7fecefe3c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; @@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, pair xi = x.tryInsert(j, Vector()); if (xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); - gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); - + xi.first->second += Ab_(pos).transpose()*E; } } @@ -595,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 68e6a00f1..c0d294adf 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = inverse(Q), Ft = trans(F); + Matrix M = Q.inverse(), Ft = trans(F); Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); @@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) const { Key k = step(p); - Matrix M = inverse(Q), Ht = trans(H); + Matrix M = Q.inverse(), Ht = trans(H); Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); 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.cpp b/gtsam/linear/NoiseModel.cpp index 15b2dcf81..5bcf3d635 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (variances(j) != variances(0)) goto full; return Isotropic::Variance(n, variances(0), true); } - full: return shared_ptr(new Diagonal(esqrt(variances))); + full: return shared_ptr(new Diagonal(variances.cwiseSqrt())); } /* ************************************************************************* */ @@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { /* ************************************************************************* */ Constrained::Constrained(const Vector& sigmas) - : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) { + : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) { internal::fix(sigmas, precisions_, invsigmas_); } @@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { /* ************************************************************************* */ Constrained::shared_ptr Constrained::unit() const { - Vector sigmas = ones(dim()); + Vector sigmas = Vector::Ones(dim()); for (size_t i=0; i actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(1*eye(3),actualBD[5])); - EXPECT(assert_equal(4*eye(3),actualBD[10])); - EXPECT(assert_equal(9*eye(3),actualBD[15])); + EXPECT(assert_equal(1*I_3x3,actualBD[5])); + EXPECT(assert_equal(4*I_3x3,actualBD[10])); + EXPECT(assert_equal(9*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices) // hessianBlockDiagonal map actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(4*eye(3),actualBD[5])); - EXPECT(assert_equal(16*eye(3),actualBD[10])); - EXPECT(assert_equal(36*eye(3),actualBD[15])); + EXPECT(assert_equal(4*I_3x3,actualBD[5])); + EXPECT(assert_equal(16*I_3x3,actualBD[10])); + EXPECT(assert_equal(36*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -324,7 +324,7 @@ TEST(JacobianFactor, operators ) { SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); @@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); + Matrix zero3x3 = Matrix::Zero(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Vector9 b; b << b1, b0, b2; @@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) { // construct a linear constraint Vector v(2); v(0)=1.2; v(1)=3.4; - JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2)); + JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2)); // eliminate it pair @@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) // verify conditional Gaussian Vector sigmas = Vector2(0.0, 0.0); - GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); + GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 999541ff5..c4da59496 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) { EXPECT(assert_equal(x_initial, p1->mean())); Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished(); EXPECT(assert_equal(Sigma, p1->covariance())); - EXPECT(assert_equal(inverse(Sigma), p1->information())); + EXPECT(assert_equal(Sigma.inverse(), p1->information())); // Create one with a sharedGaussian KalmanFilter::State p2 = kf1.init(x_initial, Sigma); @@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) { TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01*eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01*I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01*eye(2, 2); + Matrix R = 0.01*I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01*eye(2, 2); + Matrix P00 = 0.01*I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) { Vector u = Vector3(1.0, 0.0, 2.0); Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished(); Matrix M = trans(R)*R; - Matrix Q = inverse(M); + Matrix Q = M.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -167,7 +167,7 @@ TEST( KalmanFilter, predict ) { // Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update TEST( KalmanFilter, QRvsCholesky ) { - Vector mean = ones(9); + Vector mean = Vector::Ones(9); Matrix covariance = 1e-6 * (Matrix(9, 9) << 15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6, -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1, @@ -197,8 +197,8 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); - Matrix B = zeros(9, 1); - Vector u = zero(1); + Matrix B = Matrix::Zero(9, 1); + Vector u = Z_1x1; Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); // do the above update again, this time with a full Matrix Q - Matrix modelQ = diag(emul(sigmas,sigmas)); + Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal(); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 7ac4846f9..43ce271f3 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -112,7 +112,7 @@ TEST(NoiseModel, Unit) TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), - g2 = Gaussian::SqrtInformation(eye(3,3)); + g2 = Gaussian::SqrtInformation(I_3x3); Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), @@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors ) Vector3 mu(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? - EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value actual = Constrained::All(d, m); - EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); actual = Constrained::All(d, mu); EXPECT(assert_equal(mu, actual->mu())); @@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors ) EXPECT(assert_equal(mu, actual->mu())); actual = Constrained::MixedSigmas(m, sigmas); - EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); } /* ************************************************************************* */ @@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2); - Matrix M = 0.5*eye(3); + Matrix M = 0.5*I_3x3; EXPECT(checkIfDiagonal(M)); gtsam::SharedGaussian actual = Gaussian::Information(M, smart); EXPECT(assert_equal(*expected,*actual)); @@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector ) { bool smart = true; SharedGaussian expected = Unit::Create(3); - SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace) { Vector sigmas = Vector3(0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); - Matrix A = eye(3); + Matrix A = I_3x3; model->WhitenInPlace(A); - Matrix expected = eye(3) * 10; + Matrix expected = I_3x3 * 10; EXPECT(assert_equal(expected, A)); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 71fdbe6a6..20fe3ac40 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); -static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); @@ -144,8 +144,8 @@ TEST (Serialization, linear_factors) { EXPECT(equalsBinary(values)); Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; + Vector b = Vector::Ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); @@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) { { Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; + 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); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index b61a861d6..21f74ac06 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -189,7 +189,7 @@ public: Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; - *H = Matrix::Zero(2, 6); + *H = Matrix::Zero(2,6); H->block<2,3>(0,0) = H23; } return e; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index bee3e8944..3875391d0 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -154,7 +154,7 @@ public: // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) - *H2 = eye(3); + *H2 = I_3x3; return (hx - measured_); } }; @@ -205,7 +205,7 @@ public: *H2 = scale * H * (*H2); } if (H3) - *H3 = eye(3); + *H3 = I_3x3; return (hx - measured_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b170ea863..88d9c6437 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -26,6 +26,38 @@ namespace gtsam { +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegratedRotationParams { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + + virtual void print(const std::string& s) const; + virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; + + void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; } + void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } + void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } + + const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; } + boost::optional getOmegaCoriolis() const { return omegaCoriolis; } + boost::optional getBodyPSensor() const { return body_P_sensor; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } +}; + /** * PreintegratedRotation is the base class for all PreintegratedMeasurements * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). @@ -33,29 +65,7 @@ namespace gtsam { */ class PreintegratedRotation { public: - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params { - Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - - Params() : gyroscopeCovariance(I_3x3) {} - - virtual void print(const std::string& s) const; - virtual bool equals(const Params& other, double tol=1e-9) const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - } - }; + typedef PreintegratedRotationParams Params; protected: /// Parameters diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index fefd4b23c..c243ca860 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -105,7 +105,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f68f711f1..f2b2c0fef 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -23,7 +23,7 @@ namespace gtsam { /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor -struct PreintegrationParams: PreintegratedRotation::Params { +struct PreintegrationParams: PreintegratedRotationParams { Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration @@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params { void print(const std::string& s) const; bool equals(const PreintegratedRotation::Params& other, double tol) const; + void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } + void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } + void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } + + const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } + const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } + protected: /// Default constructor for serialization only: uninitialized! PreintegrationParams() {} @@ -60,7 +68,7 @@ protected: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 02911acb1..208d0e709 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); + double normx = x.norm(); const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X * X; diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 790080556..f7f0aa9ad 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/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( @@ -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( diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index de318b3e4..d9ba38e02 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/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( @@ -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( diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 38aecfcbc..1cc84751c 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/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 // (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 // (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 // (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 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 86fb34fe0..0d8d717bc 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -142,18 +142,18 @@ public: const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) - *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare + *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare return traits::Local(xj,feasible_); } else if (compare_(feasible_, xj)) { if (H) - *H = eye(nj); - return zero(nj); // set error to zero if equal + *H = Matrix::Identity(nj,nj); + return Vector::Zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); - return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal + return Vector::Constant(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } @@ -249,7 +249,7 @@ public: Vector evaluateError(const X& x1, boost::optional H = boost::none) const { if (H) - (*H) = eye(traits::GetDimension(x1)); + (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } @@ -322,8 +322,8 @@ public: Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { static const size_t p = traits::dimension; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); return traits::Local(x1,x2); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ec77b2bd6..505f58394 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/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(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -543,7 +543,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -627,7 +627,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -715,7 +715,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 0008252c4..446a67ec7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -269,25 +269,114 @@ namespace gtsam { return filter(key_value.key); } - /* ************************************************************************* */ - template - const ValueType& Values::at(Key j) const { - // Find the item - KeyValueMap::const_iterator item = values_.find(j); + /* ************************************************************************* */ - // Throw exception if it does not exist - if(item == values_.end()) - throw ValuesKeyDoesNotExist("retrieve", j); + namespace internal { // Check the type and throw exception if incorrect - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { - // NOTE(abe): clang warns about potential side effects if done in typeid - const Value* value = item->second; - throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); - } + // Generic version, partially specialized below for various Eigen Matrix types + template + struct handle { + ValueType operator()(Key j, const gtsam::Value * const pointer) { + try { + // value returns a const ValueType&, and the return makes a copy !!!!! + return dynamic_cast&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); + } + } + }; + + // Handle dynamic vectors + template<> + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const Vector&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed vector was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Handle dynamic matrices + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const Matrix&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Request for a fixed vector + // TODO(jing): is this piece of code really needed ??? + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const VectorM&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic vector was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != 1) + throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); + else + // This is not a copy because of RVO + return A; + } + } + }; + + // Request for a fixed matrix + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic matrix was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + // This is not a copy because of RVO + return A; + } + } + }; + + + } // internal + + /* ************************************************************************* */ + template + ValueType Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("at", j); + + // Check the type and throw exception if incorrect + return internal::handle()(j,item->second); } /* ************************************************************************* */ @@ -312,16 +401,48 @@ namespace gtsam { } /* ************************************************************************* */ + + // wrap all fixed in dynamics when insert and update + namespace internal { + + // general type + template + struct handle_wrap { + inline gtsam::GenericValue operator()(Key j, const ValueType& val) { + return gtsam::GenericValue(val); + } + }; + + // when insert/update a fixed size vector: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + // when insert/update a fixed size matrix: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + } + // insert a templated value template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); - } + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(internal::handle_wrap()(j, val))); + } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(internal::handle_wrap()(j, val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 07757062c..ba7a040cd 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,8 +25,6 @@ #include #include -#include - #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -38,6 +36,9 @@ #endif #include +#include +#include + using namespace std; namespace gtsam { @@ -112,24 +113,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector Values::atFixed(Key j, size_t n) { - switch (n) { - case 1: return at(j); - case 2: return at(j); - case 3: return at(j); - case 4: return at(j); - case 5: return at(j); - case 6: return at(j); - case 7: return at(j); - case 8: return at(j); - case 9: return at(j); - default: - throw runtime_error( - "Values::at fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -148,24 +131,6 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } - /* ************************************************************************* */ - void Values::insertFixed(Key j, const Vector& v, size_t n) { - switch (n) { - case 1: insert(j,v); break; - case 2: insert(j,v); break; - case 3: insert(j,v); break; - case 4: insert(j,v); break; - case 5: insert(j,v); break; - case 6: insert(j,v); break; - case 7: insert(j,v); break; - case 8: insert(j,v); break; - case 9: insert(j,v); break; - default: - throw runtime_error( - "Values::insert fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { @@ -269,4 +234,18 @@ namespace gtsam { return message_.c_str(); } + /* ************************************************************************* */ + const char* NoMatchFoundForFixed::what() const throw() { + if(message_.empty()) { + ostringstream oss; + oss + << "Attempting to retrieve fixed-size matrix with dimensions " // + << M1_ << "x" << N1_ + << ", but found dynamic Matrix with mismatched dimensions " // + << M2_ << "x" << N2_; + message_ = oss.str(); + } + return message_.c_str(); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 70aadfa06..a61d01f23 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -168,16 +168,13 @@ namespace gtsam { /** Retrieve a variable by key \c j. The type of the value associated with * this key is supplied as a template argument to this function. * @param j Retrieve the value associated with this key - * @tparam Value The type of the value stored with this key, this method - * throws DynamicValuesIncorrectType if this requested type is not correct. - * @return A const reference to the stored value + * @tparam ValueType The type of the value stored with this key, this method + * Throws DynamicValuesIncorrectType if this requested type is not correct. + * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa. + * @return The stored value */ template - const ValueType& at(Key j) const; - - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - Vector atFixed(Key j, size_t n); + ValueType at(Key j) const; /// version for double double atDouble(size_t key) const { return at(key);} @@ -259,10 +256,6 @@ namespace gtsam { template void insert(Key j, const ValueType& val); - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - void insertFixed(Key j, const Vector& v, size_t n); - /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -500,6 +493,28 @@ namespace gtsam { } }; + /* ************************************************************************* */ + class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception { + + protected: + const size_t M1_, N1_; + const size_t M2_, N2_; + + private: + mutable std::string message_; + + public: + NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () : + M1_(M1), N1_(N1), M2_(M2), N2_(N2) { + } + + virtual ~NoMatchFoundForFixed() throw () { + } + + virtual const char* what() const throw (); + }; + + /* ************************************************************************* */ /// traits template<> struct traits : public Testable { diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 5b591bcf0..19fd257e8 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -25,7 +25,7 @@ Expression compose(const Expression& t1, const Expression& t2) { } // Some typedefs -typedef Expression double_; +typedef Expression Double_; typedef Expression Vector1_; typedef Expression Vector2_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..dece9bad8 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -17,7 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ -#include +#include #include #include #include @@ -32,9 +32,7 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; -typedef Expression double_; typedef Expression Point3_; -typedef Expression Vector3_; typedef Expression Pose3_; typedef Expression Rot3_; @@ -101,7 +99,7 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - double_ e(f2, p); + Double_ e(f2, p); EXPECT(expected == e.keys()); } @@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p); // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - double_ p_cam(doubleF, x, p); + Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ @@ -269,11 +267,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) - *H1 = eye(3); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } @@ -372,8 +370,8 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(>sam::norm, Point3_(key)); - const double_ sum_ = norm_ + norm_; + const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ sum_ = norm_ + norm_; Values values; values.insert(key, Point3(6, 0, 0)); @@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bd862ef94..df1df0d20 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) { // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model)); + l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index dfc14e1e6..8c1c85038 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -477,13 +477,59 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedSize) { +TEST(Values, VectorDynamicInsertFixedRead) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; - values.insertFixed(key1, v, 3); + values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); - CHECK(assert_equal((Vector)expected, values.at(key1))); - CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorDynamicInsertDynamicRead) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertFixedRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected; expected << 5.0, 6.0, 7.0; + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertDynamicRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, MatrixDynamicInsertFixedRead) { + Values values; + Matrix v(1,3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.at(key1), exception); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d2b042fed..0f187db75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -129,7 +129,7 @@ public: if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for - return zero(2); + return Z_2x1; } } @@ -266,13 +266,13 @@ public: return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 3); + if (H3) *H3 = Matrix::Zero(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } - return zero(2); + return Z_2x1; } /** return the measured */ diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b93fe1fc1..180e5cd24 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -31,10 +31,9 @@ using namespace std; namespace gtsam { namespace InitializePose3 { -//static const Matrix I = eye(1); -static const Matrix I9 = eye(9); +static const Matrix I9 = I_9x9; static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33= Matrix::Zero(3,3); +static const Matrix zero33 = Z_3x3; static const Key keyAnchor = symbol('Z', 9999999); @@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { else std::cout << "Error in buildLinearOrientationGraph" << std::endl; - // std::cout << "Rij \n" << Rij << std::endl; - const FastVector& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; - Matrix M9 = Matrix::Zero(9,9); + Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; @@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); - Matrix ppm = Matrix::Zero(3,3); // plus plus minus + Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 16560a43e..f9f258055 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -56,7 +56,7 @@ public: Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q - Matrix Q = eye(m2) - E * P * E.transpose(); + Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? std::vector QF; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 3728e53b1..5b0c6a6b9 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, Vector e = n_hat_p.error(n_hat_q, H_p); H->resize(2, 3); H->block<2, 2>(0, 0) << H_p; - H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + H->block<2, 1>(0, 2) << Z_2x1; return e; } else { Unit3 n_hat_p = measured_p_.normal(); diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 94c19a9d0..44f317a49 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -75,7 +75,7 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); if (H) { - *H = gtsam::zeros(rDim, xDim); + *H = Matrix::Zero(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index a24421b34..0b1c0cf63 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -65,7 +65,7 @@ public: const int tDim = traits::GetDimension(newTrans); const int xDim = traits::GetDimension(pose); if (H) { - *H = gtsam::zeros(tDim, xDim); + *H = Matrix::Zero(tDim, xDim); std::pair transInterval = POSE::translationInterval(); (*H).middleCols(transInterval.first, tDim) = R.matrix(); } 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/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 1056527d1..dee8e925f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -146,15 +146,15 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector2::Constant(2.0 * K_->fx()); } /** return the measurement */ diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 90ceeafc8..966ade343 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* gtsam::eye(Point::dimension); + *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index eca7f531b..07bd20d4d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -73,7 +74,7 @@ protected: std::vector measured_; /// @name Pose of the camera in the body frame - const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + boost::optional body_P_sensor_; ///< Pose of the camera in the body frame /// @} // Cache for Fblocks, to avoid a malloc ever time we re-linearize @@ -385,7 +386,9 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(noiseModel_); ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // end class SmartFactorBase diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 00c19198b..01f89e526 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -112,6 +112,20 @@ struct GTSAM_EXPORT SmartProjectionParams { void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } }; /** @@ -277,9 +291,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -463,7 +477,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * 2); + return Vector::Zero(cameras.size() * 2); } /** @@ -535,8 +549,9 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); - ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); + ar & BOOST_SERIALIZATION_NVP(params_); + ar & BOOST_SERIALIZATION_NVP(result_); + ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_); } } ; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 578422eaf..59fc372cb 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -138,15 +138,15 @@ public: return (stereoCam.project(point, H1, H2) - measured_).vector(); } } catch(StereoCheiralityException& e) { - if (H1) *H1 = zeros(3,6); - if (H2) *H2 = zeros(3,3); + if (H1) *H1 = Matrix::Zero(3,6); + if (H2) *H2 = Z_3x3; if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(3) * 2.0 * K_->fx(); + return Vector3::Constant(2.0 * K_->fx()); } /** return the measured */ diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index aa50929a5..e97cd2730 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -124,14 +124,14 @@ public: return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(Measurement::dimension, 3); + *H2 = Matrix::Zero(Measurement::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); + return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 14192fcaa..b2a90cb84 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 = I_6x6; InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal @@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) { ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); - Matrix m = eye(6); + Matrix m = I_6x6; for (int i = 0; i < 6; i++) for (int j = i; j < 6; j++) ls >> m(i, j); @@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = eye(6); + Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; @@ -563,11 +563,13 @@ GraphAndValues load3D(const string& filename) { m(j, i) = mij; } } - Matrix mgtsam = eye(6); - 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 - mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal + Matrix mgtsam = I_6x6; + + mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation + mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation + mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal + mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); graph->push_back(factor); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index e46b8ee71..fa6fce37a 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 = I_1x1; +static const Matrix I3 = I_3x3; static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 31b276642..b2150dd86 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/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)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 3c7d5f2b2..f61beab6c 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/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(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(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2fd471c9c..bbc0c6518 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/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(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(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(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(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 309801ccb..f955aa191 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/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; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 77944da83..4184d6769 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); //************************************************************************************* TEST( regularImplicitSchurFactor, creation ) { // Matrix E = Matrix::Ones(6,3); - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); @@ -61,35 +61,35 @@ TEST( regularImplicitSchurFactor, creation ) { /* ************************************************************************* */ TEST( regularImplicitSchurFactor, addHessianMultiply ) { - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); - E.block<2,2>(4, 1) = eye(2); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); + E.block<2,2>(4, 1) = I_2x2; Matrix3 P = (E.transpose() * E).inverse(); double alpha = 0.5; VectorValues xvalues = map_list_of // - (0, gtsam::repeat(6, 2))// - (1, gtsam::repeat(6, 4))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 8)); + (0, Vector::Constant(6, 2))// + (1, Vector::Constant(6, 4))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 8)); VectorValues yExpected = map_list_of// - (0, gtsam::repeat(6, 27))// - (1, gtsam::repeat(6, -40))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 279)); + (0, Vector::Constant(6, 27))// + (1, Vector::Constant(6, -40))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 279)); // Create full F size_t M=4, m = 3, d = 6; Matrix F(2 * m, d * M); - F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; + F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x FastVector keys2; keys2 += 0,1,2,3; Vector x = xvalues.vector(keys2); - Vector expected = zero(24); + Vector expected = Vector::Zero(24); RegularImplicitSchurFactor::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(1,10*ones(6)/3); - expectedVV.insert(3,-19.5*ones(6)); + expectedVV.insert(0,-3.5*Vector::Ones(6)); + expectedVV.insert(1,10*Vector::Ones(6)/3); + 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) - zeros(2,6) 2*ones(2,6) zeros(2,6) - zeros(2,6) zeros(2,6) 3*ones(2,6)] + * F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6) + zeros(2,6) 2*Vector::Ones(2,6) zeros(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(1, 4.782608*ones(6)); - expected.insert(3, 7.043478*ones(6)); + expected.insert(0, 1.195652*Vector::Ones(6)); + expected.insert(1, 4.782608*Vector::Ones(6)); + expected.insert(3, 7.043478*Vector::Ones(6)); EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5)); // hessianBlockDiagonal @@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); // variant two - Matrix I2 = eye(2); + Matrix I2 = I_2x2; Matrix E0 = E.block<2,3>(0, 0); Matrix F0t = F0.transpose(); EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0])); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index a8f48b050..9bb296444 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/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)); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index e441c37ff..7eefb2398 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); CHECK( - assert_equal(zero(4), + assert_equal(Z_4x1, factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); } @@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); double actualError = 0.5 - * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + * (e1.norm() * e1.norm() + e2.norm() * e2.norm()); double actualErrorGraph = generalGraph.error(values); DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 5430e1088..ace75f80f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); // Calculate using computeJacobians Vector b; @@ -1409,6 +1409,26 @@ TEST(SmartProjectionPoseFactor, serialize) { EXPECT(equalsBinary(factor)); } +TEST(SmartProjectionPoseFactor, serialize2) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + vector key_view; + vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index 5b986415e..1e768ef2a 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor { this->mask_[0] = 6; this->mask_[1] = 7; this->mask_[2] = 8; - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; @@ -75,13 +75,13 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) : Base(key, model) { - this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch] + this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch] this->mask_.resize(4); this->mask_[0] = 5; // z = height this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } @@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index ecc43ad79..9ec10e39f 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = eye(p)*h_; + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*h_; return (Vector(1) << qk+v*h_-qk1).finished(); } @@ -98,9 +98,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } @@ -154,9 +154,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } @@ -210,9 +210,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b6fc61411..c1afe3882 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/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); diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index a3dd6327b..f38c256b1 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -140,7 +140,7 @@ public: } if (H3) { - *H3 = zeros(6,6); + *H3 = Z_6x6; insertSub(*H3, -h_*D_gravityBody_gk, 3, 0); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9ecc7619e..f98879e41 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = eye(p); - if (H2) *H2 = -eye(p); - if (H3) *H3 = eye(p)*dt_; + if (H1) *H1 = Matrix::Identity(p,p); + if (H2) *H2 = -Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*dt_; return (Vector(1) << x1+v*dt_-x2).finished(); } diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fc6a0197..494f2731f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/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 imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); @@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) { VelocityConstraint constraint(x1, x2, 0.1, 10000); PriorFactor 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 gyro = delta(3, 0, 0.1); // constant rotation + Vector accel = Vector::Unit(3,0)*0.5; // forward force + Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation SharedDiagonal model = noiseModel::Unit::Create(9); Values true_traj, init_traj; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index 5a4593270..d4b877008 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/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)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 855131042..025c838c9 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/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(zero(9), identity.localCoordinates(identity))); + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1))); + 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(zero(9), state1.localCoordinates(state1))); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1))); Vector delta(9); delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; @@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) { const double dt = 0.1; Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2); - Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); + Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1; x1 = x0.generalDynamics(accel, gyro, dt); x2 = x1.generalDynamics(accel, gyro, dt); @@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) { /* ************************************************************************* */ TEST(testPoseRTV, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 8b224f510..fe21d5e00 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0); // no shape Vector u2 = Vector2(0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag((Vector(3) << mass, mass, mass).finished()); -Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished()); +Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal(); +Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal(); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); @@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; EXPECT( - assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); + assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( @@ -89,8 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); - subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame + 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; Vector dV = newtonEuler(V1_g1, Fb, Inertia); @@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); + EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( boost::function( diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index f253be371..6d8206177 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/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(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(delta(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(Z_3x1, constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), 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(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), 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, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), 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(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), 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, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index ac27ae563..b8caf4414 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/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)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4ce0eaedb..4f1b42610 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -102,7 +102,7 @@ public: gtsam::Matrix J2; gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { - *H1 = (*H1) * gtsam::eye(6); + *H1 = (*H1) * I_6x6; } double cos_theta = cos(theta); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index f83cb19fb..4237170f0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const { Pose3Upright Pose3Upright::inverse(boost::optional H1) const { Pose3Upright result(T_.inverse(H1), -z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; @@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); if (H1) { - Matrix H1_ = eye(4,4); + Matrix H1_ = I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } @@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2, return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f60ea107c..90e3eeea6 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/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()); diff --git a/gtsam_unstable/geometry/tests/testBearingS2.cpp b/gtsam_unstable/geometry/tests/testBearingS2.cpp index 48d6e29af..f29b30621 100644 --- a/gtsam_unstable/geometry/tests/testBearingS2.cpp +++ b/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(origin, origin.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol)); - EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol)); - EXPECT(assert_equal(b1, b1.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol)); + EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index cd54a8813..5e4c2468d 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/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(x1, x1.retract(zero(4)), tol)); - EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol)); + EXPECT(assert_equal(x1, x1.retract(Z_4x1), 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(origin, Pose3Upright::Expmap(zero(4)), tol)); + EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol)); + EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol)); EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index e2f2a2a86..d2fbd7579 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/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; @@ -167,7 +167,7 @@ TEST( Similarity3, retract_first_order) { //****************************************************************************** TEST(Similarity3, localCoordinates_first_order) { - Vector d12 = repeat(7, 0.1); + Vector7 d12 = Vector7::Constant(0.1); d12(6) = 1.0; Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f0eb7d7fb..0d9bbae6e 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/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); diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index bf41743a2..90f3de586 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -186,7 +186,7 @@ TEST(LinearEquality, matrices) /* ************************************************************************* */ TEST(LinearEquality, operators ) { - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = (Vector(2) << 0.2,-0.1).finished(); LinearEquality lf(1, -I, 2, I, b, 0); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 8fca61ca4..7442540f5 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = 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 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), - 2.0 * ones(1, 1), zero(1), 10.0)); + 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), Z_1x1, 10.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/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), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -53,8 +53,8 @@ QP createTestCase() { TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; - values.insert(X(1), x1 * ones(1, 1)); - values.insert(X(2), x2 * ones(1, 1)); + values.insert(X(1), x1 * Matrix::Ones(1, 1)); + values.insert(X(2), x2 * Matrix::Ones(1, 1)); QP qp = createTestCase(); DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); @@ -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 * ones(1, 1), zeros(1, 1), zero(1), - 2.0 * ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, + 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(2), ones(1)); + initialValues.insert(X(1), I_1x1); + 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(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + 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(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); std::vector 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(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + 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 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), - 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, + 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(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + 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), ones(1, 1), ones(1))); - qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * 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 * 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), eye(2), zero(2))); - qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + 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)); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index f8dca75a4..84dffe7be 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/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 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]; diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index bfd3c9168..1cf94b161 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -126,33 +126,33 @@ TEST( ParticleFilter, constructor) { TEST( ParticleFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01 * eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01 * I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01 * eye(2, 2); + Matrix R = 0.01 * I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01 * eye(2, 2); + Matrix P00 = 0.01 * I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a27730f4..0a3fa0283 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -32,7 +32,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, // estimate standard deviation on gyroscope readings Pg_ = Cov(stationaryU); - Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); + Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt(); // estimate standard deviation on accelerometer readings Pa_ = Cov(stationaryF); @@ -45,14 +45,14 @@ 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_g = (0.0034 * 0.0034) * ones(3); - Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + Vector3 var_omega_w = 0 * Vector::Ones(3); // TODO + Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::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; // Quantities needed for aiding - sigmas_v_a_ = esqrt(T * Pa_.diagonal()); + sigmas_v_a_ = (T * Pa_.diagonal()).cwiseSqrt(); // gravity in nav frame n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished(); @@ -83,7 +83,7 @@ std::pair AHRS::initialize(double g_e) 0.0, 0.0, 0.0).finished(); // we don't know anything on yaw // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 - Matrix Pa = 0.025 * 0.025 * eye(3); + Matrix Pa = 0.025 * 0.025 * I_3x3; Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T); P11(2, 2) = 0.0001; Matrix P12 = -Omega_T * H_g * Pa; @@ -101,7 +101,7 @@ std::pair 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); } @@ -171,7 +171,7 @@ std::pair AHRS::aid( // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; H = collect(3, &n_g_cross_, &Z_3x3, &bRn); - R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; + R = trans(bRn) * ((Vector3) sigmas_v_a_.array().square()).asDiagonal() * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g @@ -186,7 +186,7 @@ std::pair AHRS::aid( Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given - R = diag(emul(sigmas_v_a_, sigmas_v_a_)); + R = ((Vector3) sigmas_v_a_.array().square()).asDiagonal(); } // update the Kalman filter @@ -196,7 +196,7 @@ std::pair AHRS::aid( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); @@ -217,7 +217,7 @@ std::pair AHRS::aidGeneral( Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); + Matrix R = Vector3(0.01, 0.0001, 0.01).asDiagonal(); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); @@ -226,7 +226,7 @@ std::pair AHRS::aidGeneral( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 1ce3a878d..8519f6f8d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -50,12 +50,12 @@ DummyFactor::linearize(const Values& c) const { std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { terms[j].first = keys()[j]; - terms[j].second = zeros(rowDim_, dims_[j]); + terms[j].second = Matrix::Zero(rowDim_, dims_[j]); } 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)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 79a37c2ff..58284c3a6 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -434,8 +434,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); @@ -461,7 +461,7 @@ public: noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse(); EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; // Luca: force identity covariance matrix (for testing purposes) @@ -536,9 +536,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; @@ -550,9 +550,9 @@ public: const noiseModel::Gaussian::shared_ptr& gaussian_process, Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){ - cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); } static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8216942cd..acca233d9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -367,8 +367,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 7f9564ee3..68c972a86 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -103,8 +103,8 @@ public: Vector hx(v2 - alpha_v1); - if(H1) *H1 = - diag(alpha); - if(H2) *H2 = eye(v2.size()); + if(H1) *H1 = -1 * alpha.asDiagonal(); + if(H2) *H2 = Matrix::Identity(v2.size(),v2.size()); return hx; } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 30d3a216d..c3a67232a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -278,9 +278,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index ac481f979..7509fe3b2 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -89,12 +89,12 @@ public: gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); return reprojectionError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = gtsam::zeros(2,6); - if (H2) *H2 = gtsam::zeros(2,5); - if (H3) *H2 = gtsam::zeros(2,1); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,5); + 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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2fd964a35..e9f894faf 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index fdba78445..ec2615ed6 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index feff0b62c..cc5878d85 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/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(); } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 20ffbdd4f..94126a68c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -33,7 +33,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished(); + b_g = (Vector3(3) << 0.0, 0.0, meanF.norm()).finished(); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; @@ -66,14 +66,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { - Vector3 rho = sub(dx, 0, 3); +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const { + Vector3 rho = dx.segment<3>(0); Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector3 x_g = x_g_ + sub(dx, 3, 6); - Vector3 x_a = x_a_ + sub(dx, 6, 9); + Vector3 x_g = x_g_ + dx.segment<3>(3); + Vector3 x_a = x_a_ + dx.segment<3>(6); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4c85614d4..a228b2347 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/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) { } @@ -68,7 +68,7 @@ public: * @param obj The current state * @param dx The error used to correct and return a new state */ - Mechanization_bRn2 correct(const Vector3& dx) const; + Mechanization_bRn2 correct(const Vector9& dx) const; /** * Integrate to get new state diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f1487f562..dc250fd9d 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -184,15 +184,15 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurements */ diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index e3080466f..fa06d47a3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) { + Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) { assert(model->dim() == 1); this->fillH(); } @@ -78,7 +78,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) { + Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); this->fillH(); @@ -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; ikey2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 069274065..2fd622ea1 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -132,17 +132,17 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,6); - if (H3) *H3 = zeros(2,3); - if (H4) *H4 = zeros(2,CALIBRATION::Dim()); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,6); + if (H3) *H3 = Matrix::Zero(2,3); + if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K.fx(); + return Vector::Ones(2) * 2.0 * K.fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 941a1db89..db077994f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -21,7 +21,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p boost::optional H1, boost::optional H2) const { double hx = pose.z() - point.z(); if (H1) { - *H1 = zeros(1, 6); + *H1 = Matrix::Zero(1,6); // Use bottom row of rotation matrix for derivative of translation (*H1)(0, 3) = pose.rotation().r1().z(); (*H1)(0, 4) = pose.rotation().r2().z(); @@ -29,7 +29,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p } if (H2) { - *H2 = zeros(1, 3); + *H2 = Matrix::Zero(1,3); (*H2)(0, 2) = -1.0; } return (Vector(1) << hx - measured_).finished(); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 84aed6ca8..3db302d0b 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -141,10 +141,10 @@ public: if (H) // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) - (*H)[j] = zeros(3, 1); - return zero(1); + (*H)[j] = Matrix::Zero(3, 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 ? diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 7cbeaae65..56e22aae2 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -340,9 +340,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(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); } /** diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index b500b36e3..e37e8ff20 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,7 +17,7 @@ * @date December 2014 */ -#include +#include #include namespace gtsam { @@ -25,7 +25,7 @@ namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) class TOAFactor: public ExpressionFactor { - typedef Expression double_; + typedef Expression Double_; public: @@ -40,7 +40,7 @@ public: const Expression& microphone_, double toaMeasurement, const SharedNoiseModel& model) : ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, eventExpression, microphone_)) { + Double_(&Event::toa, eventExpression, microphone_)) { } }; diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 86b4e5584..743c7398c 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/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, zeros(3,3), key2, zeros(3,3), zero(3), model3)); + key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 476447f8b..35bf5790e 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -41,8 +41,8 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0); Vector delta_angles = Vector3(0.0, 0.0, 0.0); double delta_t = 0.0; - Matrix EquivCov_Overall = zeros(15,15); - Matrix Jacobian_wrt_t0_Overall = eye(15); + Matrix EquivCov_Overall = Matrix::Zero(15,15); + Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(15,15); imuBias::ConstantBias bias1 = imuBias::ConstantBias(); // Earth Terms (gravity, etc) diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 75535eebc..b84f7e080 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/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)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index a06c39182..210018ed3 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/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( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -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( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 1d8b30850..dc05711e3 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -48,9 +48,9 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2)))); + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; } diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ee06dbce2..9ae4aa928 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -70,7 +70,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Check Jacobian for n==1 vector H1(1); f.unwhitenedError(values, H1); // with H now ! - CHECK(assert_equal(zeros(3,1), H1.front())); + CHECK(assert_equal(Matrix::Zero(3,1), H1.front())); // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 52786702d..233dbdceb 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; // typedefs -typedef Eigen::Matrix Vector1; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +51,7 @@ TEST( TOAFactor, NewWay ) { Event_ eventExpression(key); Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - double_ expression(&Event::toa, eventExpression, microphoneConstant); + Double_ expression(&Event::toa, eventExpression, microphoneConstant); ExpressionFactor factor(model, measurement, expression); } @@ -92,7 +91,7 @@ TEST( TOAFactor, WholeEnchilada ) { Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { Point3_ microphone_i(microphones[i]); // constant expression - double_ predictTOA(&Event::toa, eventExpression, microphone_i); + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d8d720e83..01d4b152d 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/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)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index dbd90f3a3..b200a3e58 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/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)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/testing_tools/base/cholScalingTest.m b/gtsam_unstable/testing_tools/base/cholScalingTest.m index ed347a523..24f597ed7 100644 --- a/gtsam_unstable/testing_tools/base/cholScalingTest.m +++ b/gtsam_unstable/testing_tools/base/cholScalingTest.m @@ -19,8 +19,8 @@ badscale = 1e-8; Acoeffs = [ 1 11 badscale - (1:10)' (1:10)' -ones(10,1) - (1:10)' (2:11)' ones(10,1) + (1:10)' (1:10)' -Matrix::Ones(10,1) + (1:10)' (2:11)' Matrix::Ones(10,1) ]'; A = full(sparse(Acoeffs(1,:), Acoeffs(2,:), Acoeffs(3,:))); b = zeros(size(A,1), 1); diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 46e161807..20931499b 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'g'; + linespec = 'g*'; end haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); @@ -15,19 +15,25 @@ keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot points and covariance matrices -for i = 0:keys.size-1 - key = keys.at(i); - try - p = values.atPoint3(key); - if haveMarginals +if haveMarginals + % Plot points and covariance matrices (slow) + for i = 0:keys.size-1 + key = keys.at(i); + try + p = values.atPoint3(key) P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); - else - gtsam.plotPoint3(p, linespec); + catch + % I guess it's not a Point3 end - catch - % I guess it's not a Point3 + end +else + % Extract all in C++ and plot all at once (fast) + P = utilities.extractPoint3(values); + if size(linespec,2)==1 + plot3(P(:,1),P(:,2),P(:,3),[linespec '*']); + else + plot3(P(:,1),P(:,2),P(:,3),linespec); end end diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index b0b37ee33..9a8a27344 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -1,3 +1,7 @@ +% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks +% author: Chris Beall +% date: July 2014 + clear all; clf; @@ -24,13 +28,19 @@ if(write_video) open(videoObj); end -% IMU parameters +%% IMU parameters IMU_metadata.AccelerometerSigma = 1e-2; IMU_metadata.GyroscopeSigma = 1e-2; IMU_metadata.AccelerometerBiasSigma = 1e-6; IMU_metadata.GyroscopeBiasSigma = 1e-6; IMU_metadata.IntegrationSigma = 1e-1; +n_gravity = [0;0;-9.8]; +IMU_params = PreintegrationParams(n_gravity); +IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)); +IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)); +IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)); + transformKey = 1000; calibrationKey = 2000; @@ -52,7 +62,7 @@ K = Cal3_S2(20,1280,960); % initialize K incorrectly K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py()); -isamParams = gtsam.ISAM2Params; +isamParams = ISAM2Params; isamParams.setFactorization('QR'); isam = ISAM2(isamParams); @@ -63,7 +73,6 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]); sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ]; -g = [0;0;-9.8]; w_coriolis = [0;0;0]; %% generate trajectory and landmarks @@ -99,7 +108,9 @@ zlabel('z'); title('Estimated vs. actual IMU-cam transform'); axis equal; +%% Main loop for i=1:size(trajectory)-1 + %% Preliminaries xKey = symbol('x',i); pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation @@ -134,6 +145,7 @@ for i=1:size(trajectory)-1 gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]); fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov)); + %% First-time initialization if i==1 % camera transform if use_camera_transform_noise @@ -153,12 +165,10 @@ for i=1:size(trajectory)-1 result = initial; end - % priors on first two poses + %% priors on first two poses if i < 3 -% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); - - end %% the 'normal' case @@ -181,12 +191,14 @@ for i=1:size(trajectory)-1 zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... + fg.add(ProjectionFactorPPPCCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + p = landmarks.atPoint3(lKey); + n = normrnd(0,landmark_noise,3,1); + noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3)); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known @@ -195,14 +207,12 @@ for i=1:size(trajectory)-1 end end % end landmark observations - %% IMU deltaT = 1; logmap = Pose3.Logmap(step); omega = logmap(1:3); velocity = logmap(4:6); - % Simulate IMU measurements, considering Coriolis effect % (in this simple example we neglect gravity and there are no other forces acting on the body) acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... @@ -211,11 +221,9 @@ for i=1:size(trajectory)-1 % [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... % currentIMUPoseGlobal, omega, velocity, velocity, deltaT); - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias); - accMeas = acc_omega(1:3)-g; + accMeas = acc_omega(1:3)-n_gravity; omegaMeas = acc_omega(4:6); currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT); @@ -223,13 +231,13 @@ for i=1:size(trajectory)-1 fg.add(ImuFactor( ... xKey_prev, currentVelKey-1, ... xKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); + currentBiasKey, currentSummarizedMeasurement)); % Bias evolution as given in the IMU metadata fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b))); - % ISAM update + %% ISAM update isam.update(fg, initial); result = isam.calculateEstimate(); @@ -295,10 +303,8 @@ for i=1:size(trajectory)-1 end -% print out final camera transform +%% print out final camera transform and write video result.at(transformKey); - - if(write_video) close(videoObj); end \ No newline at end of file diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index b635589c3..0d1349972 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -56,7 +56,6 @@ void exportValues(){ .def("empty", &Values::empty) .def("equals", &Values::equals) .def("erase", &Values::erase) - .def("insert_fixed", &Values::insertFixed) .def("print", &Values::print, print_overloads(args("s"))) .def("size", &Values::size) .def("swap", &Values::swap) @@ -69,15 +68,15 @@ void exportValues(){ .def("insert", insert_bias) .def("insert", insert_navstate) .def("insert", insert_vector3) - .def("atPoint2", &Values::at, return_value_policy()) - .def("atRot2", &Values::at, return_value_policy()) - .def("atPose2", &Values::at, return_value_policy()) - .def("atPoint3", &Values::at, return_value_policy()) - .def("atRot3", &Values::at, return_value_policy()) - .def("atPose3", &Values::at, return_value_policy()) - .def("atConstantBias", &Values::at, return_value_policy()) - .def("atNavState", &Values::at, return_value_policy()) - .def("atVector3", &Values::at, return_value_policy()) + .def("atPoint2", &Values::at) + .def("atRot2", &Values::at) + .def("atPose2", &Values::at) + .def("atPoint3", &Values::at) + .def("atRot3", &Values::at) + .def("atPose3", &Values::at) + .def("atConstantBias", &Values::at) + .def("atNavState", &Values::at) + .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) ; diff --git a/tests/simulated2D.h b/tests/simulated2D.h index a1080a6de..0012f5f45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,7 @@ namespace simulated2D { /// Prior on a single pose, optionally returns derivative inline Point2 prior(const Point2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(2); + if (H) *H = I_2x2; return x; } @@ -102,8 +102,8 @@ namespace simulated2D { /// odometry between two poses, optionally returns derivative inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return x2 - x1; } @@ -115,8 +115,8 @@ namespace simulated2D { /// measurement between landmark and pose, optionally returns derivative inline Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return l - x; } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index f593ab23a..ccc734cfd 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,7 +90,7 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, traits::GetDimension(x)); + Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index b05fb45c1..9e604cb3c 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -63,7 +63,7 @@ namespace simulated2DOriented { /// Prior on a single pose, optional derivative version Pose2 prior(const Pose2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(3); + if (H) *H = I_3x3; return x; } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 84d9ec8cd..3c92e733e 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -39,7 +39,7 @@ namespace simulated3D { * Prior on a single pose */ Point3 prior(const Point3& x, boost::optional H = boost::none) { - if (H) *H = eye(3); + if (H) *H = I_3x3; return x; } @@ -49,8 +49,8 @@ Point3 prior(const Point3& x, boost::optional H = boost::none) { Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return x2 - x1; } @@ -60,8 +60,8 @@ Point3 odo(const Point3& x1, const Point3& x2, Point3 mea(const Point3& x, const Point3& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return l - x; } diff --git a/tests/smallExample.h b/tests/smallExample.h index 9866d22aa..215655593 100644 --- a/tests/smallExample.h +++ b/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(X(1), zero(2)); - c.insert(X(2), zero(2)); + c.insert(L(1), Z_2x1); + c.insert(X(1), Z_2x1); + c.insert(X(2), Z_2x1); return c; } @@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*eye(2), -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*eye(2), X(2), 10*eye(2), Vector2(2.0, -1.0)); + fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(0.0, 1.0)); + fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5)); + fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); return fg; } @@ -409,7 +409,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -419,8 +419,8 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { // between _x_ and _y_, that is going to be the only factor on _y_ // |1 0||x_1| + |-1 0||y_1| = |0| // |0 1||x_2| | 0 -1||y_2| |0| - Matrix Ax1 = eye(2); - Matrix Ay1 = eye(2) * -1; + Matrix Ax1 = I_2x2; + Matrix Ay1 = I_2x2 * -1; Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -450,7 +450,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -466,7 +466,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Ax1(0, 1) = 2.0; Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; - Matrix Ay1 = eye(2) * 10; + Matrix Ay1 = I_2x2 * 10; Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -492,7 +492,7 @@ inline VectorValues createSingleConstraintValues() { inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 - Matrix A = eye(2); + Matrix A = I_2x2; Vector b = Vector2(-2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 7c1e20a1a..b0b748d95 100644 --- a/tests/testBoundingConstraint.cpp +++ b/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(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(Z_1x1, constraint1.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); } @@ -73,10 +73,10 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol); EXPECT(!constraint3.isGreaterThan()); EXPECT(!constraint4.isGreaterThan()); - EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol)); - EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); + 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(Z_1x1, constraint3.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); } @@ -88,10 +88,10 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { config.insert(key, pt2); EXPECT(constraint1.active(config)); EXPECT(constraint2.active(config)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.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 * ones(1), constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.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); } @@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) { config2.insert(key, pt2); GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), repeat(1, 5.0), hard_model1); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, 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(zero(1), rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); + EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); + 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); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 282a6b816..4fda27cdb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -140,7 +140,7 @@ typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; v << p, p, p; - if (H) *H << eye(3), eye(3), eye(3); + if (H) *H << I_3x3, I_3x3, I_3x3; return v; } typedef Eigen::Matrix Matrix9; @@ -334,11 +334,11 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -364,10 +364,10 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); + JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -393,10 +393,10 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); + JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -409,11 +409,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) - *H1 = eye(3); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } @@ -436,12 +436,12 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); - EXPECT( assert_equal(eye(3), H[2],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[2],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index a7bcf7153..00ab4a16c 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -223,7 +223,7 @@ public: *H1 = -F(p1); if(H2) - *H2 = eye(dim()); + *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 return (p2 - prediction).vector(); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index cf54ce96e..e3c17fa3f 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/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), eye(2)/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), eye(2)/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,9 +132,9 @@ 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 = eye(2,2) * (sigmax1 * sigmax1); + Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix actualCovarianceX1; GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); @@ -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); -// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt); +// 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); @@ -243,22 +243,22 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Conditional density elements reused by both tests - const Matrix I = eye(2), A = -0.00429185*I; + const Matrix I = I_2x2, A = -0.00429185*I; // 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(7), zero(2), -1*I/sigmax7)); + (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/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(4), zero(2), I/sigmax4)); + (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14)) + (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)); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 88dc91ce7..10de57435 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -78,7 +78,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) conditional = result.first->front(); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector2(-0.133333, -0.0222222); GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); @@ -96,8 +96,8 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + 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)); @@ -112,8 +112,8 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + 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)); @@ -129,8 +129,8 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2); + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + 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 @@ -159,8 +159,8 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + 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)); @@ -175,8 +175,8 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + 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)); @@ -186,7 +186,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) TEST( GaussianFactorGraph, eliminateAll ) { // create expected Chordal bayes Net - Matrix I = eye(2); + Matrix I = I_2x2; Ordering ordering; ordering += X(2),L(1),X(1); @@ -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 @@ -389,7 +389,7 @@ TEST( GaussianFactorGraph, elimination ) ord += X(1), X(2); // Create Gaussian Factor Graph GaussianFactorGraph fg; - Matrix Ap = eye(1), An = eye(1) * -1; + Matrix Ap = I_2x2, An = I_2x2 * -1; Vector b = (Vector(1) << 0.0).finished(); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), 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()); } } diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 6c1fd7dd5..b95f16e76 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -112,7 +112,7 @@ TEST( Graph, composePoses ) TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; - Matrix I = eye(2); + Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 071b9d12d..e0c2b7b66 100644 --- a/tests/testIterative.cpp +++ b/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)); } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 89b296824..65d26eb98 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) { EXPECT_DOUBLES_EQUAL(traits::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 chart4; // EXPECT(assert_equal(traits::Local(z, v), v)); @@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector //DefaultChart chart6; - EXPECT(assert_equal(zero(3), traits::Local(R, R))); + EXPECT(assert_equal((Vector) Z_3x1, traits::Local(R, R))); } //****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index f72e08083..86080b673 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, eye(3), 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,11 +133,11 @@ 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( - assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); + assert_equal(actual, Vector::Constant(3, std::numeric_limits::infinity()))); } //****************************************************************************** @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3, 3); + Matrix A1 = I_3x3; Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor( @@ -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(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), 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, eye(2, 2), zero(2), hard_model)); + new JacobianFactor(key, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model)); + new JacobianFactor(key, I_2x2, Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -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(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), 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, -eye(2, 2), key2, eye(2, 2), zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 05cb9c4ad..fb85c3742 100644 --- a/tests/testNonlinearFactor.cpp +++ b/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(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; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f6cdd6ee5..7a22abc67 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/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(1,zero(3)); - expectedGradient.insert(2,zero(3)); + expectedGradient.insert(0,Z_3x1); + expectedGradient.insert(1,Z_3x1); + expectedGradient.insert(2,Z_3x1); // Try LM and Dogleg LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index d51b0abd2..a094f9628 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -46,7 +46,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { vector matrices; for (size_t i=0; i H1, boost::optional H2) { Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } @@ -67,8 +67,8 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, { // TODO: Implement Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -Matrix::Identity(1,1); - if (H2) *H2 = Matrix::Identity(1,1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index e08924400..9be55f831 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -55,14 +55,14 @@ void timeAll(size_t m, size_t N) { Matrix P = (E.transpose() * E).inverse(); // RHS and sigmas - const Vector b = gtsam::repeat(2 * m, 1); + const Vector b = Vector::Constant(2*m,1); const SharedDiagonal model; // parameters for multiplyHessianAdd double alpha = 0.5; VectorValues xvalues, yvalues; for (size_t i = 0; i < m; i++) - xvalues.insert(i, gtsam::repeat(D, 2)); + xvalues.insert(i, Vector::Constant(D,2)); // Implicit RegularImplicitSchurFactor implicitFactor(keys, Fblocks, E, P, b); @@ -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())