WPILibC++ 2023.4.3-108-ge5452e3
ConditionEstimator.h
Go to the documentation of this file.
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2016 Rasmus Munk Larsen (rmlarsen@google.com)
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#ifndef EIGEN_CONDITIONESTIMATOR_H
11#define EIGEN_CONDITIONESTIMATOR_H
12
13namespace Eigen {
14
15namespace internal {
16
17template <typename Vector, typename RealVector, bool IsComplex>
19 static inline Vector run(const Vector& v) {
20 const RealVector v_abs = v.cwiseAbs();
21 return (v_abs.array() == static_cast<typename Vector::RealScalar>(0))
22 .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs));
23 }
24};
25
26// Partial specialization to avoid elementwise division for real vectors.
27template <typename Vector>
28struct rcond_compute_sign<Vector, Vector, false> {
29 static inline Vector run(const Vector& v) {
30 return (v.array() < static_cast<typename Vector::RealScalar>(0))
31 .select(-Vector::Ones(v.size()), Vector::Ones(v.size()));
32 }
33};
34
35/**
36 * \returns an estimate of ||inv(matrix)||_1 given a decomposition of
37 * \a matrix that implements .solve() and .adjoint().solve() methods.
38 *
39 * This function implements Algorithms 4.1 and 5.1 from
40 * http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf
41 * which also forms the basis for the condition number estimators in
42 * LAPACK. Since at most 10 calls to the solve method of dec are
43 * performed, the total cost is O(dims^2), as opposed to O(dims^3)
44 * needed to compute the inverse matrix explicitly.
45 *
46 * The most common usage is in estimating the condition number
47 * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be
48 * computed directly in O(n^2) operations.
49 *
50 * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and
51 * LLT.
52 *
53 * \sa FullPivLU, PartialPivLU, LDLT, LLT.
54 */
55template <typename Decomposition>
56typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec)
57{
58 typedef typename Decomposition::MatrixType MatrixType;
59 typedef typename Decomposition::Scalar Scalar;
60 typedef typename Decomposition::RealScalar RealScalar;
61 typedef typename internal::plain_col_type<MatrixType>::type Vector;
63 const bool is_complex = (NumTraits<Scalar>::IsComplex != 0);
64
65 eigen_assert(dec.rows() == dec.cols());
66 const Index n = dec.rows();
67 if (n == 0)
68 return 0;
69
70 // Disable Index to float conversion warning
71#ifdef __INTEL_COMPILER
72 #pragma warning push
73 #pragma warning ( disable : 2259 )
74#endif
75 Vector v = dec.solve(Vector::Ones(n) / Scalar(n));
76#ifdef __INTEL_COMPILER
77 #pragma warning pop
78#endif
79
80 // lower_bound is a lower bound on
81 // ||inv(matrix)||_1 = sup_v ||inv(matrix) v||_1 / ||v||_1
82 // and is the objective maximized by the ("super-") gradient ascent
83 // algorithm below.
84 RealScalar lower_bound = v.template lpNorm<1>();
85 if (n == 1)
86 return lower_bound;
87
88 // Gradient ascent algorithm follows: We know that the optimum is achieved at
89 // one of the simplices v = e_i, so in each iteration we follow a
90 // super-gradient to move towards the optimal one.
91 RealScalar old_lower_bound = lower_bound;
92 Vector sign_vector(n);
93 Vector old_sign_vector;
94 Index v_max_abs_index = -1;
95 Index old_v_max_abs_index = v_max_abs_index;
96 for (int k = 0; k < 4; ++k)
97 {
99 if (k > 0 && !is_complex && sign_vector == old_sign_vector) {
100 // Break if the solution stagnated.
101 break;
102 }
103 // v_max_abs_index = argmax |real( inv(matrix)^T * sign_vector )|
104 v = dec.adjoint().solve(sign_vector);
105 v.real().cwiseAbs().maxCoeff(&v_max_abs_index);
106 if (v_max_abs_index == old_v_max_abs_index) {
107 // Break if the solution stagnated.
108 break;
109 }
110 // Move to the new simplex e_j, where j = v_max_abs_index.
111 v = dec.solve(Vector::Unit(n, v_max_abs_index)); // v = inv(matrix) * e_j.
112 lower_bound = v.template lpNorm<1>();
113 if (lower_bound <= old_lower_bound) {
114 // Break if the gradient step did not increase the lower_bound.
115 break;
116 }
117 if (!is_complex) {
118 old_sign_vector = sign_vector;
119 }
120 old_v_max_abs_index = v_max_abs_index;
121 old_lower_bound = lower_bound;
122 }
123 // The following calculates an independent estimate of ||matrix||_1 by
124 // multiplying matrix by a vector with entries of slowly increasing
125 // magnitude and alternating sign:
126 // v_i = (-1)^{i} (1 + (i / (dim-1))), i = 0,...,dim-1.
127 // This improvement to Hager's algorithm above is due to Higham. It was
128 // added to make the algorithm more robust in certain corner cases where
129 // large elements in the matrix might otherwise escape detection due to
130 // exact cancellation (especially when op and op_adjoint correspond to a
131 // sequence of backsubstitutions and permutations), which could cause
132 // Hager's algorithm to vastly underestimate ||matrix||_1.
133 Scalar alternating_sign(RealScalar(1));
134 for (Index i = 0; i < n; ++i) {
135 // The static_cast is needed when Scalar is a complex and RealScalar implements expression templates
136 v[i] = alternating_sign * static_cast<RealScalar>(RealScalar(1) + (RealScalar(i) / (RealScalar(n - 1))));
137 alternating_sign = -alternating_sign;
138 }
139 v = dec.solve(v);
140 const RealScalar alternate_lower_bound = (2 * v.template lpNorm<1>()) / (3 * RealScalar(n));
141 return numext::maxi(lower_bound, alternate_lower_bound);
142}
143
144/** \brief Reciprocal condition number estimator.
145 *
146 * Computing a decomposition of a dense matrix takes O(n^3) operations, while
147 * this method estimates the condition number quickly and reliably in O(n^2)
148 * operations.
149 *
150 * \returns an estimate of the reciprocal condition number
151 * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and
152 * its decomposition. Supports the following decompositions: FullPivLU,
153 * PartialPivLU, LDLT, and LLT.
154 *
155 * \sa FullPivLU, PartialPivLU, LDLT, LLT.
156 */
157template <typename Decomposition>
158typename Decomposition::RealScalar
159rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Decomposition& dec)
160{
161 typedef typename Decomposition::RealScalar RealScalar;
162 eigen_assert(dec.rows() == dec.cols());
163 if (dec.rows() == 0) return NumTraits<RealScalar>::infinity();
164 if (matrix_norm == RealScalar(0)) return RealScalar(0);
165 if (dec.rows() == 1) return RealScalar(1);
166 const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec);
167 return (inverse_matrix_norm == RealScalar(0) ? RealScalar(0)
168 : (RealScalar(1) / inverse_matrix_norm) / matrix_norm);
169}
170
171} // namespace internal
172
173} // namespace Eigen
174
175#endif
#define eigen_assert(x)
Definition: Macros.h:1047
Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition &dec)
Definition: ConditionEstimator.h:56
Decomposition::RealScalar rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Decomposition &dec)
Reciprocal condition number estimator.
Definition: ConditionEstimator.h:159
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE T maxi(const T &x, const T &y)
Definition: MathFunctions.h:1091
Namespace containing all symbols from the Eigen library.
Definition: Core:141
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Definition: Eigen_Colamd.h:50
Holds information about the various numeric (i.e.
Definition: NumTraits.h:233
static Vector run(const Vector &v)
Definition: ConditionEstimator.h:29
Definition: ConditionEstimator.h:18
static Vector run(const Vector &v)
Definition: ConditionEstimator.h:19
Definition: Meta.h:96