12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changed the default Matrix to use column major, rather than row major. Removed Matrix-inl.h, as it isn't used

release/4.3a0
Alex Cunningham 2011-06-04 22:15:23 +00:00
parent 36f9ebae90
commit a22586362b
22 changed files with 96 additions and 434 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Makefile.am
View File

@ -13,7 +13,7 @@ check_PROGRAMS =
# base Math # 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 += Vector.cpp Matrix.cpp
sources += cholesky.cpp sources += cholesky.cpp
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices

107
gtsam/base/Matrix-inl.h
View File

@ -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 <gtsam/base/Matrix.h>
#include <iostream>
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<class MATRIX, class VECTOR>
//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false) {
//// const MATRIX& U(*static_cast<const MATRIX*>(&_U));
//// const VECTOR& b(*static_cast<const VECTOR*>(&_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<double>::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<class VECTOR, class MATRIX>
//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false) {
//// const VECTOR& b(*static_cast<const VECTOR*>(&_b));
//// const MATRIX& U(*static_cast<const MATRIX*>(&_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<double>::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;
//}
}

145
gtsam/base/Matrix.cpp
View File

@ -28,23 +28,19 @@
#include <gtsam/3rdparty/Eigen/Dense> #include <gtsam/3rdparty/Eigen/Dense>
#include <gtsam/3rdparty/Eigen/SVD> #include <gtsam/3rdparty/Eigen/SVD>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/Matrix-inl.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiations of template functions for standard types */
//template Vector backSubstituteUpper<Matrix,Vector>(const Matrix& U, const Vector& b, bool unit);
//template Vector backSubstituteUpper<Vector,Matrix>(const Vector& b, const Matrix& U, bool unit);
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Matrix_( size_t m, size_t n, const double* const data) { 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()); 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; 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<Matrix>& As, const std::list<Matrix>& Bs, double tol) { bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol) {
if (As.size() != Bs.size()) return false; if (As.size() != Bs.size()) return false;
@ -250,25 +227,6 @@ void print(const Matrix& A, const string &s, ostream& stream) {
stream << "];" << endl; 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) { void save(const Matrix& A, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app); 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()) ; Vector v (A.cols()) ;
for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) {
v[i] = A.col(i).dot(A.col(i)); v[i] = A.col(i).dot(A.col(i));
@ -369,33 +327,6 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
return make_pair(Q,R); 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<boost::tuple<Vector, double, double> > list<boost::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { 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. // Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (size_t j=0; j<n; ++j) { for (size_t j=0; j<n; ++j) {
// extract the first column of A // extract the first column of A
Vector a(column(A, j)); // ublas::matrix_column is slower ! Vector a(column(A, j));
// Calculate weighted pseudo-inverse and corresponding precision // Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo); double precision = weightedPseudoinverse(a, weights, pseudo);
// if precision is zero, no information on this column // if precision is zero, no information on this column
if (precision < 1e-8) continue; if (precision < 1e-8) continue;
@ -440,7 +370,8 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
// update A, b, expensive, using outer product // update A, b, expensive, using outer product
// A' \define A_{S}-a*r and b'\define b-d*a // A' \define A_{S}-a*r and b'\define b-d*a
updateAb(A, b, j, a, r, d); A -= a * r.transpose();
b -= d * a;
} }
return results; return results;
@ -450,38 +381,9 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
/** Imperative version of Householder QR factorization, Golub & Van Loan p 224 /** Imperative version of Householder QR factorization, Golub & Van Loan p 224
* version with Householder vectors below diagonal, as in GVL * version with Householder vectors below diagonal, as in GVL
*/ */
/* ************************************************************************* */
void householder_(Matrix &A, size_t k)
{
const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
// loop over the kprime first columns
for(size_t j=0; j < kprime; j++){
// copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j)
Vector vjm = A.col(j).segment(j, m-j);
// calculate the Householder vector, in place
double beta = houseInPlace(vjm);
// do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
Vector w = beta * A.middleRows(j,m-j).transpose() * vjm;
A.middleRows(j,m-j) -= vjm * w.transpose();
// the Householder vector is copied in the zeroed out part
A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1));
} // column j
}
/* ************************************************************************* */ /* ************************************************************************* */
void householder(Matrix &A, size_t k) { void householder_(Matrix& A, size_t k, bool copy_vectors) {
// version with zeros below diagonal
householder_(A,k);
const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
for(size_t j=0; j < kprime; j++)
A.col(j).segment(j+1, m-(j+1)).setZero();
}
/* ************************************************************************* */
void householder_(MatrixColMajor& A, size_t k, bool copy_vectors) {
const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
// loop over the kprime first columns // loop over the kprime first columns
for(size_t j=0; j < kprime; j++) { for(size_t j=0; j < kprime; j++) {
@ -508,7 +410,7 @@ void householder_(MatrixColMajor& A, size_t k, bool copy_vectors) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void householder(MatrixColMajor& A, size_t k) { void householder(Matrix& A, size_t k) {
// version with zeros below diagonal // version with zeros below diagonal
tic(1, "householder_"); tic(1, "householder_");
householder_(A,k,false); householder_(A,k,false);
@ -638,21 +540,9 @@ Matrix collect(size_t nrMatrices, ...)
/* ************************************************************************* */ /* ************************************************************************* */
// row scaling, in-place // row scaling, in-place
void vector_scale_inplace(const Vector& v, Matrix& A) { void vector_scale_inplace(const Vector& v, Matrix& A) {
size_t m = A.rows(); size_t n = A.cols(); const size_t m = A.rows();
for (size_t i=0; i<m; ++i) { // loop over rows for (size_t i=0; i<m; ++i)
double vi = v(i); A.row(i) *= v(i);
double *Aij = A.data() + i*n;
for (size_t j=0; j<n; ++j, ++Aij) (*Aij) *= vi;
}
}
/* ************************************************************************* */
// row scaling, in-place
void vector_scale_inplace(const Vector& v, MatrixColMajor& A) {
size_t m = A.rows(); size_t n = A.cols();
double *Aij = A.data();
for (size_t j=0; j<n; ++j) // loop over rows
for (size_t i=0; i<m; ++i, ++Aij) (*Aij) *= v(i);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -667,14 +557,9 @@ Matrix vector_scale(const Vector& v, const Matrix& A) {
// column scaling // column scaling
Matrix vector_scale(const Matrix& A, const Vector& v) { Matrix vector_scale(const Matrix& A, const Vector& v) {
Matrix M(A); Matrix M(A);
size_t m = A.rows(); size_t n = A.cols(); const size_t n = A.cols();
const double * vptr = v.data(); for (size_t j=0; j<n; ++j)
for (size_t i=0; i<m; ++i) { // loop over rows M.col(j) *= v(j);
for (size_t j=0; j<n; ++j) { // loop over columns
double * Mptr = M.data() + i*n + j;
(*Mptr) = (*Mptr) * *(vptr+j);
}
}
return M; return M;
} }

65
gtsam/base/Matrix.h
View File

@ -30,11 +30,11 @@
/** /**
* Matrix is a *global* typedef * Matrix is a *global* typedef
* we use the default < double,row_major,unbounded_array<double> > * we use the default < double,col_major,unbounded_array<double> >
*/ */
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix; typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixColMajor; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;
@ -109,9 +109,7 @@ inline bool operator!=(const Matrix& A, const Matrix& B) {
/** /**
* equals with an tolerance, prints out message if unequal * 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 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 * equals with an tolerance, prints out message if unequal
@ -175,7 +173,6 @@ Vector Vector_(const Matrix& A);
* print a matrix * print a matrix
*/ */
void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout); 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 * 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 j);
void insertColumn(Matrix& A, const Vector& col, size_t i, 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 * 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 cols = A.cols();
size_t size = std::min(rows,cols); size_t size = std::min(rows,cols);
typedef Eigen::internal::plain_diag_type<MatrixColMajor>::type HCoeffsType; typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
typedef Eigen::internal::plain_row_type<MatrixColMajor>::type RowVectorType; typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
HCoeffsType hCoeffs(size); HCoeffsType hCoeffs(size);
RowVectorType temp(cols); RowVectorType temp(cols);
@ -315,9 +312,10 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
* Householder tranformation, Householder vectors below diagonal * Householder tranformation, Householder vectors below diagonal
* @param k number of columns to zero out below diagonal * @param k number of columns to zero out below diagonal
* @param A matrix * @param A matrix
* @param copy_vectors - true to copy Householder vectors below diagonal
* @return nothing: in place !!! * @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 * Householder tranformation, zeros below diagonal
@ -327,25 +325,6 @@ void householder_(Matrix& A, size_t k);
*/ */
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 * backSubstitute U*x=b
* @param U an upper triangular matrix * @param U an upper triangular matrix
@ -405,7 +384,6 @@ Matrix collect(size_t nrMatrices, ...);
* Arguments (Matrix, Vector) scales the columns, * Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows * (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 void vector_scale_inplace(const Vector& v, Matrix& A); // row
Matrix vector_scale(const Vector& v, const Matrix& A); // row Matrix vector_scale(const Vector& v, const Matrix& A); // row
Matrix vector_scale(const Matrix& A, const Vector& v); // column 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 boost {
namespace serialization { namespace serialization {
// split version for Row-major matrix - sends sizes ahead // split version - sends sizes ahead
template<class Archive> template<class Archive>
void save(Archive & ar, const Matrix & m, unsigned int version) 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()); std::copy(raw_data.begin(), raw_data.end(), m.data());
} }
// split version for Column-major matrix - sends sizes ahead
template<class Archive>
void save(Archive & ar, const MatrixColMajor & m, unsigned int version)
{
const int rows = m.rows(), cols = m.cols(), elements = rows*cols;
std::vector<double> 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<class Archive>
void load(Archive & ar, MatrixColMajor & m, unsigned int version)
{
size_t rows, cols;
std::vector<double> 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 serialization
} // namespace boost } // namespace boost
BOOST_SERIALIZATION_SPLIT_FREE(Matrix) BOOST_SERIALIZATION_SPLIT_FREE(Matrix)
BOOST_SERIALIZATION_SPLIT_FREE(MatrixColMajor)

6
gtsam/base/cholesky.cpp
View File

@ -34,7 +34,7 @@ namespace gtsam {
static const double underconstrainedPrior = 1e-5; 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 // Get pivot value
double alpha = ATA(k,k); double alpha = ATA(k,k);
@ -76,7 +76,7 @@ static inline bool choleskyStep(MatrixColMajor& ATA, size_t k, size_t order) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order) { pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {
const bool debug = ISDEBUG("choleskyCareful"); const bool debug = ISDEBUG("choleskyCareful");
@ -112,7 +112,7 @@ pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal) { void choleskyPartial(Matrix& ABC, size_t nFrontal) {
const bool debug = ISDEBUG("choleskyPartial"); const bool debug = ISDEBUG("choleskyPartial");

4
gtsam/base/cholesky.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
* The optional order argument specifies the size of the square upper-left * The optional order argument specifies the size of the square upper-left
* submatrix to operate on, ignoring the rest of the matrix. * submatrix to operate on, ignoring the rest of the matrix.
*/ */
std::pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order = -1); std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
/** /**
* Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B * Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B
@ -48,7 +48,7 @@ std::pair<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order = -1);
* nFrontal determines the split between A, B, and C, with A being of size * nFrontal determines the split between A, B, and C, with A being of size
* nFrontal x nFrontal. * nFrontal x nFrontal.
*/ */
void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal); void choleskyPartial(Matrix& ABC, size_t nFrontal);
} }

6
gtsam/base/tests/testBlockMatrices.cpp
View File

@ -12,7 +12,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testBlockMatrices, jacobian_factor1) { TEST(testBlockMatrices, jacobian_factor1) {
typedef MatrixColMajor AbMatrix; typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb; typedef VerticalBlockView<AbMatrix> BlockAb;
AbMatrix matrix; // actual matrix - empty to start with AbMatrix matrix; // actual matrix - empty to start with
@ -47,7 +47,7 @@ TEST(testBlockMatrices, jacobian_factor1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testBlockMatrices, jacobian_factor2) { TEST(testBlockMatrices, jacobian_factor2) {
typedef MatrixColMajor AbMatrix; typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb; typedef VerticalBlockView<AbMatrix> BlockAb;
AbMatrix matrix; // actual matrix - empty to start with AbMatrix matrix; // actual matrix - empty to start with
@ -88,7 +88,7 @@ TEST(testBlockMatrices, jacobian_factor2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testBlockMatrices, hessian_factor1) { TEST(testBlockMatrices, hessian_factor1) {
typedef MatrixColMajor InfoMatrix; typedef Matrix InfoMatrix;
typedef SymmetricBlockView<InfoMatrix> BlockInfo; typedef SymmetricBlockView<InfoMatrix> BlockInfo;
Matrix expected_full = Matrix_(3, 3, Matrix expected_full = Matrix_(3, 3,

10
gtsam/base/tests/testCholesky.cpp
View File

@ -37,19 +37,19 @@ TEST(cholesky, choleskyPartial) {
0., 0., 0., 0., 0., 0., 2.5776); 0., 0., 0., 0., 0., 0., 2.5776);
// Do partial Cholesky on 3 frontal scalar variables // Do partial Cholesky on 3 frontal scalar variables
MatrixColMajor RSL(ABC); Matrix RSL(ABC);
choleskyPartial(RSL, 3); choleskyPartial(RSL, 3);
// See the function comment for choleskyPartial, this decomposition should hold. // See the function comment for choleskyPartial, this decomposition should hold.
MatrixColMajor R1 = RSL.transpose(); Matrix R1 = RSL.transpose();
MatrixColMajor R2 = RSL; Matrix R2 = RSL;
R1.block(3, 3, 4, 4).setIdentity(); R1.block(3, 3, 4, 4).setIdentity();
R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>(); R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>();
MatrixColMajor actual = R1 * R2; Matrix actual = R1 * R2;
MatrixColMajor expected = ABC.selfadjointView<Eigen::Upper>(); Matrix expected = ABC.selfadjointView<Eigen::Upper>();
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }

43
gtsam/base/tests/testMatrix.cpp
View File

@ -26,6 +26,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
static const double tol = 1e-9;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, constructor_data ) 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); Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
const double * const a = &A(0, 0); const double * const a = &A(0, 0);
EXPECT(a[0] == 1); EXPECT_DOUBLES_EQUAL(1, a[0], tol);
EXPECT(a[1] == 2); EXPECT_DOUBLES_EQUAL(3, a[1], tol);
EXPECT(a[2] == 3); EXPECT_DOUBLES_EQUAL(2, a[2], tol);
EXPECT(a[3] == 4); EXPECT_DOUBLES_EQUAL(4, a[3], tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -528,13 +529,13 @@ TEST( matrix, zero_below_diagonal ) {
zeroBelowDiagonal(actual1r); zeroBelowDiagonal(actual1r);
EXPECT(assert_equal(expected1, actual1r, 1e-10)); EXPECT(assert_equal(expected1, actual1r, 1e-10));
MatrixColMajor actual1c = A1; Matrix actual1c = A1;
zeroBelowDiagonal(actual1c); zeroBelowDiagonal(actual1c);
EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10)); EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10));
actual1c = A1; actual1c = A1;
zeroBelowDiagonal(actual1c, 4); zeroBelowDiagonal(actual1c, 4);
EXPECT(assert_equal(MatrixColMajor(expected1), actual1c, 1e-10)); EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10));
Matrix A2 = Matrix_(5, 3, Matrix A2 = Matrix_(5, 3,
1.0, 2.0, 3.0, 1.0, 2.0, 3.0,
@ -553,9 +554,9 @@ TEST( matrix, zero_below_diagonal ) {
zeroBelowDiagonal(actual2r); zeroBelowDiagonal(actual2r);
EXPECT(assert_equal(expected2, actual2r, 1e-10)); EXPECT(assert_equal(expected2, actual2r, 1e-10));
MatrixColMajor actual2c = A2; Matrix actual2c = A2;
zeroBelowDiagonal(actual2c); zeroBelowDiagonal(actual2c);
EXPECT(assert_equal(MatrixColMajor(expected2), actual2c, 1e-10)); EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10));
Matrix expected2_partial = Matrix_(5, 3, Matrix expected2_partial = Matrix_(5, 3,
1.0, 2.0, 3.0, 1.0, 2.0, 3.0,
@ -565,7 +566,7 @@ TEST( matrix, zero_below_diagonal ) {
0.0, 2.0, 3.0); 0.0, 2.0, 3.0);
actual2c = A2; actual2c = A2;
zeroBelowDiagonal(actual2c, 1); 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, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
-0.618034, 0, 4.4721, 0, -4.4721, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 };
MatrixColMajor expected1(Matrix_(4, 7, data1)); Matrix expected1(Matrix_(4, 7, data1));
MatrixColMajor A1(Matrix_(4, 7, data)); Matrix A1(Matrix_(4, 7, data));
householder_(A1, 3); householder_(A1, 3);
EXPECT(assert_equal(expected1, A1, 1e-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, 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, -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 }; 0, 0, 4.4721, 0, -4.4721, 0.894 };
MatrixColMajor expected(Matrix_(4, 7, data2)); Matrix expected(Matrix_(4, 7, data2));
MatrixColMajor A2(Matrix_(4, 7, data)); Matrix A2(Matrix_(4, 7, data));
householder(A2, 3); householder(A2, 3);
EXPECT(assert_equal(expected, A2, 1e-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, 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, -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 }; 0, 0, 4.4721, 0, -4.4721, 0.894 };
MatrixColMajor expected(Matrix_(4, 7, data2)); Matrix expected(Matrix_(4, 7, data2));
MatrixColMajor A(Matrix_(4, 7, data)); Matrix A(Matrix_(4, 7, data));
MatrixColMajor actual = A.householderQr().matrixQR(); Matrix actual = A.householderQr().matrixQR();
zeroBelowDiagonal(actual); zeroBelowDiagonal(actual);
EXPECT(assert_equal(expected, actual, 1e-3)); EXPECT(assert_equal(expected, actual, 1e-3));
// use shiny new in place QR inside gtsam // use shiny new in place QR inside gtsam
A = MatrixColMajor(Matrix_(4, 7, data)); A = Matrix(Matrix_(4, 7, data));
inplace_QR(A); inplace_QR(A);
EXPECT(assert_equal(expected, A, 1e-3)); 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); Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
const double* a = &A(0, 0); const double* a = &A(0, 0);
DOUBLES_EQUAL(3,a[2],1e-9); DOUBLES_EQUAL(2.0,a[2],1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/geometry/Pose2.cpp
View File

@ -185,12 +185,10 @@ namespace gtsam {
if (H1) { if (H1) {
double dt1 = -s2 * x + c2 * y; double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y; double dt2 = -c2 * x - s2 * y;
H1->resize(3,3); *H1 = Matrix_(3,3,
double data[9] = {
-c, -s, dt1, -c, -s, dt1,
s, -c, dt2, s, -c, dt2,
0.0, 0.0, -1.0}; 0.0, 0.0,-1.0);
copy(data, data+9, H1->data());
} }
if (H2) *H2 = I3; if (H2) *H2 = I3;

2
gtsam/geometry/Tensor2.h
View File

@ -33,7 +33,7 @@ public:
Tensor2() { Tensor2() {
} }
/* construct from data */ /* construct from data - expressed in row major form */
Tensor2(const double data[N2][N1]) { Tensor2(const double data[N2][N1]) {
for (int j = 0; j < N2; j++) for (int j = 0; j < N2; j++)
T[j] = Tensor1<N1> (data[j]); T[j] = Tensor1<N1> (data[j]);

12
gtsam/geometry/tensorInterface.h
View File

@ -28,12 +28,12 @@ namespace gtsam {
Matrix reshape(const tensors::Tensor2Expression<A, I, J>& T, int m, int n) { Matrix reshape(const tensors::Tensor2Expression<A, I, J>& T, int m, int n) {
if (m * n != I::dim * J::dim) throw std::invalid_argument( if (m * n != I::dim * J::dim) throw std::invalid_argument(
"reshape: incompatible dimensions"); "reshape: incompatible dimensions");
Matrix M(m, n); MatrixRowMajor M(m, n);
size_t t = 0; size_t t = 0;
for (int j = 0; j < J::dim; j++) for (int j = 0; j < J::dim; j++)
for (int i = 0; i < I::dim; i++) for (int i = 0; i < I::dim; i++)
M.data()[t++] = T(i, j); M.data()[t++] = T(i, j);
return M; return Matrix(M);
} }
/** Reshape rank 2 tensor into Vector */ /** Reshape rank 2 tensor into Vector */
@ -68,8 +68,8 @@ namespace gtsam {
Matrix M(m, n); Matrix M(m, n);
int t = 0; int t = 0;
for (int k = 0; k < K::dim; k++) 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); M.data()[t++] = T(i, j, k);
return M; return M;
} }
@ -99,8 +99,8 @@ namespace gtsam {
for (int m = 0; m < M::dim; m++) for (int m = 0; m < M::dim; m++)
for (int l = 0; l < L::dim; l++) for (int l = 0; l < L::dim; l++)
for (int k = 0; k < K::dim; k++) 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); R.data()[t++] = T(i, j, k, l, m);
return R; return R;
} }

1
gtsam/linear/GaussianBayesNet.cpp
View File

@ -19,7 +19,6 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <gtsam/base/Matrix-inl.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>

1
gtsam/linear/GaussianConditional.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/base/Matrix-inl.h>
using namespace std; using namespace std;

2
gtsam/linear/GaussianConditional.h
View File

@ -47,7 +47,7 @@ public:
typedef boost::shared_ptr<GaussianConditional> shared_ptr; typedef boost::shared_ptr<GaussianConditional> shared_ptr;
/** Store the conditional matrix as upper-triangular column-major */ /** Store the conditional matrix as upper-triangular column-major */
typedef MatrixColMajor AbMatrix; typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> rsd_type; typedef VerticalBlockView<AbMatrix> rsd_type;
typedef rsd_type::Block r_type; typedef rsd_type::Block r_type;

8
gtsam/linear/HessianFactor.cpp
View File

@ -173,7 +173,7 @@ void HessianFactor::print(const std::string& s) const {
for(const_iterator key=this->begin(); key!=this->end(); ++key) for(const_iterator key=this->begin(); key!=this->end(); ++key)
cout << *key << "(" << this->getDim(key) << ") "; cout << *key << "(" << this->getDim(key) << ") ";
cout << "\n"; cout << "\n";
gtsam::print(MatrixColMajor(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: "); gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -181,9 +181,9 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if(!dynamic_cast<const HessianFactor*>(&lf)) if(!dynamic_cast<const HessianFactor*>(&lf))
return false; return false;
else { else {
MatrixColMajor thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>(); Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
MatrixColMajor rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>(); Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
} }
@ -421,7 +421,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
// Extract conditionals // Extract conditionals
tic(1, "extract conditionals"); tic(1, "extract conditionals");
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet()); GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
typedef VerticalBlockView<MatrixColMajor> BlockAb; typedef VerticalBlockView<Matrix> BlockAb;
BlockAb Ab(matrix_, info_); BlockAb Ab(matrix_, info_);
for(size_t j=0; j<nrFrontals; ++j) { for(size_t j=0; j<nrFrontals; ++j) {
// Temporarily restrict the matrix view to the conditional blocks of the // Temporarily restrict the matrix view to the conditional blocks of the

2
gtsam/linear/HessianFactor.h
View File

@ -46,7 +46,7 @@ namespace gtsam {
class HessianFactor : public GaussianFactor { class HessianFactor : public GaussianFactor {
protected: protected:
typedef MatrixColMajor InfoMatrix; typedef Matrix InfoMatrix;
typedef SymmetricBlockView<InfoMatrix> BlockInfo; typedef SymmetricBlockView<InfoMatrix> BlockInfo;
InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]

2
gtsam/linear/JacobianFactor.h
View File

@ -44,7 +44,7 @@ namespace gtsam {
class JacobianFactor : public GaussianFactor { class JacobianFactor : public GaussianFactor {
public: public:
typedef MatrixColMajor AbMatrix; typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb; typedef VerticalBlockView<AbMatrix> BlockAb;
protected: protected:

80
gtsam/linear/NoiseModel.cpp
View File

@ -109,11 +109,6 @@ void Gaussian::WhitenInPlace(Matrix& H) const {
H = thisR() * H; H = thisR() * H;
} }
/* ************************************************************************* */
void Gaussian::WhitenInPlace(MatrixColMajor& H) const {
H = thisR() * H;
}
/* ************************************************************************* */ /* ************************************************************************* */
// General QR, see also special version in Constrained // General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const { 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::Cholesky(Matrix& Ab, size_t nFrontals) const {
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 {
// get size(A) and maxRank // get size(A) and maxRank
// TODO: really no rank problems ? // TODO: really no rank problems ?
@ -271,11 +239,6 @@ void Diagonal::WhitenInPlace(Matrix& H) const {
vector_scale_inplace(invsigmas(), H); vector_scale_inplace(invsigmas(), H);
} }
/* ************************************************************************* */
void Diagonal::WhitenInPlace(MatrixColMajor& H) const {
vector_scale_inplace(invsigmas(), H);
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector Diagonal::sample() const { Vector Diagonal::sample() const {
Vector result(dim_); Vector result(dim_);
@ -312,15 +275,11 @@ void Constrained::WhitenInPlace(Matrix& H) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Constrained::WhitenInPlace(MatrixColMajor& H) const { // Special version of QR for Constrained calls slower but smarter code
throw logic_error("noiseModel::Constrained cannot Whiten"); // that deals with possibly zero sigmas
} // It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
/* ************************************************************************* */ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// templated generic constrained QR
template<class MATRIX>
SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
bool verbose = false; bool verbose = false;
if (verbose) cout << "\nStarting Constrained::QR" << endl; if (verbose) cout << "\nStarting Constrained::QR" << endl;
@ -333,7 +292,7 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
list<Triple> Rd; list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse Vector pseudo(m); // allocate storage for pseudo-inverse
Vector invsigmas = reciprocal(sigmas); Vector invsigmas = reciprocal(sigmas_);
Vector weights = emul(invsigmas,invsigmas); // calculate weights once Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We loop over all columns, because the columns that can be eliminated // 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"); tic(3, "constrained_QR update Ab");
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
toc(3, "constrained_QR update Ab"); toc(3, "constrained_QR update Ab");
// updateAb(Ab, j, a, rd);
} }
// Create storage for precisions // Create storage for precisions
@ -388,8 +346,9 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
const Vector& rd = t.get<1>(); const Vector& rd = t.get<1>();
precisions(i) = t.get<2>(); precisions(i) = t.get<2>();
if (precisions(i)==inf) mixed = true; if (precisions(i)==inf) mixed = true;
for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway for (size_t j2=0; j2<j; ++j2)
for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2)
Ab(i,j2) = rd(j2); Ab(i,j2) = rd(j2);
i+=1; i+=1;
} }
@ -398,20 +357,6 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions); return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
} }
/* ************************************************************************* */
// 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 {
return constrained_QR(Ab, sigmas_);
}
/* ************************************************************************* */
SharedDiagonal Constrained::QR(MatrixColMajor& Ab) const {
return constrained_QR(Ab, sigmas_);
}
/* ************************************************************************* */ /* ************************************************************************* */
// Isotropic // Isotropic
/* ************************************************************************* */ /* ************************************************************************* */
@ -450,11 +395,6 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
H *= invsigma_; H *= invsigma_;
} }
/* ************************************************************************* */
void Isotropic::WhitenInPlace(MatrixColMajor& H) const {
H *= invsigma_;
}
/* ************************************************************************* */ /* ************************************************************************* */
// faster version // faster version
Vector Isotropic::sample() const { Vector Isotropic::sample() const {

10
gtsam/linear/NoiseModel.h
View File

@ -165,7 +165,6 @@ namespace gtsam {
* In-place version * In-place version
*/ */
virtual void WhitenInPlace(Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(MatrixColMajor& H) const;
/** /**
* Whiten a system, in place as well * Whiten a system, in place as well
@ -183,15 +182,14 @@ namespace gtsam {
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/ */
virtual SharedDiagonal QR(Matrix& Ab) const; virtual SharedDiagonal QR(Matrix& Ab) const;
virtual SharedDiagonal QR(MatrixColMajor& Ab) const;
// FIXME: these previously had firstZeroRows - what did this do? // FIXME: these previously had firstZeroRows - what did this do?
// virtual SharedDiagonal QRColumnWise(MatrixColMajor& Ab, std::vector<int>& firstZeroRows) const; // virtual SharedDiagonal QRColumnWise(Matrix& Ab, std::vector<int>& firstZeroRows) const;
// virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const; // virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
/** /**
* Cholesky factorization * 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 * 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 Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(MatrixColMajor& H) const;
/** /**
* Return standard deviations (sqrt of diagonal) * Return standard deviations (sqrt of diagonal)
@ -369,13 +366,11 @@ namespace gtsam {
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(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 * Apply QR factorization to the system [A b], taking into account constraints
*/ */
virtual SharedDiagonal QR(Matrix& Ab) const; virtual SharedDiagonal QR(Matrix& Ab) const;
virtual SharedDiagonal QR(MatrixColMajor& Ab) const;
/** /**
* Check constrained is always true * Check constrained is always true
@ -439,7 +434,6 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(MatrixColMajor& H) const;
/** /**
* Return standard deviation * Return standard deviation

4
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -113,8 +113,8 @@ TEST(HessianFactor, Constructor1)
HessianFactor factor(0, G, g, f); HessianFactor factor(0, G, g, f);
// extract underlying parts // extract underlying parts
MatrixColMajor info_matrix = factor.info_.range(0, 1, 0, 1); Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
EXPECT(assert_equal(MatrixColMajor(G), info_matrix)); EXPECT(assert_equal(Matrix(G), info_matrix));
EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10);
EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10));
EXPECT_LONGS_EQUAL(1, factor.size()); EXPECT_LONGS_EQUAL(1, factor.size());

10
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -212,13 +212,13 @@ TEST( NoiseModel, QR )
//TEST( NoiseModel, QRColumnWise ) //TEST( NoiseModel, QRColumnWise )
//{ //{
// // Call Gaussian version // // Call Gaussian version
// MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! // Matrix Ab = exampleQR::Ab; // otherwise overwritten !
// vector<int> firstZeroRows; // vector<int> firstZeroRows;
// firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-( // firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-(
// SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); // SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows);
// SharedDiagonal expected = noiseModel::Unit::Create(4); // SharedDiagonal expected = noiseModel::Unit::Create(4);
// EXPECT(assert_equal(*expected,*actual)); // EXPECT(assert_equal(*expected,*actual));
// Matrix AbResized = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab); // Matrix AbResized = ublas::triangular_adaptor<Matrix, ublas::upper>(Ab);
// print(exampleQR::Rd, "Rd: "); // print(exampleQR::Rd, "Rd: ");
// print(Ab, "Ab: "); // print(Ab, "Ab: ");
// print(AbResized, "AbResized: "); // print(AbResized, "AbResized: ");
@ -229,12 +229,12 @@ TEST( NoiseModel, QR )
TEST(NoiseModel, Cholesky) TEST(NoiseModel, Cholesky)
{ {
SharedDiagonal expected = noiseModel::Unit::Create(4); 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 SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); // ASSERTION FAILURE: access out of bounds
EXPECT(assert_equal(*expected,*actual)); EXPECT(assert_equal(*expected,*actual));
// Ab was modified in place !!! // Ab was modified in place !!!
MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>(); Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
// ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab), // ublas::project(ublas::triangular_adaptor<Matrix, ublas::upper>(Ab),
// ublas::range(0,actual->dim()), ublas::range(0, Ab.cols())); // ublas::range(0,actual->dim()), ublas::range(0, Ab.cols()));
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test
} }