diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index ddb9e22cf..9d818b1d4 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -25,6 +25,8 @@ namespace ublas = boost::numeric::ublas; +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -51,9 +53,9 @@ void cholesky_inplace(MatrixColMajor& I) { } /* ************************************************************************* */ -size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) { +size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal) { - bool debug = false; + static const bool debug = false; size_t m = Ab.size1(); size_t n = Ab.size2(); @@ -62,7 +64,8 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) { // 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-1); +// size_t rank = std::min(m,n-1); + size_t rank = nFrontal; if(rank > 0) { @@ -105,9 +108,124 @@ size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab) { print(S, "S: "); } - return rank; + return m; } else return 0; } +/* ************************************************************************* */ +static inline bool choleskyStep(MatrixColMajor& ATA, size_t k) { + + // Tolerance for being equal to zero +// static const double zeroTol = numeric_limits::epsilon(); + static const double zeroTol = 1.e-15; + + const size_t n = ATA.size1(); + + const double alpha = ATA(k,k); + const double beta = sqrt(alpha); + + if(beta > zeroTol) { + const double betainv = 1.0 / beta; + + // Update k,k + ATA(k,k) = beta; + + if(k < (n-1)) { + // Update A(k,k+1:end) <- A(k,k+1:end) / beta + cblas_dscal(n-k-1, betainv, &(ATA(k,k+1)), n); + + // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha + cblas_dsyr(CblasColMajor, CblasUpper, n-k-1, -1.0, &(ATA(k,k+1)), n, &(ATA(k+1,k+1)), n); + } + + return true; + } else + return false; +} + +/* ************************************************************************* */ +size_t choleskyCareful(MatrixColMajor& ATA) { + + static const bool debug = false; + +// // Tolerance for being equal to zero +//// static const double zeroTol = numeric_limits::epsilon(); +// static const double zeroTol = 1.e-15; + + // Check that the matrix is square (we do not check for symmetry) + assert(ATA.size1() == ATA.size2()); + + // Number of rows/columns + const size_t n = ATA.size1(); + + // The index of the row after the last non-zero row of the square-root factor + size_t maxrank = 0; + + for(size_t k = 0; k < n; ++k) { + + if(choleskyStep(ATA, k)) { + if(debug) cout << "choleskyCareful: Factored through " << k << endl; + if(debug) print(ATA, "ATA: "); + maxrank = k+1; + } else { + if(debug) cout << "choleskyCareful: Skipping " << k << endl; + } + +// // If the diagonal element is not zero, run Cholesky as far as possible - +// // Cholesky will stop on the first zero diagonal element. Because ATA is +// // symmetric positive semi-definite, a zero diagonal element implies +// // a corresponding row and column of zeros, thus we need only check the +// // diagonal. +// if(ATA(k,k) > zeroTol || ATA(k,k) < -zeroTol) { +// +// // Try to do Cholesky on the remaining lower-right square submatrix. +// int info = lapack_dpotf2('U', n-k, &(ATA(k,k)), n); +// +// if(info > 0) { +// // The submatrix is rank-deficient, but Cholesky factored the first +// // subRank rows/columns, leaving a positive semi-definite matrix +// // starting at subRank. (we're speaking in zero-based indexices). +// size_t subRank = info - 1; +// +// // The row/column after the last nonzero one. +// maxrank = k + subRank; +// +// // We know that the row/column just after the factored part is zero, so +// // skip it. Note that after this statement k will be the next +// // row/column to process. +// k += subRank + 1; +// +// if(debug) cout << "choleskyCareful: Factored until " << maxrank << ", skipping next." << endl; +// +// } else if(info == 0) { +// // Cholesky successfully factored the rest of the matrix. Note that +// // after this statement k will be the last processed row/column, and +// // will be incremented by 1 by the 'for' loop. +// k += n - k; +// +// // The last row/column is nonzero. +// maxrank = n; +// +// if(debug) cout << "choleskyCareful: Factored the remainder" << endl; +// +// } else { +// throw std::domain_error(boost::str(boost::format( +// "Bad input to choleskyFactorUnderdetermined, dpotrf returned %d.\n")%info)); +// } +// } else { +// +// if(debug) cout << "choleskyCareful: Skipping " << k << endl; +// +// // The diagonal element is numerically zero, so skip this row/column. +// ++ k; +// } +// +// if(debug) print(ATA, "ATA: "); + } + + return maxrank; + +} + } diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index dcb6aebbe..c7c815462 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -42,7 +42,23 @@ void cholesky_inplace(MatrixColMajor& I); * * This function returns the rank of the factor. */ -size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab); +size_t choleskyFactorUnderdetermined(MatrixColMajor& Ab, size_t nFrontal); + +/** + * "Careful" Cholesky computes the positive square-root of a positive symmetric + * semi-definite matrix (i.e. that may be rank-deficient). Unlike standard + * Cholesky, the square-root factor may have all-zero rows for free variables. + * + * Additionally, this function returns the index of the row after the last + * non-zero row in the computed factor, so that it may be truncated to an + * upper-trapazoidal matrix. + * + * Note that this returned index is the rank of the matrix if and only if all + * of the zero-rows of the factor occur after any non-zero rows. This is + * (always?) the case during elimination of a fully-constrained least-squares + * problem. + */ +size_t choleskyCareful(MatrixColMajor& ATA); } diff --git a/gtsam/base/lapack.h b/gtsam/base/lapack.h index 87aef7d74..2f03ef97b 100644 --- a/gtsam/base/lapack.h +++ b/gtsam/base/lapack.h @@ -23,14 +23,23 @@ extern "C" { #include -/* Subroutine */ int dpotrf_(char *uplo, int *n, double *a, int *lda, - int *info); + /* FORTRAN 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; -} + /* C wrapper */ + inline int lapack_dpotrf(char uplo, int n, double* a, int lda) { + int info; + dpotrf_(&uplo, &n, a, &lda, &info); + return info; + } + + int dpotf2_(char *uplo, int *n, double *a, int *lda, int *info); + + inline int lapack_dpotf2(char uplo, int n, double* a, int lda) { + int info; + dpotf2_(&uplo, &n, a, &lda, &info); + return info; + } } diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 8626d4f25..2c84becc7 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -60,7 +60,7 @@ TEST(cholesky, choleskyFactorUnderdetermined) { 0.0, 0.0, 0.3332, -0.2273, -0.4825, -0.4652, 0.0660); MatrixColMajor actualColmaj = Ab; - choleskyFactorUnderdetermined(actualColmaj); + choleskyFactorUnderdetermined(actualColmaj, 3); Matrix actual = ublas::triangular_adaptor(actualColmaj); CHECK(assert_equal(expected, actual, 1e-3)); diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 2378a6046..ff040a737 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -355,38 +355,26 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve size_t firstVarDim = Ab_(0).size2(); + // Cholesky currently does not work with a constrained noise model + SolveMethod correctedSolveMethod; + if(model_->isConstrained()) + correctedSolveMethod = SOLVE_QR; + else + correctedSolveMethod = solveMethod; + // Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel SharedDiagonal noiseModel; - if(!debug) { - if(solveMethod == SOLVE_QR || model_->isConstrained()) { - tic("eliminateFirst: QR"); - noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); - toc("eliminateFirst: QR"); - } else if(solveMethod == SOLVE_CHOLESKY) { - tic("eliminateFirst: Cholesky"); - noiseModel = model_->Cholesky(matrix_); - toc("eliminateFirst: Cholesky"); - } else - assert(false); - } else { - // In debug mode, compare outputs of Cholesky and QR - MatrixColMajor matrixCopy = matrix_; - SharedDiagonal noiseModelDup = model_->Cholesky(matrixCopy); + if(correctedSolveMethod == SOLVE_QR) { + tic("eliminateFirst: QR"); noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); - - matrixCopy = ublas::triangular_adaptor(matrixCopy); - matrix_ = ublas::triangular_adaptor(matrix_); - - assert(linear_dependent( - Matrix( - ublas::project(ublas::triangular_adaptor(matrix_), - ublas::range(0,noiseModel->dim()), ublas::range(0,matrix_.size2()))), - Matrix( - ublas::project(ublas::triangular_adaptor(matrixCopy), - ublas::range(0,noiseModel->dim()), ublas::range(0,matrix_.size2()))), - 1e-2)); - assert(assert_equal(*noiseModel, *noiseModelDup, 1e-2)); - } + toc("eliminateFirst: QR"); + } else if(correctedSolveMethod == SOLVE_CHOLESKY) { + tic("eliminateFirst: Cholesky"); + noiseModel = model_->Cholesky(matrix_, firstVarDim); + Ab_.rowEnd() = noiseModel->dim(); + toc("eliminateFirst: Cholesky"); + } else + assert(false); if(matrix_.size1() > 0) { for(size_t j=0; jsigmas(), firstVarDim, noiseModel->dim())); else model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), firstVarDim, noiseModel->dim())); - assert(Ab_.size1() <= Ab_.size2()-1); + if(correctedSolveMethod == SOLVE_QR) assert(Ab_.size1() <= Ab_.size2()-1); toc("eliminateFirst: remaining factor"); // todo SL: deal with "dead" pivot columns!!! @@ -441,10 +429,10 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve ++ varpos; firstNonzeroBlocks_[row] = varpos; if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl; -// if(debug && varpos < size()) { -// ABlock block(Ab_(varpos)); -// assert(!gtsam::equal_with_abs_tol(ublas::row(block, row), zero(block.size2()), 1e-5)); -// } + // if(debug && varpos < size()) { + // ABlock block(Ab_(varpos)); + // assert(!gtsam::equal_with_abs_tol(ublas::row(block, row), zero(block.size2()), 1e-5)); + // } } toc("eliminateFirst: rowstarts"); @@ -468,6 +456,7 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveM tic("eliminate"); + if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl; if(debug) this->print("Eliminating GaussianFactor: "); tic("eliminate: stairs"); @@ -499,15 +488,25 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveM size_t frontalDim = Ab_.range(0,nrFrontals).size2(); + if(debug) cout << "frontalDim = " << frontalDim << endl; + + // Cholesky currently does not work with a constrained noise model + SolveMethod correctedSolveMethod; + if(model_->isConstrained()) + correctedSolveMethod = SOLVE_QR; + else + correctedSolveMethod = solveMethod; + // Use in-place QR or Cholesky on dense Ab appropriate to NoiseModel SharedDiagonal noiseModel; - if(solveMethod == SOLVE_QR || model_->isConstrained()) { + if(correctedSolveMethod == SOLVE_QR) { tic("eliminateFirst: QR"); noiseModel = model_->QRColumnWise(matrix_, firstZeroRows); toc("eliminateFirst: QR"); - } else if(solveMethod == SOLVE_CHOLESKY) { + } else if(correctedSolveMethod == SOLVE_CHOLESKY) { tic("eliminateFirst: Cholesky"); - noiseModel = model_->Cholesky(matrix_); + noiseModel = model_->Cholesky(matrix_, frontalDim); + Ab_.rowEnd() = noiseModel->dim(); toc("eliminateFirst: Cholesky"); } else assert(false); @@ -558,7 +557,7 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals, SolveM else model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim())); if(debug) this->print("Eliminated factor: "); - assert(Ab_.size1() <= Ab_.size2()-1); + if(correctedSolveMethod == SOLVE_QR) assert(Ab_.size1() <= Ab_.size2()-1); toc("eliminate: remaining factor"); // todo SL: deal with "dead" pivot columns!!! diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index db2c73135..74c09868a 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -247,20 +247,26 @@ SharedDiagonal Gaussian::QRColumnWise(MatrixColMajor& Ab, vector& firstZero } /* ************************************************************************* */ -SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab) const { +SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const { // get size(A) and maxRank // TODO: really no rank problems ? - size_t m = Ab.size1(), n = Ab.size2()-1; - size_t maxRank = min(m,n); // pre-whiten everything (cheaply if possible) WhitenInPlace(Ab); - // Use Cholesky to factor Ab - size_t rank = choleskyFactorUnderdetermined(Ab); - assert(rank == maxRank); + // Form A'*A (todo: this is probably less efficient than possible) + Ab = ublas::trans(Ab) * Ab; - return Unit::Create(maxRank); + // Use Cholesky to factor Ab + size_t maxrank = choleskyCareful(Ab); + + // Due to numerical error the rank could appear to be more than the number + // of variables. The important part is that it does not includes the + // augmented b column. + if(maxrank == Ab.size2()) + -- maxrank; + + return Unit::Create(maxrank); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 3744697d5..af738d94f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -172,7 +172,7 @@ namespace gtsam { /** * Cholesky factorization */ - virtual SharedDiagonal Cholesky(MatrixColMajor& Ab) const; + virtual SharedDiagonal Cholesky(MatrixColMajor& Ab, size_t nFrontals) const; /** * Return R itself, but note that Whiten(H) is cheaper than R*H diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9c7f7bd7c..499b2dd56 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include using namespace boost::assign; @@ -218,13 +219,15 @@ TEST( NoiseModel, QR ) /* ************************************************************************* */ TEST(NoiseModel, Cholesky) { - MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! - SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab); - SharedDiagonal expected = noiseModel::Unit::Create(4); - EXPECT(assert_equal(*expected,*actual)); - // Ab was modified in place !!! - Matrix actualRd = ublas::triangular_adaptor(Ab); - EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); + SharedDiagonal expected = noiseModel::Unit::Create(4); + MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten ! + SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); + EXPECT(assert_equal(*expected,*actual)); + // Ab was modified in place !!! + Matrix actualRd = + ublas::project(ublas::triangular_adaptor(Ab), + ublas::range(0,actual->dim()), ublas::range(0, Ab.size2())); + EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); } /* ************************************************************************* */