base/cholesky.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/base/cholesky.h>
21 #include <gtsam/base/timing.h>
22 
23 #include <cmath>
24 
25 using namespace std;
26 
27 namespace gtsam {
28 
29 static const double negativePivotThreshold = -1e-1;
30 static const double zeroPivotThreshold = 1e-6;
31 static const double underconstrainedPrior = 1e-5;
32 static const int underconstrainedExponentDifference = 12;
33 
34 /* ************************************************************************* */
35 static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {
36  // Get pivot value
37  double alpha = ATA(k, k);
38 
39  // Correct negative pivots from round-off error
40  if (alpha < negativePivotThreshold) {
41  return -1;
42  } else if (alpha < 0.0)
43  alpha = 0.0;
44 
45  const double beta = sqrt(alpha);
46 
47  if (beta > zeroPivotThreshold) {
48  const double betainv = 1.0 / beta;
49 
50  // Update k,k
51  ATA(k, k) = beta;
52 
53  if (k < (order - 1)) {
54  // Update A(k,k+1:end) <- A(k,k+1:end) / beta
55  typedef Matrix::RowXpr::SegmentReturnType BlockRow;
56  BlockRow V = ATA.row(k).segment(k + 1, order - (k + 1));
57  V *= betainv;
58 
59  // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
60  ATA.block(k + 1, k + 1, order - (k + 1), order - (k + 1)) -= V.transpose() * V;
61  // ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
62  // .rankUpdate(V.adjoint(), -1);
63  }
64  return 1;
65  } else {
66  // For zero pivots, add the underconstrained variable prior
67  ATA(k, k) = underconstrainedPrior;
68  for (size_t j = k + 1; j < order; ++j)
69  ATA(k, j) = 0.0;
70  return 0;
71  }
72 }
73 
74 /* ************************************************************************* */
75 pair<size_t, bool> choleskyCareful(Matrix& ATA, int order) {
76  // Check that the matrix is square (we do not check for symmetry)
77  assert(ATA.rows() == ATA.cols());
78 
79  // Number of rows/columns
80  const size_t n = ATA.rows();
81 
82  // Negative order means factor the entire matrix
83  if (order < 0)
84  order = int(n);
85 
86  assert(size_t(order) <= n);
87 
88  // The index of the row after the last non-zero row of the square-root factor
89  size_t maxrank = 0;
90  bool success = true;
91 
92  // Factor row-by-row
93  for (size_t k = 0; k < size_t(order); ++k) {
94  int stepResult = choleskyStep(ATA, k, size_t(order));
95  if (stepResult == 1) {
96  maxrank = k + 1;
97  } else if (stepResult == -1) {
98  success = false;
99  break;
100  } /* else if(stepResult == 0) Found zero pivot */
101  }
102 
103  return make_pair(maxrank, success);
104 }
105 
106 /* ************************************************************************* */
107 bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
109  if (nFrontal == 0)
110  return true;
111 
112  assert(ABC.cols() == ABC.rows());
113  assert(size_t(ABC.rows()) >= topleft);
114  const size_t n = static_cast<size_t>(ABC.rows() - topleft);
115  assert(nFrontal <= size_t(n));
116 
117  // Create views on blocks
118  auto A = ABC.block(topleft, topleft, nFrontal, nFrontal);
119  auto B = ABC.block(topleft, topleft + nFrontal, nFrontal, n - nFrontal);
120  auto C = ABC.block(topleft + nFrontal, topleft + nFrontal, n - nFrontal, n - nFrontal);
121 
122  // Compute Cholesky factorization A = R'*R, overwrites A.
123  gttic(LLT);
125  Eigen::ComputationInfo lltResult = llt.info();
126  if (lltResult != Eigen::Success)
127  return false;
128  auto R = A.triangularView<Eigen::Upper>();
129  R = llt.matrixU();
130  gttoc(LLT);
131 
132  // Compute S = inv(R') * B
133  gttic(compute_S);
134  if (nFrontal < n)
135  R.transpose().solveInPlace(B);
136  gttoc(compute_S);
137 
138  // Compute L = C - S' * S
139  gttic(compute_L);
140  if (nFrontal < n)
141  C.selfadjointView<Eigen::Upper>().rankUpdate(B.transpose(), -1.0);
142  gttoc(compute_L);
143 
144  // Check last diagonal element - Eigen does not check it
145  if (nFrontal >= 2) {
146  int exp2, exp1;
147  // NOTE(gareth): R is already the size of A, so we don't need to add topleft here.
148  (void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2);
149  (void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1);
150  return (exp2 - exp1 < underconstrainedExponentDifference);
151  } else if (nFrontal == 1) {
152  int exp1;
153  (void)frexp(R(0, 0), &exp1);
154  return (exp1 > -underconstrainedExponentDifference);
155  } else {
156  return true;
157  }
158 }
159 } // namespace gtsam
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
VectorBlock< Derived > SegmentReturnType
Definition: BlockMethods.h:38
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
Definition: BFloat16.h:88
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
static const int underconstrainedExponentDifference
#define gttic(label)
Definition: timing.h:295
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
Definition: llt.cpp:5
static const double zeroPivotThreshold
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Definition: LLT.h:66
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ComputationInfo info() const
Reports whether previous computation was successful.
Definition: LLT.h:191
static int choleskyStep(Matrix &ATA, size_t k, size_t order)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
traits
Definition: chartTesting.h:28
static const double negativePivotThreshold
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
#define gttoc(label)
Definition: timing.h:296
Traits::MatrixU matrixU() const
Definition: LLT.h:128
static const double underconstrainedPrior
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
ComputationInfo
Definition: Constants.h:440
std::ptrdiff_t j
Timing utilities.
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:01