![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_MATRIX_SQUARE_ROOT 00011 #define EIGEN_MATRIX_SQUARE_ROOT 00012 00013 namespace Eigen { 00014 00015 namespace internal { 00016 00017 // pre: T.block(i,i,2,2) has complex conjugate eigenvalues 00018 // post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2) 00019 template <typename MatrixType, typename ResultType> 00020 void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, typename MatrixType::Index i, ResultType& sqrtT) 00021 { 00022 // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere 00023 // in EigenSolver. If we expose it, we could call it directly from here. 00024 typedef typename traits<MatrixType>::Scalar Scalar; 00025 Matrix<Scalar,2,2> block = T.template block<2,2>(i,i); 00026 EigenSolver<Matrix<Scalar,2,2> > es(block); 00027 sqrtT.template block<2,2>(i,i) 00028 = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real(); 00029 } 00030 00031 // pre: block structure of T is such that (i,j) is a 1x1 block, 00032 // all blocks of sqrtT to left of and below (i,j) are correct 00033 // post: sqrtT(i,j) has the correct value 00034 template <typename MatrixType, typename ResultType> 00035 void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT) 00036 { 00037 typedef typename traits<MatrixType>::Scalar Scalar; 00038 Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value(); 00039 sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j)); 00040 } 00041 00042 // similar to compute1x1offDiagonalBlock() 00043 template <typename MatrixType, typename ResultType> 00044 void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT) 00045 { 00046 typedef typename traits<MatrixType>::Scalar Scalar; 00047 Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j); 00048 if (j-i > 1) 00049 rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2); 00050 Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity(); 00051 A += sqrtT.template block<2,2>(j,j).transpose(); 00052 sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose()); 00053 } 00054 00055 // similar to compute1x1offDiagonalBlock() 00056 template <typename MatrixType, typename ResultType> 00057 void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT) 00058 { 00059 typedef typename traits<MatrixType>::Scalar Scalar; 00060 Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j); 00061 if (j-i > 2) 00062 rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1); 00063 Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity(); 00064 A += sqrtT.template block<2,2>(i,i); 00065 sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs); 00066 } 00067 00068 // solves the equation A X + X B = C where all matrices are 2-by-2 00069 template <typename MatrixType> 00070 void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B, const MatrixType& C) 00071 { 00072 typedef typename traits<MatrixType>::Scalar Scalar; 00073 Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero(); 00074 coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0); 00075 coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1); 00076 coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0); 00077 coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1); 00078 coeffMatrix.coeffRef(0,1) = B.coeff(1,0); 00079 coeffMatrix.coeffRef(0,2) = A.coeff(0,1); 00080 coeffMatrix.coeffRef(1,0) = B.coeff(0,1); 00081 coeffMatrix.coeffRef(1,3) = A.coeff(0,1); 00082 coeffMatrix.coeffRef(2,0) = A.coeff(1,0); 00083 coeffMatrix.coeffRef(2,3) = B.coeff(1,0); 00084 coeffMatrix.coeffRef(3,1) = A.coeff(1,0); 00085 coeffMatrix.coeffRef(3,2) = B.coeff(0,1); 00086 00087 Matrix<Scalar,4,1> rhs; 00088 rhs.coeffRef(0) = C.coeff(0,0); 00089 rhs.coeffRef(1) = C.coeff(0,1); 00090 rhs.coeffRef(2) = C.coeff(1,0); 00091 rhs.coeffRef(3) = C.coeff(1,1); 00092 00093 Matrix<Scalar,4,1> result; 00094 result = coeffMatrix.fullPivLu().solve(rhs); 00095 00096 X.coeffRef(0,0) = result.coeff(0); 00097 X.coeffRef(0,1) = result.coeff(1); 00098 X.coeffRef(1,0) = result.coeff(2); 00099 X.coeffRef(1,1) = result.coeff(3); 00100 } 00101 00102 // similar to compute1x1offDiagonalBlock() 00103 template <typename MatrixType, typename ResultType> 00104 void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT) 00105 { 00106 typedef typename traits<MatrixType>::Scalar Scalar; 00107 Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i); 00108 Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j); 00109 Matrix<Scalar,2,2> C = T.template block<2,2>(i,j); 00110 if (j-i > 2) 00111 C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2); 00112 Matrix<Scalar,2,2> X; 00113 matrix_sqrt_quasi_triangular_solve_auxiliary_equation(X, A, B, C); 00114 sqrtT.template block<2,2>(i,j) = X; 00115 } 00116 00117 // pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size 00118 // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T 00119 template <typename MatrixType, typename ResultType> 00120 void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT) 00121 { 00122 using std::sqrt; 00123 typedef typename MatrixType::Index Index; 00124 const Index size = T.rows(); 00125 for (Index i = 0; i < size; i++) { 00126 if (i == size - 1 || T.coeff(i+1, i) == 0) { 00127 eigen_assert(T(i,i) >= 0); 00128 sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i)); 00129 } 00130 else { 00131 matrix_sqrt_quasi_triangular_2x2_diagonal_block(T, i, sqrtT); 00132 ++i; 00133 } 00134 } 00135 } 00136 00137 // pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T. 00138 // post: sqrtT is the square root of T. 00139 template <typename MatrixType, typename ResultType> 00140 void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT) 00141 { 00142 typedef typename MatrixType::Index Index; 00143 const Index size = T.rows(); 00144 for (Index j = 1; j < size; j++) { 00145 if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block 00146 continue; 00147 for (Index i = j-1; i >= 0; i--) { 00148 if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block 00149 continue; 00150 bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0); 00151 bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0); 00152 if (iBlockIs2x2 && jBlockIs2x2) 00153 matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(T, i, j, sqrtT); 00154 else if (iBlockIs2x2 && !jBlockIs2x2) 00155 matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(T, i, j, sqrtT); 00156 else if (!iBlockIs2x2 && jBlockIs2x2) 00157 matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(T, i, j, sqrtT); 00158 else if (!iBlockIs2x2 && !jBlockIs2x2) 00159 matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(T, i, j, sqrtT); 00160 } 00161 } 00162 } 00163 00164 } // end of namespace internal 00165 00181 template <typename MatrixType, typename ResultType> 00182 void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result) 00183 { 00184 eigen_assert(arg.rows() == arg.cols()); 00185 result.resize(arg.rows(), arg.cols()); 00186 internal::matrix_sqrt_quasi_triangular_diagonal(arg, result); 00187 internal::matrix_sqrt_quasi_triangular_off_diagonal(arg, result); 00188 } 00189 00190 00205 template <typename MatrixType, typename ResultType> 00206 void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result) 00207 { 00208 using std::sqrt; 00209 typedef typename MatrixType::Index Index; 00210 typedef typename MatrixType::Scalar Scalar; 00211 00212 eigen_assert(arg.rows() == arg.cols()); 00213 00214 // Compute square root of arg and store it in upper triangular part of result 00215 // This uses that the square root of triangular matrices can be computed directly. 00216 result.resize(arg.rows(), arg.cols()); 00217 for (Index i = 0; i < arg.rows(); i++) { 00218 result.coeffRef(i,i) = sqrt(arg.coeff(i,i)); 00219 } 00220 for (Index j = 1; j < arg.cols(); j++) { 00221 for (Index i = j-1; i >= 0; i--) { 00222 // if i = j-1, then segment has length 0 so tmp = 0 00223 Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value(); 00224 // denominator may be zero if original matrix is singular 00225 result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j)); 00226 } 00227 } 00228 } 00229 00230 00231 namespace internal { 00232 00240 template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex> 00241 struct matrix_sqrt_compute 00242 { 00250 template <typename ResultType> static void run(const MatrixType &arg, ResultType &result); 00251 }; 00252 00253 00254 // ********** Partial specialization for real matrices ********** 00255 00256 template <typename MatrixType> 00257 struct matrix_sqrt_compute<MatrixType, 0> 00258 { 00259 template <typename ResultType> 00260 static void run(const MatrixType &arg, ResultType &result) 00261 { 00262 eigen_assert(arg.rows() == arg.cols()); 00263 00264 // Compute Schur decomposition of arg 00265 const RealSchur<MatrixType> schurOfA(arg); 00266 const MatrixType& T = schurOfA.matrixT(); 00267 const MatrixType& U = schurOfA.matrixU(); 00268 00269 // Compute square root of T 00270 MatrixType sqrtT = MatrixType::Zero(arg.rows(), arg.cols()); 00271 matrix_sqrt_quasi_triangular(T, sqrtT); 00272 00273 // Compute square root of arg 00274 result = U * sqrtT * U.adjoint(); 00275 } 00276 }; 00277 00278 00279 // ********** Partial specialization for complex matrices ********** 00280 00281 template <typename MatrixType> 00282 struct matrix_sqrt_compute<MatrixType, 1> 00283 { 00284 template <typename ResultType> 00285 static void run(const MatrixType &arg, ResultType &result) 00286 { 00287 eigen_assert(arg.rows() == arg.cols()); 00288 00289 // Compute Schur decomposition of arg 00290 const ComplexSchur<MatrixType> schurOfA(arg); 00291 const MatrixType& T = schurOfA.matrixT(); 00292 const MatrixType& U = schurOfA.matrixU(); 00293 00294 // Compute square root of T 00295 MatrixType sqrtT; 00296 matrix_sqrt_triangular(T, sqrtT); 00297 00298 // Compute square root of arg 00299 result = U * (sqrtT.template triangularView<Upper>() * U.adjoint()); 00300 } 00301 }; 00302 00303 } // end namespace internal 00304 00317 template<typename Derived> class MatrixSquareRootReturnValue 00318 : public ReturnByValue<MatrixSquareRootReturnValue<Derived> > 00319 { 00320 protected: 00321 typedef typename Derived::Index Index; 00322 typedef typename internal::ref_selector<Derived>::type DerivedNested; 00323 00324 public: 00330 explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { } 00331 00337 template <typename ResultType> 00338 inline void evalTo(ResultType& result) const 00339 { 00340 typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType; 00341 typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean; 00342 DerivedEvalType tmp(m_src); 00343 internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result); 00344 } 00345 00346 Index rows() const { return m_src.rows(); } 00347 Index cols() const { return m_src.cols(); } 00348 00349 protected: 00350 const DerivedNested m_src; 00351 }; 00352 00353 namespace internal { 00354 template<typename Derived> 00355 struct traits<MatrixSquareRootReturnValue<Derived> > 00356 { 00357 typedef typename Derived::PlainObject ReturnType; 00358 }; 00359 } 00360 00361 template <typename Derived> 00362 const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const 00363 { 00364 eigen_assert(rows() == cols()); 00365 return MatrixSquareRootReturnValue<Derived>(derived()); 00366 } 00367 00368 } // end namespace Eigen 00369 00370 #endif // EIGEN_MATRIX_FUNCTION