diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index 62c158476..b1a11a0fc 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -13,7 +13,7 @@ check_PROGRAMS = # base Math -headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h +headers += FixedVector.h types.h blockMatrices.h sources += Vector.cpp Matrix.cpp sources += cholesky.cpp check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices diff --git a/gtsam/base/Matrix-inl.h b/gtsam/base/Matrix-inl.h deleted file mode 100644 index da59874ca..000000000 --- a/gtsam/base/Matrix-inl.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Matrix-inl.h - * @brief - * @author Richard Roberts - * @created Sep 26, 2010 - */ - -#pragma once - -#include - -#include - -namespace gtsam { - -using namespace std; - -///* ************************************************************************* */ -///** -// * backSubstitute U*x=b -// * @param U an upper triangular matrix -// * @param b an RHS vector -// * @return the solution x of U*x=b -// */ -//template -//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false) { -//// const MATRIX& U(*static_cast(&_U)); -//// const VECTOR& b(*static_cast(&_b)); -// size_t n = U.cols(); -//#ifndef NDEBUG -// size_t m = U.rows(); -// if (m!=n) -// throw invalid_argument("backSubstituteUpper: U must be square"); -//#endif -// -// Vector result(n); -// for (size_t i = n; i > 0; i--) { -// double zi = b(i-1); -// for (size_t j = i+1; j <= n; j++) -// zi -= U(i-1,j-1) * result(j-1); -//#ifndef NDEBUG -// if(!unit && fabs(U(i-1,i-1)) <= numeric_limits::epsilon()) { -// stringstream ss; -// ss << "backSubstituteUpper: U is singular,\n"; -// print(U, "U: ", ss); -// throw invalid_argument(ss.str()); -// } -//#endif -// if (!unit) zi /= U(i-1,i-1); -// result(i-1) = zi; -// } -// -// return result; -//} -// -///* ************************************************************************* */ -///** -// * backSubstitute x'*U=b' -// * @param U an upper triangular matrix -// * @param b an RHS vector -// * @param unit, set true if unit triangular -// * @return the solution x of x'*U=b' -// * TODO: use boost -// */ -//template -//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false) { -//// const VECTOR& b(*static_cast(&_b)); -//// const MATRIX& U(*static_cast(&_U)); -// size_t n = U.cols(); -//#ifndef NDEBUG -// size_t m = U.rows(); -// if (m!=n) -// throw invalid_argument("backSubstituteUpper: U must be square"); -//#endif -// -// Vector result(n); -// for (size_t i = 1; i <= n; i++) { -// double zi = b(i-1); -// for (size_t j = 1; j < i; j++) -// zi -= U(j-1,i-1) * result(j-1); -//#ifndef NDEBUG -// if(!unit && fabs(U(i-1,i-1)) <= numeric_limits::epsilon()) { -// stringstream ss; -// ss << "backSubstituteUpper: U is singular,\n"; -// print(U, "U: ", ss); -// throw invalid_argument(ss.str()); -// } -//#endif -// if (!unit) zi /= U(i-1,i-1); -// result(i-1) = zi; -// } -// -// return result; -//} - -} diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d44701448..9781f6316 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -28,23 +28,19 @@ #include #include +#include #include -#include #include using namespace std; namespace gtsam { -/** Explicit instantiations of template functions for standard types */ -//template Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit); -//template Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit); - /* ************************************************************************* */ Matrix Matrix_( size_t m, size_t n, const double* const data) { - Matrix A(m,n); + MatrixRowMajor A(m,n); copy(data, data+m*n, A.data()); - return A; + return Matrix(A); } /* ************************************************************************* */ @@ -107,25 +103,6 @@ bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) { return false; } -/* ************************************************************************* */ -bool assert_equal(const MatrixColMajor& expected, const MatrixColMajor& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; - - size_t n1 = expected.cols(), m1 = expected.rows(); - size_t n2 = actual.cols(), m2 = actual.rows(); - - cout << "not equal:" << endl; - print(expected,"expected = "); - print(actual,"actual = "); - if(m1!=m2 || n1!=n2) - cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl; - else { - Matrix diff = actual-expected; - print(diff, "actual - expected = "); - } - return false; -} - /* ************************************************************************* */ bool assert_equal(const std::list& As, const std::list& Bs, double tol) { if (As.size() != Bs.size()) return false; @@ -250,25 +227,6 @@ void print(const Matrix& A, const string &s, ostream& stream) { stream << "];" << endl; } -/* ************************************************************************* */ -void print(const MatrixColMajor& A, const string &s, ostream& stream) { - size_t m = A.rows(), n = A.cols(); - - // print out all elements - stream << s << "[\n"; - for( size_t i = 0 ; i < m ; i++) { - for( size_t j = 0 ; j < n ; j++) { - double aij = A(i,j); - if(aij != 0.0) - stream << setw(12) << setprecision(9) << aij << ",\t"; - else - stream << " 0.0,\t"; - } - stream << endl; - } - stream << "];" << endl; -} - /* ************************************************************************* */ void save(const Matrix& A, const string &s, const string& filename) { fstream stream(filename.c_str(), fstream::out | fstream::app); @@ -292,7 +250,7 @@ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { } /* ************************************************************************* */ -Vector columnNormSquare(const MatrixColMajor &A) { +Vector columnNormSquare(const Matrix &A) { Vector v (A.cols()) ; for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { v[i] = A.col(i).dot(A.col(i)); @@ -369,33 +327,6 @@ pair qr(const Matrix& A) { return make_pair(Q,R); } -/* ************************************************************************* */ -/** Imperative version of Householder rank 1 update - * i.e. do outer product update A = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w' - * but only in relevant part, from row j onwards - * If called from householder_ does actually more work as first j columns - * will not be touched. However, is called from GaussianFactor.eliminate - * on a number of different matrices for which all columns change. - */ -/* ************************************************************************* */ - -/** - * Perform updates of system matrices - */ -static void updateAb(Matrix& A, Vector& b, size_t j, const Vector& a, - const Vector& r, double d) { - const size_t m = A.rows(), n = A.cols(); - for (size_t i = 0; i < m; i++) { // update all rows - double ai = a(i); - b(i) -= ai * d; - double *Aij = A.data() + i * n + j + 1; - const double *rptr = r.data() + j + 1; - - for (size_t j2 = j + 1; j2 < n; j2++, Aij++, rptr++) - *Aij -= ai * (*rptr); - } -} - /* ************************************************************************* */ list > weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { @@ -414,12 +345,11 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { // Then update A and b by substituting x with d-rS, zero-ing out x's column. for (size_t j=0; j > + * we use the default < double,col_major,unbounded_array > */ -typedef Eigen::Matrix Matrix; -typedef Eigen::Matrix MatrixColMajor; +typedef Eigen::MatrixXd Matrix; +typedef Eigen::Matrix MatrixRowMajor; // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; @@ -109,9 +109,7 @@ inline bool operator!=(const Matrix& A, const Matrix& B) { /** * equals with an tolerance, prints out message if unequal */ -// FIXME: make better use of templates to test these properly bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9); -bool assert_equal(const MatrixColMajor& A, const MatrixColMajor& B, double tol = 1e-9); /** * equals with an tolerance, prints out message if unequal @@ -175,7 +173,6 @@ Vector Vector_(const Matrix& A); * print a matrix */ void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); -void print(const MatrixColMajor& A, const std::string& s = "", std::ostream& stream = std::cout); /** * save a matrix to file, which can be loaded by matlab @@ -240,7 +237,7 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { void insertColumn(Matrix& A, const Vector& col, size_t j); void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); -Vector columnNormSquare(const MatrixColMajor &A); +Vector columnNormSquare(const Matrix &A); /** * Zeros all of the elements below the diagonal of a matrix, in place @@ -290,8 +287,8 @@ void inplace_QR(MATRIX& A, bool clear_below_diagonal=true) { size_t cols = A.cols(); size_t size = std::min(rows,cols); - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; HCoeffsType hCoeffs(size); RowVectorType temp(cols); @@ -315,9 +312,10 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas); * Householder tranformation, Householder vectors below diagonal * @param k number of columns to zero out below diagonal * @param A matrix + * @param copy_vectors - true to copy Householder vectors below diagonal * @return nothing: in place !!! */ -void householder_(Matrix& A, size_t k); +void householder_(Matrix& A, size_t k, bool copy_vectors=true); /** * Householder tranformation, zeros below diagonal @@ -327,25 +325,6 @@ void householder_(Matrix& A, size_t k); */ void householder(Matrix& A, size_t k); -/** - * Householder tranformation, Householder vectors below diagonal - * Column Major Version - * @param k number of columns to zero out below diagonal - * @param A matrix - * @param copy_vectors - true to copy Householder vectors below diagonal - * @return nothing: in place !!! - */ -void householder_(MatrixColMajor& A, size_t k, bool copy_vectors=true); - -/** - * Householder tranformation, zeros below diagonal - * Column Major version - * @param k number of columns to zero out below diagonal - * @param A matrix - * @return nothing: in place !!! - */ -void householder(MatrixColMajor& A, size_t k); - /** * backSubstitute U*x=b * @param U an upper triangular matrix @@ -405,7 +384,6 @@ Matrix collect(size_t nrMatrices, ...); * Arguments (Matrix, Vector) scales the columns, * (Vector, Matrix) scales the rows */ -void vector_scale_inplace(const Vector& v, MatrixColMajor& A); // row void vector_scale_inplace(const Vector& v, Matrix& A); // row Matrix vector_scale(const Vector& v, const Matrix& A); // row Matrix vector_scale(const Matrix& A, const Vector& v); // column @@ -489,7 +467,7 @@ Matrix expm(const Matrix& A, size_t K=7); namespace boost { namespace serialization { -// split version for Row-major matrix - sends sizes ahead +// split version - sends sizes ahead template void save(Archive & ar, const Matrix & m, unsigned int version) { @@ -512,32 +490,7 @@ void load(Archive & ar, Matrix & m, unsigned int version) std::copy(raw_data.begin(), raw_data.end(), m.data()); } -// split version for Column-major matrix - sends sizes ahead -template -void save(Archive & ar, const MatrixColMajor & m, unsigned int version) -{ - const int rows = m.rows(), cols = m.cols(), elements = rows*cols; - std::vector raw_data(elements); - std::copy(m.data(), m.data()+elements, raw_data.begin()); - ar << make_nvp("rows", rows); - ar << make_nvp("cols", cols); - ar << make_nvp("data", raw_data); -} -template -void load(Archive & ar, MatrixColMajor & m, unsigned int version) -{ - size_t rows, cols; - std::vector raw_data; - ar >> make_nvp("rows", rows); - ar >> make_nvp("cols", cols); - ar >> make_nvp("data", raw_data); - m = MatrixColMajor(rows, cols); - std::copy(raw_data.begin(), raw_data.end(), m.data()); -} - } // namespace serialization } // namespace boost BOOST_SERIALIZATION_SPLIT_FREE(Matrix) -BOOST_SERIALIZATION_SPLIT_FREE(MatrixColMajor) - diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 40158b079..e129a6d30 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -34,7 +34,7 @@ namespace gtsam { static const double underconstrainedPrior = 1e-5; /* ************************************************************************* */ -static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { +static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { // Get pivot value double alpha = ATA(k,k); @@ -76,7 +76,7 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) { } /* ************************************************************************* */ -pair choleskyCareful(MatrixColMajor& ATA, int order) { +pair choleskyCareful(Matrix& ATA, int order) { const bool debug = ISDEBUG("choleskyCareful"); @@ -112,7 +112,7 @@ pair choleskyCareful(MatrixColMajor& ATA, int order) { } /* ************************************************************************* */ -void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) { +void choleskyPartial(Matrix& ABC, size_t nFrontal) { const bool debug = ISDEBUG("choleskyPartial"); diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 1f0fa2862..48e1d5314 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -38,7 +38,7 @@ namespace gtsam { * The optional order argument specifies the size of the square upper-left * submatrix to operate on, ignoring the rest of the matrix. */ -std::pair choleskyCareful(MatrixColMajor& ATA, int order = -1); +std::pair choleskyCareful(Matrix& ATA, int order = -1); /** * Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B @@ -48,7 +48,7 @@ std::pair choleskyCareful(MatrixColMajor& ATA, int order = -1); * nFrontal determines the split between A, B, and C, with A being of size * nFrontal x nFrontal. */ -void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal); +void choleskyPartial(Matrix& ABC, size_t nFrontal); } diff --git a/gtsam/base/tests/testBlockMatrices.cpp b/gtsam/base/tests/testBlockMatrices.cpp index 2a140bc5d..09b04d4ac 100644 --- a/gtsam/base/tests/testBlockMatrices.cpp +++ b/gtsam/base/tests/testBlockMatrices.cpp @@ -12,7 +12,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor1) { - typedef MatrixColMajor AbMatrix; + typedef Matrix AbMatrix; typedef VerticalBlockView BlockAb; AbMatrix matrix; // actual matrix - empty to start with @@ -47,7 +47,7 @@ TEST(testBlockMatrices, jacobian_factor1) { /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor2) { - typedef MatrixColMajor AbMatrix; + typedef Matrix AbMatrix; typedef VerticalBlockView BlockAb; AbMatrix matrix; // actual matrix - empty to start with @@ -88,7 +88,7 @@ TEST(testBlockMatrices, jacobian_factor2) { /* ************************************************************************* */ TEST(testBlockMatrices, hessian_factor1) { - typedef MatrixColMajor InfoMatrix; + typedef Matrix InfoMatrix; typedef SymmetricBlockView BlockInfo; Matrix expected_full = Matrix_(3, 3, diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index c095d7c71..c6bdc5cfd 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -37,19 +37,19 @@ TEST(cholesky, choleskyPartial) { 0., 0., 0., 0., 0., 0., 2.5776); // Do partial Cholesky on 3 frontal scalar variables - MatrixColMajor RSL(ABC); + Matrix RSL(ABC); choleskyPartial(RSL, 3); // See the function comment for choleskyPartial, this decomposition should hold. - MatrixColMajor R1 = RSL.transpose(); - MatrixColMajor R2 = RSL; + Matrix R1 = RSL.transpose(); + Matrix R2 = RSL; R1.block(3, 3, 4, 4).setIdentity(); R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView(); - MatrixColMajor actual = R1 * R2; + Matrix actual = R1 * R2; - MatrixColMajor expected = ABC.selfadjointView(); + Matrix expected = ABC.selfadjointView(); EXPECT(assert_equal(expected, actual, 1e-9)); } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index c0a11257d..0e8dff261 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -26,6 +26,7 @@ using namespace std; using namespace gtsam; static double inf = std::numeric_limits::infinity(); +static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) @@ -68,14 +69,14 @@ TEST( matrix, Matrix_ ) } /* ************************************************************************* */ -TEST( matrix, row_major ) +TEST( matrix, col_major ) { Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); const double * const a = &A(0, 0); - EXPECT(a[0] == 1); - EXPECT(a[1] == 2); - EXPECT(a[2] == 3); - EXPECT(a[3] == 4); + EXPECT_DOUBLES_EQUAL(1, a[0], tol); + EXPECT_DOUBLES_EQUAL(3, a[1], tol); + EXPECT_DOUBLES_EQUAL(2, a[2], tol); + EXPECT_DOUBLES_EQUAL(4, a[3], tol); } /* ************************************************************************* */ @@ -528,13 +529,13 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual1r); EXPECT(assert_equal(expected1, actual1r, 1e-10)); - MatrixColMajor actual1c = A1; + Matrix actual1c = A1; zeroBelowDiagonal(actual1c); - EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10)); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); actual1c = A1; zeroBelowDiagonal(actual1c, 4); - EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10)); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); Matrix A2 = Matrix_(5, 3, 1.0, 2.0, 3.0, @@ -553,9 +554,9 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual2r); EXPECT(assert_equal(expected2, actual2r, 1e-10)); - MatrixColMajor actual2c = A2; + Matrix actual2c = A2; zeroBelowDiagonal(actual2c); - EXPECT(assert_equal(MatrixColMajor(expected2), actual2c, 1e-10)); + EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); Matrix expected2_partial = Matrix_(5, 3, 1.0, 2.0, 3.0, @@ -565,7 +566,7 @@ TEST( matrix, zero_below_diagonal ) { 0.0, 2.0, 3.0); actual2c = A2; zeroBelowDiagonal(actual2c, 1); - EXPECT(assert_equal(MatrixColMajor(expected2_partial), actual2c, 1e-10)); + EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); } /* ************************************************************************* */ @@ -714,8 +715,8 @@ TEST( matrix, householder_colMajor ) 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - MatrixColMajor expected1(Matrix_(4, 7, data1)); - MatrixColMajor A1(Matrix_(4, 7, data)); + Matrix expected1(Matrix_(4, 7, data1)); + Matrix A1(Matrix_(4, 7, data)); householder_(A1, 3); EXPECT(assert_equal(expected1, A1, 1e-3)); @@ -724,8 +725,8 @@ TEST( matrix, householder_colMajor ) 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; - MatrixColMajor expected(Matrix_(4, 7, data2)); - MatrixColMajor A2(Matrix_(4, 7, data)); + Matrix expected(Matrix_(4, 7, data2)); + Matrix A2(Matrix_(4, 7, data)); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -745,15 +746,15 @@ TEST( matrix, eigen_QR ) 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; - MatrixColMajor expected(Matrix_(4, 7, data2)); - MatrixColMajor A(Matrix_(4, 7, data)); - MatrixColMajor actual = A.householderQr().matrixQR(); + Matrix expected(Matrix_(4, 7, data2)); + Matrix A(Matrix_(4, 7, data)); + Matrix actual = A.householderQr().matrixQR(); zeroBelowDiagonal(actual); EXPECT(assert_equal(expected, actual, 1e-3)); // use shiny new in place QR inside gtsam - A = MatrixColMajor(Matrix_(4, 7, data)); + A = Matrix(Matrix_(4, 7, data)); inplace_QR(A); EXPECT(assert_equal(expected, A, 1e-3)); } @@ -808,11 +809,11 @@ TEST( matrix, trans ) } /* ************************************************************************* */ -TEST( matrix, row_major_access ) +TEST( matrix, col_major_access ) { Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); const double* a = &A(0, 0); - DOUBLES_EQUAL(3,a[2],1e-9); + DOUBLES_EQUAL(2.0,a[2],1e-9); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 548054e28..9d09c7947 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -185,12 +185,10 @@ namespace gtsam { if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - H1->resize(3,3); - double data[9] = { + *H1 = Matrix_(3,3, -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0, -1.0}; - copy(data, data+9, H1->data()); + s, -c, dt2, + 0.0, 0.0,-1.0); } if (H2) *H2 = I3; diff --git a/gtsam/geometry/Tensor2.h b/gtsam/geometry/Tensor2.h index 29a7c26f2..75f07f2a3 100644 --- a/gtsam/geometry/Tensor2.h +++ b/gtsam/geometry/Tensor2.h @@ -33,7 +33,7 @@ public: Tensor2() { } - /* construct from data */ + /* construct from data - expressed in row major form */ Tensor2(const double data[N2][N1]) { for (int j = 0; j < N2; j++) T[j] = Tensor1 (data[j]); diff --git a/gtsam/geometry/tensorInterface.h b/gtsam/geometry/tensorInterface.h index 73b21956e..dbea2472e 100644 --- a/gtsam/geometry/tensorInterface.h +++ b/gtsam/geometry/tensorInterface.h @@ -28,12 +28,12 @@ namespace gtsam { Matrix reshape(const tensors::Tensor2Expression& T, int m, int n) { if (m * n != I::dim * J::dim) throw std::invalid_argument( "reshape: incompatible dimensions"); - Matrix M(m, n); + MatrixRowMajor M(m, n); size_t t = 0; for (int j = 0; j < J::dim; j++) for (int i = 0; i < I::dim; i++) M.data()[t++] = T(i, j); - return M; + return Matrix(M); } /** Reshape rank 2 tensor into Vector */ @@ -68,8 +68,8 @@ namespace gtsam { Matrix M(m, n); int t = 0; for (int k = 0; k < K::dim; k++) - for (int j = 0; j < J::dim; j++) - for (int i = 0; i < I::dim; i++) + for (int i = 0; i < I::dim; i++) + for (int j = 0; j < J::dim; j++) M.data()[t++] = T(i, j, k); return M; } @@ -99,8 +99,8 @@ namespace gtsam { for (int m = 0; m < M::dim; m++) for (int l = 0; l < L::dim; l++) for (int k = 0; k < K::dim; k++) - for (int j = 0; j < J::dim; j++) - for (int i = 0; i < I::dim; i++) + for (int i = 0; i < I::dim; i++) + for (int j = 0; j < J::dim; j++) R.data()[t++] = T(i, j, k, l, m); return R; } diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 846a4476c..b1403dffc 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index d131a8618..1c1fe39ec 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -22,7 +22,6 @@ #include #include #include -#include using namespace std; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 3619e653c..9b5e56287 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -47,7 +47,7 @@ public: typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ - typedef MatrixColMajor AbMatrix; + typedef Matrix AbMatrix; typedef VerticalBlockView rsd_type; typedef rsd_type::Block r_type; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 652ab28ef..4a4dc886e 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -173,7 +173,7 @@ void HessianFactor::print(const std::string& s) const { for(const_iterator key=this->begin(); key!=this->end(); ++key) cout << *key << "(" << this->getDim(key) << ") "; cout << "\n"; - gtsam::print(MatrixColMajor(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); } /* ************************************************************************* */ @@ -181,9 +181,9 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { if(!dynamic_cast(&lf)) return false; else { - MatrixColMajor thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = this->info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - MatrixColMajor rhsMatrix = static_cast(lf).info_.full().selfadjointView(); + Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); } @@ -421,7 +421,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key // Extract conditionals tic(1, "extract conditionals"); GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet()); - typedef VerticalBlockView BlockAb; + typedef VerticalBlockView BlockAb; BlockAb Ab(matrix_, info_); for(size_t j=0; j BlockInfo; InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d79d58205..4e60dc1ef 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -44,7 +44,7 @@ namespace gtsam { class JacobianFactor : public GaussianFactor { public: - typedef MatrixColMajor AbMatrix; + typedef Matrix AbMatrix; typedef VerticalBlockView BlockAb; protected: diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 81c932d1b..0a51ad966 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -109,11 +109,6 @@ void Gaussian::WhitenInPlace(Matrix& H) const { H = thisR() * H; } -/* ************************************************************************* */ -void Gaussian::WhitenInPlace(MatrixColMajor& H) const { - H = thisR() * H; -} - /* ************************************************************************* */ // General QR, see also special version in Constrained SharedDiagonal Gaussian::QR(Matrix& Ab) const { @@ -141,34 +136,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { } /* ************************************************************************* */ -// General QR, see also special version in Constrained -SharedDiagonal Gaussian::QR(MatrixColMajor& Ab) const { - - static const bool debug = false; - - // get size(A) and maxRank - // TODO: really no rank problems ? - size_t m = Ab.rows(), n = Ab.cols()-1; - size_t maxRank = min(m,n); - - // pre-whiten everything (cheaply if possible) - WhitenInPlace(Ab); - - if(debug) gtsam::print(Matrix(Ab), "Whitened Ab: "); - - // Eigen QR - much faster than older householder approach - inplace_QR(Ab, false); - - // hand-coded householder implementation - // TODO: necessary to isolate last column? -// householder(Ab, maxRank); - - return Unit::Create(maxRank); -} - - -/* ************************************************************************* */ -SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const { +SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const { // get size(A) and maxRank // TODO: really no rank problems ? @@ -271,11 +239,6 @@ void Diagonal::WhitenInPlace(Matrix& H) const { vector_scale_inplace(invsigmas(), H); } -/* ************************************************************************* */ -void Diagonal::WhitenInPlace(MatrixColMajor& H) const { - vector_scale_inplace(invsigmas(), H); -} - /* ************************************************************************* */ Vector Diagonal::sample() const { Vector result(dim_); @@ -312,15 +275,11 @@ void Constrained::WhitenInPlace(Matrix& H) const { } /* ************************************************************************* */ -void Constrained::WhitenInPlace(MatrixColMajor& H) const { - throw logic_error("noiseModel::Constrained cannot Whiten"); -} - -/* ************************************************************************* */ -// templated generic constrained QR -template -SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) { - +// Special version of QR for Constrained calls slower but smarter code +// that deals with possibly zero sigmas +// It is Gram-Schmidt orthogonalization rather than Householder +// Previously Diagonal::QR +SharedDiagonal Constrained::QR(Matrix& Ab) const { bool verbose = false; if (verbose) cout << "\nStarting Constrained::QR" << endl; @@ -333,7 +292,7 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas); + Vector invsigmas = reciprocal(sigmas_); Vector weights = emul(invsigmas,invsigmas); // calculate weights once // We loop over all columns, because the columns that can be eliminated @@ -372,7 +331,6 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) { tic(3, "constrained_QR update Ab"); Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); toc(3, "constrained_QR update Ab"); -// updateAb(Ab, j, a, rd); } // Create storage for precisions @@ -388,8 +346,9 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) { const Vector& rd = t.get<1>(); precisions(i) = t.get<2>(); if (precisions(i)==inf) mixed = true; - for (size_t j2=0; j2& firstZeroRows) const; +// virtual SharedDiagonal QRColumnWise(Matrix& Ab, std::vector& firstZeroRows) const; // virtual SharedDiagonal QR(Matrix& Ab, boost::optional&> firstZeroRows = boost::none) const; /** * Cholesky factorization */ - virtual SharedDiagonal Cholesky(MatrixColMajor& Ab, size_t nFrontals) const; + virtual SharedDiagonal Cholesky(Matrix& Ab, size_t nFrontals) const; /** * Return R itself, but note that Whiten(H) is cheaper than R*H @@ -267,7 +265,6 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; - virtual void WhitenInPlace(MatrixColMajor& H) const; /** * Return standard deviations (sqrt of diagonal) @@ -369,13 +366,11 @@ namespace gtsam { virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; - virtual void WhitenInPlace(MatrixColMajor& H) const; /** * Apply QR factorization to the system [A b], taking into account constraints */ virtual SharedDiagonal QR(Matrix& Ab) const; - virtual SharedDiagonal QR(MatrixColMajor& Ab) const; /** * Check constrained is always true @@ -439,7 +434,6 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; - virtual void WhitenInPlace(MatrixColMajor& H) const; /** * Return standard deviation diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 4b097a81c..7eb18c0d8 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -113,8 +113,8 @@ TEST(HessianFactor, Constructor1) HessianFactor factor(0, G, g, f); // extract underlying parts - MatrixColMajor info_matrix = factor.info_.range(0, 1, 0, 1); - EXPECT(assert_equal(MatrixColMajor(G), info_matrix)); + Matrix info_matrix = factor.info_.range(0, 1, 0, 1); + EXPECT(assert_equal(Matrix(G), info_matrix)); EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); EXPECT_LONGS_EQUAL(1, factor.size()); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 178b5c4dd..ab3571803 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -212,13 +212,13 @@ TEST( NoiseModel, QR ) //TEST( NoiseModel, QRColumnWise ) //{ // // Call Gaussian version -// MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! +// Matrix Ab = exampleQR::Ab; // otherwise overwritten ! // vector firstZeroRows; // firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-( // SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); // SharedDiagonal expected = noiseModel::Unit::Create(4); // EXPECT(assert_equal(*expected,*actual)); -// Matrix AbResized = ublas::triangular_adaptor(Ab); +// Matrix AbResized = ublas::triangular_adaptor(Ab); // print(exampleQR::Rd, "Rd: "); // print(Ab, "Ab: "); // print(AbResized, "AbResized: "); @@ -229,12 +229,12 @@ TEST( NoiseModel, QR ) TEST(NoiseModel, Cholesky) { SharedDiagonal expected = noiseModel::Unit::Create(4); - MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! + Matrix Ab = exampleQR::Ab; // otherwise overwritten ! SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); // ASSERTION FAILURE: access out of bounds EXPECT(assert_equal(*expected,*actual)); // Ab was modified in place !!! - MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView(); -// ublas::project(ublas::triangular_adaptor(Ab), + Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView(); +// ublas::project(ublas::triangular_adaptor(Ab), // ublas::range(0,actual->dim()), ublas::range(0, Ab.cols())); EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test }