Go to the documentation of this file.
37 double alpha = ATA(k, k);
42 }
else if (
alpha < 0.0)
48 const double betainv = 1.0 /
beta;
53 if (k < (order - 1)) {
56 BlockRow
V = ATA.row(k).segment(k + 1, order - (k + 1));
60 ATA.block(k + 1, k + 1, order - (k + 1), order - (k + 1)) -=
V.transpose() *
V;
68 for (
size_t j = k + 1;
j < order; ++
j)
77 assert(ATA.rows() == ATA.cols());
80 const size_t n = ATA.rows();
86 assert(
size_t(order) <=
n);
93 for (
size_t k = 0; k <
size_t(order); ++k) {
95 if (stepResult == 1) {
97 }
else if (stepResult == -1) {
103 return make_pair(maxrank, success);
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));
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);
135 R.transpose().solveInPlace(
B);
141 C.selfadjointView<
Eigen::Upper>().rankUpdate(
B.transpose(), -1.0);
148 (void)frexp(
R(nFrontal - 2, nFrontal - 2), &
exp2);
149 (void)frexp(
R(nFrontal - 1, nFrontal - 1), &exp1);
151 }
else if (nFrontal == 1) {
153 (void)frexp(
R(0, 0), &exp1);
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
static const double zeroPivotThreshold
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
VectorBlock< Derived > SegmentReturnType
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
static const double negativePivotThreshold
double beta(double a, double b)
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
static const double underconstrainedPrior
static int choleskyStep(Matrix &ATA, size_t k, size_t order)
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Matrix< Scalar, Dynamic, Dynamic > C
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
static const int underconstrainedExponentDifference
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:05