diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index 4ae23a523..9f22c36e8 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -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 diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp new file mode 100644 index 000000000..581cad0c2 --- /dev/null +++ b/gtsam/base/cholesky.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 +#include + +#include +#include +#include + +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 F(ublas::project(Ab, ublas::range(0,m), ublas::range(0,rank))); + ublas::matrix_range G(ublas::project(Ab, ublas::range(0,m), ublas::range(rank,n))); + + ublas::matrix_range R(ublas::project(Ab, ublas::range(0,rank), ublas::range(0,rank))); + ublas::matrix_range 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); +} + +} diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h new file mode 100644 index 000000000..364f662aa --- /dev/null +++ b/gtsam/base/cholesky.h @@ -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 + +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); + +} + diff --git a/gtsam/base/lapack.h b/gtsam/base/lapack.h new file mode 100644 index 000000000..820d4c934 --- /dev/null +++ b/gtsam/base/lapack.h @@ -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 +#include + +/* 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; +} + +} + diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp new file mode 100644 index 000000000..8626d4f25 --- /dev/null +++ b/gtsam/base/tests/testCholesky.cpp @@ -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 +#include + +#include + +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(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(actualColmaj); + + CHECK(assert_equal(expected, actual, 1e-3)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */