2010-11-07 05:06:52 +08:00
|
|
|
/* ----------------------------------------------------------------------------
|
|
|
|
|
|
|
|
* 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
|
2011-09-07 13:01:46 +08:00
|
|
|
* @date Nov 5, 2010
|
2010-11-07 05:06:52 +08:00
|
|
|
*/
|
|
|
|
|
2011-02-04 08:53:00 +08:00
|
|
|
#include <gtsam/base/debug.h>
|
2010-11-07 05:06:52 +08:00
|
|
|
#include <gtsam/base/cholesky.h>
|
2011-01-21 06:22:00 +08:00
|
|
|
#include <gtsam/base/timing.h>
|
2010-11-07 05:06:52 +08:00
|
|
|
|
2012-06-07 21:18:17 +08:00
|
|
|
#include <boost/format.hpp>
|
2012-08-23 06:40:27 +08:00
|
|
|
#include <cmath>
|
2012-06-07 21:18:17 +08:00
|
|
|
|
2010-11-29 09:31:51 +08:00
|
|
|
using namespace std;
|
|
|
|
|
2010-11-07 05:06:52 +08:00
|
|
|
namespace gtsam {
|
|
|
|
|
2011-02-04 08:47:08 +08:00
|
|
|
static const double negativePivotThreshold = -1e-1;
|
2011-02-04 10:38:35 +08:00
|
|
|
static const double zeroPivotThreshold = 1e-6;
|
|
|
|
static const double underconstrainedPrior = 1e-5;
|
2012-10-02 22:40:07 +08:00
|
|
|
static const int underconstrainedExponentDifference = 12;
|
2011-02-04 08:47:08 +08:00
|
|
|
|
2010-11-29 09:31:51 +08:00
|
|
|
/* ************************************************************************* */
|
2012-08-23 06:40:27 +08:00
|
|
|
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {
|
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
const bool debug = ISDEBUG("choleskyCareful");
|
2010-11-29 09:31:51 +08:00
|
|
|
|
2011-02-15 23:21:09 +08:00
|
|
|
// Get pivot value
|
2011-02-04 08:47:08 +08:00
|
|
|
double alpha = ATA(k,k);
|
2011-02-15 23:21:09 +08:00
|
|
|
|
|
|
|
// Correct negative pivots from round-off error
|
2011-02-04 08:47:08 +08:00
|
|
|
if(alpha < negativePivotThreshold) {
|
2012-10-02 22:40:07 +08:00
|
|
|
if(debug) {
|
|
|
|
cout << "pivot = " << alpha << endl;
|
|
|
|
print(ATA, "Partially-factorized matrix: ");
|
|
|
|
}
|
2012-08-23 06:40:27 +08:00
|
|
|
return -1;
|
2011-02-04 08:47:08 +08:00
|
|
|
} else if(alpha < 0.0)
|
|
|
|
alpha = 0.0;
|
|
|
|
|
2010-11-29 09:31:51 +08:00
|
|
|
const double beta = sqrt(alpha);
|
|
|
|
|
2011-02-04 08:47:08 +08:00
|
|
|
if(beta > zeroPivotThreshold) {
|
2010-11-29 09:31:51 +08:00
|
|
|
const double betainv = 1.0 / beta;
|
|
|
|
|
|
|
|
// Update k,k
|
|
|
|
ATA(k,k) = beta;
|
|
|
|
|
2011-02-04 08:47:08 +08:00
|
|
|
if(k < (order-1)) {
|
2010-11-29 09:31:51 +08:00
|
|
|
// Update A(k,k+1:end) <- A(k,k+1:end) / beta
|
2012-10-02 22:40:07 +08:00
|
|
|
typedef Matrix::RowXpr::SegmentReturnType BlockRow;
|
|
|
|
BlockRow V = ATA.row(k).segment(k+1, order-(k+1));
|
|
|
|
V *= betainv;
|
2010-11-29 09:31:51 +08:00
|
|
|
|
|
|
|
// Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
|
2012-10-02 22:40:07 +08:00
|
|
|
ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V;
|
|
|
|
// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
|
|
|
|
// .rankUpdate(V.adjoint(), -1);
|
2010-11-29 09:31:51 +08:00
|
|
|
}
|
2012-10-02 22:40:07 +08:00
|
|
|
return 1;
|
2011-02-04 08:47:08 +08:00
|
|
|
} else {
|
2011-02-15 23:21:09 +08:00
|
|
|
// For zero pivots, add the underconstrained variable prior
|
2011-02-04 08:47:08 +08:00
|
|
|
ATA(k,k) = underconstrainedPrior;
|
|
|
|
for(size_t j=k+1; j<order; ++j)
|
|
|
|
ATA(k,j) = 0.0;
|
2012-10-02 22:40:07 +08:00
|
|
|
if(debug) cout << "choleskyCareful: Skipping " << k << endl;
|
|
|
|
return 0;
|
2011-02-04 08:47:08 +08:00
|
|
|
}
|
2010-11-29 09:31:51 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
2011-06-05 06:15:23 +08:00
|
|
|
pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {
|
2010-11-29 09:31:51 +08:00
|
|
|
|
2011-02-04 08:47:08 +08:00
|
|
|
const bool debug = ISDEBUG("choleskyCareful");
|
2010-11-29 09:31:51 +08:00
|
|
|
|
|
|
|
// Check that the matrix is square (we do not check for symmetry)
|
2011-05-20 21:52:08 +08:00
|
|
|
assert(ATA.rows() == ATA.cols());
|
2010-11-29 09:31:51 +08:00
|
|
|
|
|
|
|
// Number of rows/columns
|
2011-05-20 21:52:08 +08:00
|
|
|
const size_t n = ATA.rows();
|
2010-11-29 09:31:51 +08:00
|
|
|
|
2011-02-15 23:21:09 +08:00
|
|
|
// Negative order means factor the entire matrix
|
2011-02-04 08:47:08 +08:00
|
|
|
if(order < 0)
|
2012-08-23 06:40:27 +08:00
|
|
|
order = int(n);
|
2011-02-04 08:47:08 +08:00
|
|
|
|
2011-02-15 23:21:09 +08:00
|
|
|
assert(size_t(order) <= n);
|
2011-02-04 08:47:08 +08:00
|
|
|
|
2010-11-29 09:31:51 +08:00
|
|
|
// The index of the row after the last non-zero row of the square-root factor
|
|
|
|
size_t maxrank = 0;
|
2012-10-02 22:40:07 +08:00
|
|
|
bool success = true;
|
2010-11-29 09:31:51 +08:00
|
|
|
|
2011-02-15 23:21:09 +08:00
|
|
|
// Factor row-by-row
|
|
|
|
for(size_t k = 0; k < size_t(order); ++k) {
|
2012-08-23 06:40:27 +08:00
|
|
|
int stepResult = choleskyStep(ATA, k, size_t(order));
|
2012-10-02 22:40:07 +08:00
|
|
|
if(stepResult == 1) {
|
2010-11-29 09:31:51 +08:00
|
|
|
if(debug) cout << "choleskyCareful: Factored through " << k << endl;
|
|
|
|
if(debug) print(ATA, "ATA: ");
|
|
|
|
maxrank = k+1;
|
2012-08-23 06:40:27 +08:00
|
|
|
} else if(stepResult == -1) {
|
|
|
|
success = false;
|
2012-10-02 22:40:07 +08:00
|
|
|
break;
|
2012-08-23 06:40:27 +08:00
|
|
|
} /* else if(stepResult == 0) Found zero pivot */
|
2010-11-29 09:31:51 +08:00
|
|
|
}
|
|
|
|
|
2012-08-23 06:40:27 +08:00
|
|
|
return make_pair(maxrank, success);
|
2011-01-21 06:22:00 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* ************************************************************************* */
|
2012-08-23 06:40:27 +08:00
|
|
|
bool choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
2011-01-21 06:22:00 +08:00
|
|
|
|
2014-03-01 06:36:07 +08:00
|
|
|
gttic(choleskyPartial);
|
|
|
|
|
2011-02-04 08:47:08 +08:00
|
|
|
const bool debug = ISDEBUG("choleskyPartial");
|
2011-01-21 06:22:00 +08:00
|
|
|
|
2011-02-07 03:42:15 +08:00
|
|
|
assert(ABC.rows() == ABC.cols());
|
2011-02-15 23:21:09 +08:00
|
|
|
assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));
|
2011-01-21 06:22:00 +08:00
|
|
|
|
2011-02-07 03:42:15 +08:00
|
|
|
const size_t n = ABC.rows();
|
2010-11-29 09:31:51 +08:00
|
|
|
|
2011-01-21 06:22:00 +08:00
|
|
|
// Compute Cholesky factorization of A, overwrites A.
|
2012-10-03 04:18:41 +08:00
|
|
|
gttic(lld);
|
2013-11-19 03:23:23 +08:00
|
|
|
Eigen::ComputationInfo lltResult;
|
|
|
|
if(nFrontal > 0)
|
|
|
|
{
|
|
|
|
Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0, 0, nFrontal, nFrontal).selfadjointView<Eigen::Upper>().llt();
|
|
|
|
ABC.block(0, 0, nFrontal, nFrontal).triangularView<Eigen::Upper>() = llt.matrixU();
|
|
|
|
lltResult = llt.info();
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
lltResult = Eigen::Success;
|
|
|
|
}
|
2012-10-03 04:18:41 +08:00
|
|
|
gttoc(lld);
|
2011-02-07 03:42:15 +08:00
|
|
|
|
|
|
|
if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;
|
2011-01-21 06:22:00 +08:00
|
|
|
|
|
|
|
// Compute S = inv(R') * B
|
2012-10-03 04:18:41 +08:00
|
|
|
gttic(compute_S);
|
2011-02-07 03:42:15 +08:00
|
|
|
if(n - nFrontal > 0) {
|
|
|
|
ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
|
|
|
|
ABC.topRightCorner(nFrontal, n-nFrontal));
|
2011-02-04 09:05:52 +08:00
|
|
|
}
|
2011-02-07 03:42:15 +08:00
|
|
|
if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
|
2012-10-03 04:18:41 +08:00
|
|
|
gttoc(compute_S);
|
2011-01-21 06:22:00 +08:00
|
|
|
|
|
|
|
// Compute L = C - S' * S
|
2012-10-03 04:18:41 +08:00
|
|
|
gttic(compute_L);
|
2011-02-07 03:42:15 +08:00
|
|
|
if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
|
|
|
if(n - nFrontal > 0)
|
|
|
|
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
|
|
|
|
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
|
|
|
|
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
2012-10-03 04:18:41 +08:00
|
|
|
gttoc(compute_L);
|
2012-08-23 06:40:27 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
// Check last diagonal element - Eigen does not check it
|
|
|
|
bool ok;
|
2013-11-19 03:23:23 +08:00
|
|
|
if(lltResult == Eigen::Success) {
|
2012-10-02 22:40:07 +08:00
|
|
|
if(nFrontal >= 2) {
|
|
|
|
int exp2, exp1;
|
|
|
|
(void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2);
|
|
|
|
(void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1);
|
|
|
|
ok = (exp2 - exp1 < underconstrainedExponentDifference);
|
|
|
|
} else if(nFrontal == 1) {
|
|
|
|
int exp1;
|
|
|
|
(void)frexp(ABC(0,0), &exp1);
|
|
|
|
ok = (exp1 > -underconstrainedExponentDifference);
|
|
|
|
} else {
|
|
|
|
ok = true;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
ok = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return ok;
|
2010-11-29 09:31:51 +08:00
|
|
|
}
|
|
|
|
|
2010-11-07 05:06:52 +08:00
|
|
|
}
|