Changed the default Matrix to use column major, rather than row major. Removed Matrix-inl.h, as it isn't used
Alex Cunningham
parent
36f9ebae90
commit
a22586362b
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
//}
|
||||
|
||||
}
|
||||
|
|
@ -28,23 +28,19 @@
|
|||
#include <gtsam/3rdparty/Eigen/Dense>
|
||||
#include <gtsam/3rdparty/Eigen/SVD>
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/Matrix-inl.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
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 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<Matrix>& As, const std::list<Matrix>& 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<Matrix,Matrix> 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<boost::tuple<Vector, double, double> >
|
||||
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<n; ++j) {
|
||||
// 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
|
||||
double precision = weightedPseudoinverse(a, weights, pseudo);
|
||||
|
||||
|
||||
// if precision is zero, no information on this column
|
||||
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
|
||||
// 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;
|
||||
|
|
@ -450,38 +381,9 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
|
|||
/** Imperative version of Householder QR factorization, Golub & Van Loan p 224
|
||||
* 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) {
|
||||
// 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) {
|
||||
void householder_(Matrix& A, size_t k, bool copy_vectors) {
|
||||
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++) {
|
||||
|
|
@ -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
|
||||
tic(1, "householder_");
|
||||
householder_(A,k,false);
|
||||
|
|
@ -638,21 +540,9 @@ Matrix collect(size_t nrMatrices, ...)
|
|||
/* ************************************************************************* */
|
||||
// row scaling, in-place
|
||||
void vector_scale_inplace(const Vector& v, Matrix& A) {
|
||||
size_t m = A.rows(); size_t n = A.cols();
|
||||
for (size_t i=0; i<m; ++i) { // loop over rows
|
||||
double vi = 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);
|
||||
const size_t m = A.rows();
|
||||
for (size_t i=0; i<m; ++i)
|
||||
A.row(i) *= v(i);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -667,14 +557,9 @@ Matrix vector_scale(const Vector& v, const Matrix& A) {
|
|||
// column scaling
|
||||
Matrix vector_scale(const Matrix& A, const Vector& v) {
|
||||
Matrix M(A);
|
||||
size_t m = A.rows(); size_t n = A.cols();
|
||||
const double * vptr = v.data();
|
||||
for (size_t i=0; i<m; ++i) { // loop over rows
|
||||
for (size_t j=0; j<n; ++j) { // loop over columns
|
||||
double * Mptr = M.data() + i*n + j;
|
||||
(*Mptr) = (*Mptr) * *(vptr+j);
|
||||
}
|
||||
}
|
||||
const size_t n = A.cols();
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M.col(j) *= v(j);
|
||||
return M;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,11 +30,11 @@
|
|||
|
||||
/**
|
||||
* 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::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixColMajor;
|
||||
typedef Eigen::MatrixXd Matrix;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
|
||||
|
||||
// Matrix expressions for accessing parts of matrices
|
||||
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
|
||||
*/
|
||||
// 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<MatrixColMajor>::type HCoeffsType;
|
||||
typedef Eigen::internal::plain_row_type<MatrixColMajor>::type RowVectorType;
|
||||
typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
|
||||
typedef Eigen::internal::plain_row_type<Matrix>::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<class Archive>
|
||||
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<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 boost
|
||||
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(Matrix)
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(MatrixColMajor)
|
||||
|
||||
|
|
|
|||
|
|
@ -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<size_t,bool> choleskyCareful(MatrixColMajor& ATA, int order) {
|
||||
pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {
|
||||
|
||||
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");
|
||||
|
||||
|
|
|
|||
|
|
@ -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<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
|
||||
|
|
@ -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 x nFrontal.
|
||||
*/
|
||||
void choleskyPartial(MatrixColMajor& ABC, size_t nFrontal);
|
||||
void choleskyPartial(Matrix& ABC, size_t nFrontal);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(testBlockMatrices, jacobian_factor1) {
|
||||
typedef MatrixColMajor AbMatrix;
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> 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<AbMatrix> 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<InfoMatrix> BlockInfo;
|
||||
|
||||
Matrix expected_full = Matrix_(3, 3,
|
||||
|
|
|
|||
|
|
@ -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<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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
static double inf = std::numeric_limits<double>::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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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<N1> (data[j]);
|
||||
|
|
|
|||
|
|
@ -28,12 +28,12 @@ namespace gtsam {
|
|||
Matrix reshape(const tensors::Tensor2Expression<A, I, J>& 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include <gtsam/base/Matrix-inl.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/base/Matrix-inl.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ public:
|
|||
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
|
||||
|
||||
/** Store the conditional matrix as upper-triangular column-major */
|
||||
typedef MatrixColMajor AbMatrix;
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> rsd_type;
|
||||
|
||||
typedef rsd_type::Block r_type;
|
||||
|
|
|
|||
|
|
@ -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<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))
|
||||
return false;
|
||||
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;
|
||||
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;
|
||||
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
|
||||
}
|
||||
|
|
@ -421,7 +421,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
|
|||
// Extract conditionals
|
||||
tic(1, "extract conditionals");
|
||||
GaussianBayesNet::shared_ptr conditionals(new GaussianBayesNet());
|
||||
typedef VerticalBlockView<MatrixColMajor> BlockAb;
|
||||
typedef VerticalBlockView<Matrix> BlockAb;
|
||||
BlockAb Ab(matrix_, info_);
|
||||
for(size_t j=0; j<nrFrontals; ++j) {
|
||||
// Temporarily restrict the matrix view to the conditional blocks of the
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ namespace gtsam {
|
|||
|
||||
class HessianFactor : public GaussianFactor {
|
||||
protected:
|
||||
typedef MatrixColMajor InfoMatrix;
|
||||
typedef Matrix InfoMatrix;
|
||||
typedef SymmetricBlockView<InfoMatrix> BlockInfo;
|
||||
|
||||
InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ namespace gtsam {
|
|||
|
||||
class JacobianFactor : public GaussianFactor {
|
||||
public:
|
||||
typedef MatrixColMajor AbMatrix;
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -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<class MATRIX>
|
||||
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<Triple> 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<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
|
||||
for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
|
||||
for (size_t j2=0; j2<j; ++j2)
|
||||
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);
|
||||
i+=1;
|
||||
}
|
||||
|
|
@ -398,20 +357,6 @@ SharedDiagonal constrained_QR(MATRIX& Ab, const Vector& sigmas) {
|
|||
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
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -450,11 +395,6 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
|
|||
H *= invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Isotropic::WhitenInPlace(MatrixColMajor& H) const {
|
||||
H *= invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// faster version
|
||||
Vector Isotropic::sample() const {
|
||||
|
|
|
|||
|
|
@ -165,7 +165,6 @@ namespace gtsam {
|
|||
* In-place version
|
||||
*/
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
virtual void WhitenInPlace(MatrixColMajor& H) const;
|
||||
|
||||
/**
|
||||
* Whiten a system, in place as well
|
||||
|
|
@ -183,15 +182,14 @@ namespace gtsam {
|
|||
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
|
||||
*/
|
||||
virtual SharedDiagonal QR(Matrix& Ab) const;
|
||||
virtual SharedDiagonal QR(MatrixColMajor& Ab) const;
|
||||
// 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;
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
|
|
|||
|
|
@ -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<int> 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<MatrixColMajor, ublas::upper>(Ab);
|
||||
// Matrix AbResized = ublas::triangular_adaptor<Matrix, ublas::upper>(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<Eigen::Upper>();
|
||||
// ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab),
|
||||
Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
|
||||
// ublas::project(ublas::triangular_adaptor<Matrix, ublas::upper>(Ab),
|
||||
// ublas::range(0,actual->dim()), ublas::range(0, Ab.cols()));
|
||||
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue