MatrixSquareRoot.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) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
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_MATRIX_SQUARE_ROOT
11 #define EIGEN_MATRIX_SQUARE_ROOT
12 
13 namespace Eigen {
14 
15 namespace internal {
16 
17 // pre: T.block(i,i,2,2) has complex conjugate eigenvalues
18 // post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
19 template <typename MatrixType, typename ResultType>
21 {
22  // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
23  // in EigenSolver. If we expose it, we could call it directly from here.
24  typedef typename traits<MatrixType>::Scalar Scalar;
25  Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
27  sqrtT.template block<2,2>(i,i)
28  = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
29 }
30 
31 // pre: block structure of T is such that (i,j) is a 1x1 block,
32 // all blocks of sqrtT to left of and below (i,j) are correct
33 // post: sqrtT(i,j) has the correct value
34 template <typename MatrixType, typename ResultType>
36 {
37  typedef typename traits<MatrixType>::Scalar Scalar;
38  Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
39  sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
40 }
41 
42 // similar to compute1x1offDiagonalBlock()
43 template <typename MatrixType, typename ResultType>
45 {
46  typedef typename traits<MatrixType>::Scalar Scalar;
47  Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
48  if (j-i > 1)
49  rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
51  A += sqrtT.template block<2,2>(j,j).transpose();
52  sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
53 }
54 
55 // similar to compute1x1offDiagonalBlock()
56 template <typename MatrixType, typename ResultType>
58 {
59  typedef typename traits<MatrixType>::Scalar Scalar;
60  Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
61  if (j-i > 2)
62  rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
64  A += sqrtT.template block<2,2>(i,i);
65  sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
66 }
67 
68 // solves the equation A X + X B = C where all matrices are 2-by-2
69 template <typename MatrixType>
71 {
72  typedef typename traits<MatrixType>::Scalar Scalar;
74  coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
75  coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
76  coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
77  coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
78  coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
79  coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
80  coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
81  coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
82  coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
83  coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
84  coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
85  coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
86 
88  rhs.coeffRef(0) = C.coeff(0,0);
89  rhs.coeffRef(1) = C.coeff(0,1);
90  rhs.coeffRef(2) = C.coeff(1,0);
91  rhs.coeffRef(3) = C.coeff(1,1);
92 
94  result = coeffMatrix.fullPivLu().solve(rhs);
95 
96  X.coeffRef(0,0) = result.coeff(0);
97  X.coeffRef(0,1) = result.coeff(1);
98  X.coeffRef(1,0) = result.coeff(2);
99  X.coeffRef(1,1) = result.coeff(3);
100 }
101 
102 // similar to compute1x1offDiagonalBlock()
103 template <typename MatrixType, typename ResultType>
105 {
106  typedef typename traits<MatrixType>::Scalar Scalar;
107  Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
108  Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
109  Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
110  if (j-i > 2)
111  C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
114  sqrtT.template block<2,2>(i,j) = X;
115 }
116 
117 // pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
118 // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
119 template <typename MatrixType, typename ResultType>
120 void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
121 {
122  using std::sqrt;
123  const Index size = T.rows();
124  for (Index i = 0; i < size; i++) {
125  if (i == size - 1 || T.coeff(i+1, i) == 0) {
126  eigen_assert(T(i,i) >= 0);
127  sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i));
128  }
129  else {
131  ++i;
132  }
133  }
134 }
135 
136 // pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
137 // post: sqrtT is the square root of T.
138 template <typename MatrixType, typename ResultType>
140 {
141  const Index size = T.rows();
142  for (Index j = 1; j < size; j++) {
143  if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block
144  continue;
145  for (Index i = j-1; i >= 0; i--) {
146  if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block
147  continue;
148  bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
149  bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
150  if (iBlockIs2x2 && jBlockIs2x2)
152  else if (iBlockIs2x2 && !jBlockIs2x2)
154  else if (!iBlockIs2x2 && jBlockIs2x2)
156  else if (!iBlockIs2x2 && !jBlockIs2x2)
158  }
159  }
160 }
161 
162 } // end of namespace internal
163 
179 template <typename MatrixType, typename ResultType>
181 {
182  eigen_assert(arg.rows() == arg.cols());
183  result.resize(arg.rows(), arg.cols());
186 }
187 
188 
203 template <typename MatrixType, typename ResultType>
204 void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
205 {
206  using std::sqrt;
207  typedef typename MatrixType::Scalar Scalar;
208 
209  eigen_assert(arg.rows() == arg.cols());
210 
211  // Compute square root of arg and store it in upper triangular part of result
212  // This uses that the square root of triangular matrices can be computed directly.
213  result.resize(arg.rows(), arg.cols());
214  for (Index i = 0; i < arg.rows(); i++) {
215  result.coeffRef(i,i) = sqrt(arg.coeff(i,i));
216  }
217  for (Index j = 1; j < arg.cols(); j++) {
218  for (Index i = j-1; i >= 0; i--) {
219  // if i = j-1, then segment has length 0 so tmp = 0
220  Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
221  // denominator may be zero if original matrix is singular
222  result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
223  }
224  }
225 }
226 
227 
228 namespace internal {
229 
239 {
247  template <typename ResultType> static void run(const MatrixType &arg, ResultType &result);
248 };
249 
250 
251 // ********** Partial specialization for real matrices **********
252 
253 template <typename MatrixType>
255 {
256  typedef typename MatrixType::PlainObject PlainType;
257  template <typename ResultType>
258  static void run(const MatrixType &arg, ResultType &result)
259  {
260  eigen_assert(arg.rows() == arg.cols());
261 
262  // Compute Schur decomposition of arg
264  const PlainType& T = schurOfA.matrixT();
265  const PlainType& U = schurOfA.matrixU();
266 
267  // Compute square root of T
268  PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols());
270 
271  // Compute square root of arg
272  result = U * sqrtT * U.adjoint();
273  }
274 };
275 
276 
277 // ********** Partial specialization for complex matrices **********
278 
279 template <typename MatrixType>
281 {
282  typedef typename MatrixType::PlainObject PlainType;
283  template <typename ResultType>
284  static void run(const MatrixType &arg, ResultType &result)
285  {
286  eigen_assert(arg.rows() == arg.cols());
287 
288  // Compute Schur decomposition of arg
290  const PlainType& T = schurOfA.matrixT();
291  const PlainType& U = schurOfA.matrixU();
292 
293  // Compute square root of T
294  PlainType sqrtT;
295  matrix_sqrt_triangular(T, sqrtT);
296 
297  // Compute square root of arg
298  result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
299  }
300 };
301 
302 } // end namespace internal
303 
316 template<typename Derived> class MatrixSquareRootReturnValue
317 : public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
318 {
319  protected:
321 
322  public:
328  explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
329 
335  template <typename ResultType>
336  inline void evalTo(ResultType& result) const
337  {
338  typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
339  typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
340  DerivedEvalType tmp(m_src);
342  }
343 
344  Index rows() const { return m_src.rows(); }
345  Index cols() const { return m_src.cols(); }
346 
347  protected:
349 };
350 
351 namespace internal {
352 template<typename Derived>
354 {
355  typedef typename Derived::PlainObject ReturnType;
356 };
357 }
358 
359 template <typename Derived>
361 {
362  eigen_assert(rows() == cols());
363  return MatrixSquareRootReturnValue<Derived>(derived());
364 }
365 
366 } // end namespace Eigen
367 
368 #endif // EIGEN_MATRIX_FUNCTION
es
EigenSolver< MatrixXf > es
Definition: EigenSolver_compute.cpp:1
Eigen::MatrixSquareRootReturnValue::cols
Index cols() const
Definition: MatrixSquareRoot.h:345
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::matrix_sqrt_quasi_triangular
void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
Compute matrix square root of quasi-triangular matrix.
Definition: MatrixSquareRoot.h:180
Eigen::internal::matrix_sqrt_quasi_triangular_1x1_off_diagonal_block
void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:35
Eigen::internal::traits< MatrixSquareRootReturnValue< Derived > >::ReturnType
Derived::PlainObject ReturnType
Definition: MatrixSquareRoot.h:355
MatrixType
MatrixXf MatrixType
Definition: benchmark-blocking-sizes.cpp:52
Eigen::internal::matrix_sqrt_quasi_triangular_2x2_diagonal_block
void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType &T, Index i, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:20
Eigen::MatrixSquareRootReturnValue
Proxy for the matrix square root of some matrix (expression).
Definition: ForwardDeclarations.h:306
eigen_assert
#define eigen_assert(x)
Definition: Macros.h:1037
B
Definition: test_numpy_dtypes.cpp:299
Eigen::internal::matrix_sqrt_quasi_triangular_diagonal
void matrix_sqrt_quasi_triangular_diagonal(const MatrixType &T, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:120
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
schurOfA
cout<< "Here is a random 4x4 matrix, A:"<< endl<< A<< endl<< endl;ComplexSchur< MatrixXcf > schurOfA(A, false)
X
#define X
Definition: icosphere.cpp:20
Eigen::internal::matrix_sqrt_compute< MatrixType, 1 >::PlainType
MatrixType::PlainObject PlainType
Definition: MatrixSquareRoot.h:282
Eigen::internal::matrix_sqrt_compute
Helper struct for computing matrix square roots of general matrices.
Definition: MatrixSquareRoot.h:238
Eigen::internal::matrix_sqrt_compute< MatrixType, 0 >::PlainType
MatrixType::PlainObject PlainType
Definition: MatrixSquareRoot.h:256
result
Values result
Definition: OdometryOptimize.cpp:8
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
block
m m block(1, 0, 2, 2)<< 4
IsComplex
@ IsComplex
Definition: gtsam/3rdparty/Eigen/blas/common.h:98
Eigen::internal::matrix_sqrt_quasi_triangular_2x2_off_diagonal_block
void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:104
Eigen::MatrixSquareRootReturnValue::m_src
const DerivedNested m_src
Definition: MatrixSquareRoot.h:348
Eigen::internal::true_type
Definition: Meta.h:96
Eigen::RealSchur
Performs a real Schur decomposition of a square matrix.
Definition: RealSchur.h:54
A
Definition: test_numpy_dtypes.cpp:298
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
arg
Definition: cast.h:1412
Eigen::internal::matrix_sqrt_compute< MatrixType, 1 >::run
static void run(const MatrixType &arg, ResultType &result)
Definition: MatrixSquareRoot.h:284
Eigen::MatrixSquareRootReturnValue::MatrixSquareRootReturnValue
MatrixSquareRootReturnValue(const Derived &src)
Constructor.
Definition: MatrixSquareRoot.h:328
Eigen::internal::matrix_sqrt_compute< MatrixType, 0 >::run
static void run(const MatrixType &arg, ResultType &result)
Definition: MatrixSquareRoot.h:258
Eigen::Matrix::coeffRef
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Definition: PlainObjectBase.h:175
Eigen::Triplet
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:162
Eigen::matrix_sqrt_triangular
void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
Compute matrix square root of triangular matrix.
Definition: MatrixSquareRoot.h:204
Eigen::real
const AutoDiffScalar< DerType > & real(const AutoDiffScalar< DerType > &x)
Definition: AutoDiffScalar.h:576
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
Eigen::internal::traits
Definition: ForwardDeclarations.h:17
Eigen::PlainObjectBase::coeff
EIGEN_DEVICE_FUNC const EIGEN_STRONG_INLINE Scalar & coeff(Index rowId, Index colId) const
Definition: PlainObjectBase.h:152
Eigen::MatrixSquareRootReturnValue::rows
Index rows() const
Definition: MatrixSquareRoot.h:344
Eigen::MatrixSquareRootReturnValue::evalTo
void evalTo(ResultType &result) const
Compute the matrix square root.
Definition: MatrixSquareRoot.h:336
Eigen::EigenSolver
Computes eigenvalues and eigenvectors of general matrices.
Definition: EigenSolver.h:64
Eigen::internal::matrix_sqrt_quasi_triangular_2x1_off_diagonal_block
void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:57
U
@ U
Definition: testDecisionTree.cpp:349
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
internal
Definition: BandTriangularSolver.h:13
Eigen::internal::matrix_sqrt_quasi_triangular_off_diagonal
void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType &T, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:139
Eigen::internal::matrix_sqrt_quasi_triangular_1x2_off_diagonal_block
void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType &T, Index i, Index j, ResultType &sqrtT)
Definition: MatrixSquareRoot.h:44
Eigen::internal::size
EIGEN_CONSTEXPR Index size(const T &x)
Definition: Meta.h:479
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
Eigen::MatrixSquareRootReturnValue::DerivedNested
internal::ref_selector< Derived >::type DerivedNested
Definition: MatrixSquareRoot.h:320
Eigen::ComplexSchur
Performs a complex Schur decomposition of a real or complex square matrix.
Definition: ComplexSchur.h:51
test_callbacks.value
value
Definition: test_callbacks.py:160
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Eigen::internal::matrix_sqrt_compute::run
static void run(const MatrixType &arg, ResultType &result)
Compute the matrix square root.
Eigen::internal::matrix_sqrt_quasi_triangular_solve_auxiliary_equation
void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType &X, const MatrixType &A, const MatrixType &B, const MatrixType &C)
Definition: MatrixSquareRoot.h:70
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:46