Cholesky working but disabled by default
Richard Roberts
parent
7b2c62caea
commit
5e00148315
|
|
@ -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<double>::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<double>::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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -23,14 +23,23 @@ extern "C" {
|
|||
|
||||
#include <cblas.h>
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<MatrixColMajor, ublas::upper>(actualColmaj);
|
||||
|
||||
CHECK(assert_equal(expected, actual, 1e-3));
|
||||
|
|
|
|||
|
|
@ -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<MatrixColMajor, ublas::upper>(matrixCopy);
|
||||
matrix_ = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrix_);
|
||||
|
||||
assert(linear_dependent(
|
||||
Matrix(
|
||||
ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(matrix_),
|
||||
ublas::range(0,noiseModel->dim()), ublas::range(0,matrix_.size2()))),
|
||||
Matrix(
|
||||
ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(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; j<matrix_.size2(); ++j)
|
||||
|
|
@ -429,7 +417,7 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst(SolveMethod solve
|
|||
model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), 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!!!
|
||||
|
|
|
|||
|
|
@ -247,20 +247,26 @@ SharedDiagonal Gaussian::QRColumnWise(MatrixColMajor& Ab, vector<int>& 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/numeric/ublas/triangular.hpp>
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
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<MatrixColMajor, ublas::upper>(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<MatrixColMajor, ublas::upper>(Ab),
|
||||
ublas::range(0,actual->dim()), ublas::range(0, Ab.size2()));
|
||||
EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue