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


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:14