Code to factor rank-deficient A matrices using Cholesky, not yet used by GaussianFactor
Richard Roberts
parent
05b0110ac2
commit
0013911960
|
|
@ -13,9 +13,9 @@ check_PROGRAMS =
|
|||
|
||||
# base Math
|
||||
|
||||
headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h
|
||||
sources += Vector.cpp Matrix.cpp
|
||||
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix
|
||||
headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h lapack.h
|
||||
sources += Vector.cpp Matrix.cpp cholesky.cpp
|
||||
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testCholesky
|
||||
|
||||
if USE_LAPACK
|
||||
sources += DenseQR.cpp DenseQRUtil.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,90 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 cholesky.cpp
|
||||
* @brief Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
|
||||
* @author Richard Roberts
|
||||
* @created Nov 5, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <gtsam/base/lapack.h>
|
||||
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
#include <boost/numeric/ublas/blas.hpp>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
namespace ublas = boost::numeric::ublas;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void cholesky_inplace(MatrixColMajor& I) {
|
||||
|
||||
// We do not check for symmetry but we do check for squareness
|
||||
assert(I.size1() == I.size2());
|
||||
|
||||
// Do Cholesky, return value info as follows (from dpotrf.f):
|
||||
// 00054 * INFO (output) INTEGER
|
||||
// 00055 * = 0: successful exit
|
||||
// 00056 * < 0: if INFO = -i, the i-th argument had an illegal value
|
||||
// 00057 * > 0: if INFO = i, the leading minor of order i is not
|
||||
// 00058 * positive definite, and the factorization could not be
|
||||
// 00059 * completed.
|
||||
int info = lapack_dpotrf('U', I.size1(), &I(0,0), I.size1());
|
||||
if(info != 0)
|
||||
if(info < 0)
|
||||
throw std::domain_error(boost::str(boost::format(
|
||||
"Bad input to cholesky_inplace, dpotrf returned %d.\n")%info));
|
||||
else
|
||||
throw std::domain_error("The matrix passed into cholesky_inplace is rank-deficient");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void choleskyFactorUnderdetermined(MatrixColMajor& Ab) {
|
||||
|
||||
size_t m = Ab.size1();
|
||||
size_t n = Ab.size2();
|
||||
|
||||
// If m >= n, this function will just compute the plain Cholesky
|
||||
// factorization of A'A. If m < n, A'A is rank-deficient this function
|
||||
// instead computes the upper-trapazoidal factor [ R S ], as described in the
|
||||
// header file comment.
|
||||
size_t rank = std::min(m,n);
|
||||
|
||||
// F is the first 'rank' columns of Ab, G is the remaining columns
|
||||
ublas::matrix_range<MatrixColMajor> F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n)));
|
||||
|
||||
ublas::matrix_range<MatrixColMajor> R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank)));
|
||||
ublas::matrix_range<MatrixColMajor> S(ublas::project(Ab, ublas::range(0,rank), ublas::range(rank,n)));
|
||||
|
||||
// First compute F' * G (ublas makes a copy here to avoid aliasing)
|
||||
S = ublas::prod(ublas::trans(F), G);
|
||||
|
||||
// ublas makes a copy to avoid aliasing on this assignment
|
||||
R = ublas::prod(ublas::trans(F), F);
|
||||
|
||||
// Compute the values of R from F'F
|
||||
int info = lapack_dpotrf('U', rank, &R(0,0), Ab.size1());
|
||||
if(info != 0)
|
||||
if(info < 0)
|
||||
throw std::domain_error(boost::str(boost::format(
|
||||
"Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info));
|
||||
else
|
||||
throw std::domain_error("The matrix passed into choleskyFactorUnderdetermined is numerically rank-deficient");
|
||||
|
||||
// Compute S = inv(R') * F' * G, i.e. solve S when R'S = F'G
|
||||
cblas_dtrsm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, S.size1(), S.size2(), 1.0, &R(0,0), m, &S(0,0), m);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 cholesky.h
|
||||
* @brief Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
|
||||
* @author Richard Roberts
|
||||
* @created Nov 5, 2010
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Plain Cholesky on a symmetric positive semi-definite matrix, in place. */
|
||||
void cholesky_inplace(MatrixColMajor& I);
|
||||
|
||||
/**
|
||||
* Factor an underdetermined Gaussian into a Gaussian conditional. This means
|
||||
* for a Gaussian exp(-1/2 * ||Ax - b||^2), with a "wide" A, i.e. m < n, to
|
||||
* find an upper-triangular m x m R, rectangular m x (n-m) S, and m-vector d,
|
||||
* such that ||Ax - b||^2 == || [R S]x - d ||^2.
|
||||
*
|
||||
* The matrices [ R S ] and [ R S d ] are each upper-trapazoidal.
|
||||
*
|
||||
* This returns the same upper-trapazoidal factor as QR, but uses Cholesky for
|
||||
* efficiency. Given a matrix [ F G b ], with F square, this first computes
|
||||
* the upper-triangular R = chol(F), i.e. R'R == F'F. It then computes the
|
||||
* upper-trapazoidal factor [ R S d ], with [ S d ] = inv(R') * F' * [ G b ].
|
||||
*
|
||||
* Note that this operates on the "canonical" A matrix, not the symmetric
|
||||
* information matrix like plain Cholesky.
|
||||
*/
|
||||
void choleskyFactorUnderdetermined(MatrixColMajor& Ab);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 lapack.h
|
||||
* @brief Handles wrapping of LAPACK functions that we use
|
||||
* @author Richard Roberts
|
||||
* @created Nov 6, 2010
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// Prototypes of LAPACK functions that we use
|
||||
extern "C" {
|
||||
|
||||
#include <cblas.h>
|
||||
#include <clapack.h>
|
||||
|
||||
/* Subroutine */ int dpotrf_(char *uplo, int *n, double *a, int *lda,
|
||||
int *info);
|
||||
|
||||
inline int lapack_dpotrf(char uplo, int n, double* a, int lda) {
|
||||
int info;
|
||||
dpotrf_(&uplo, &n, a, &lda, &info);
|
||||
return info;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testCholesky.cpp
|
||||
* @brief
|
||||
* @author Richard Roberts
|
||||
* @created Nov 5, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/numeric/ublas/triangular.hpp>
|
||||
|
||||
using namespace gtsam;
|
||||
namespace ublas = boost::numeric::ublas;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(cholesky, cholesky_inplace) {
|
||||
Matrix I = Matrix_(5,5,
|
||||
1.739214152118470, 0.714164147286383, 1.893437990040579, 1.469485110232169, 1.549910412077052,
|
||||
0.714164147286383, 0.587767128354794, 0.738548568260523, 0.588775778746414, 0.645015863153235,
|
||||
1.893437990040579, 0.738548568260523, 2.165530165155413, 1.631203698494913, 1.617901992621506,
|
||||
1.469485110232169, 0.588775778746414, 1.631203698494913, 1.591363675348797, 1.584779118350080,
|
||||
1.549910412077052, 0.645015863153235, 1.617901992621506, 1.584779118350080, 1.928887183904716);
|
||||
|
||||
Matrix expected = Matrix_(5,5,
|
||||
1.318792687316119, 0.541528743793524, 1.435735888021887, 1.114265437142152, 1.175249473995251,
|
||||
0.0, 0.542691208699940, -0.071760299365346, -0.026960052875681, 0.015818372803848,
|
||||
0.0, 0.0, 0.314711112667495, 0.093667363355578, -0.217058504307151,
|
||||
0.0, 0.0, 0.0, 0.583331630832890, 0.507424929100112,
|
||||
0.0, 0.0, 0.0, 0.0, 0.492779041656733);
|
||||
|
||||
MatrixColMajor actualColMaj = I;
|
||||
cholesky_inplace(actualColMaj);
|
||||
Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColMaj);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(cholesky, choleskyFactorUnderdetermined) {
|
||||
|
||||
Matrix Ab = Matrix_(3,7,
|
||||
0.0357, 0.6787, 0.3922, 0.7060, 0.0462, 0.6948, 0.0344,
|
||||
0.8491, 0.7577, 0.6555, 0.0318, 0.0971, 0.3171, 0.4387,
|
||||
0.9340, 0.7431, 0.1712, 0.2769, 0.8235, 0.9502, 0.3816);
|
||||
|
||||
Matrix expected = Matrix_(3,7,
|
||||
1.2628, 1.0784, 0.5785, 0.2462, 0.6757, 0.9357, 0.5782,
|
||||
0.0, 0.6513, 0.4089, 0.6811, -0.0180, 0.6280, 0.0244,
|
||||
0.0, 0.0, 0.3332, -0.2273, -0.4825, -0.4652, 0.0660);
|
||||
|
||||
MatrixColMajor actualColmaj = Ab;
|
||||
choleskyFactorUnderdetermined(actualColmaj);
|
||||
Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColmaj);
|
||||
|
||||
CHECK(assert_equal(expected, actual, 1e-3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue