MatrixSquareRoot.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2011 Jitse Niesen <jitse@maths.leeds.ac.uk>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_MATRIX_SQUARE_ROOT
26 #define EIGEN_MATRIX_SQUARE_ROOT
27 
28 namespace Eigen {
29 
41 template <typename MatrixType>
43 {
44  public:
45 
54  MatrixSquareRootQuasiTriangular(const MatrixType& A)
55  : m_A(A)
56  {
57  eigen_assert(A.rows() == A.cols());
58  }
59 
68  template <typename ResultType> void compute(ResultType &result);
69 
70  private:
71  typedef typename MatrixType::Index Index;
72  typedef typename MatrixType::Scalar Scalar;
73 
74  void computeDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T);
75  void computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T);
76  void compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i);
77  void compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
78  typename MatrixType::Index i, typename MatrixType::Index j);
79  void compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
80  typename MatrixType::Index i, typename MatrixType::Index j);
81  void compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
82  typename MatrixType::Index i, typename MatrixType::Index j);
83  void compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
84  typename MatrixType::Index i, typename MatrixType::Index j);
85 
86  template <typename SmallMatrixType>
87  static void solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A,
88  const SmallMatrixType& B, const SmallMatrixType& C);
89 
90  const MatrixType& m_A;
91 };
92 
93 template <typename MatrixType>
94 template <typename ResultType>
96 {
97  // Compute Schur decomposition of m_A
98  const RealSchur<MatrixType> schurOfA(m_A);
99  const MatrixType& T = schurOfA.matrixT();
100  const MatrixType& U = schurOfA.matrixU();
101 
102  // Compute square root of T
103  MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows());
104  computeDiagonalPartOfSqrt(sqrtT, T);
105  computeOffDiagonalPartOfSqrt(sqrtT, T);
106 
107  // Compute square root of m_A
108  result = U * sqrtT * U.adjoint();
109 }
110 
111 // pre: T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
112 // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
113 template <typename MatrixType>
115  const MatrixType& T)
116 {
117  const Index size = m_A.rows();
118  for (Index i = 0; i < size; i++) {
119  if (i == size - 1 || T.coeff(i+1, i) == 0) {
120  eigen_assert(T(i,i) > 0);
121  sqrtT.coeffRef(i,i) = internal::sqrt(T.coeff(i,i));
122  }
123  else {
124  compute2x2diagonalBlock(sqrtT, T, i);
125  ++i;
126  }
127  }
128 }
129 
130 // pre: T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
131 // post: sqrtT is the square root of T.
132 template <typename MatrixType>
133 void MatrixSquareRootQuasiTriangular<MatrixType>::computeOffDiagonalPartOfSqrt(MatrixType& sqrtT,
134  const MatrixType& T)
135 {
136  const Index size = m_A.rows();
137  for (Index j = 1; j < size; j++) {
138  if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block
139  continue;
140  for (Index i = j-1; i >= 0; i--) {
141  if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block
142  continue;
143  bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
144  bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
145  if (iBlockIs2x2 && jBlockIs2x2)
146  compute2x2offDiagonalBlock(sqrtT, T, i, j);
147  else if (iBlockIs2x2 && !jBlockIs2x2)
148  compute2x1offDiagonalBlock(sqrtT, T, i, j);
149  else if (!iBlockIs2x2 && jBlockIs2x2)
150  compute1x2offDiagonalBlock(sqrtT, T, i, j);
151  else if (!iBlockIs2x2 && !jBlockIs2x2)
152  compute1x1offDiagonalBlock(sqrtT, T, i, j);
153  }
154  }
155 }
156 
157 // pre: T.block(i,i,2,2) has complex conjugate eigenvalues
158 // post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
159 template <typename MatrixType>
160 void MatrixSquareRootQuasiTriangular<MatrixType>
161  ::compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i)
162 {
163  // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
164  // in EigenSolver. If we expose it, we could call it directly from here.
165  Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
166  EigenSolver<Matrix<Scalar,2,2> > es(block);
167  sqrtT.template block<2,2>(i,i)
168  = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
169 }
170 
171 // pre: block structure of T is such that (i,j) is a 1x1 block,
172 // all blocks of sqrtT to left of and below (i,j) are correct
173 // post: sqrtT(i,j) has the correct value
174 template <typename MatrixType>
175 void MatrixSquareRootQuasiTriangular<MatrixType>
176  ::compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
177  typename MatrixType::Index i, typename MatrixType::Index j)
178 {
179  Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
180  sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
181 }
182 
183 // similar to compute1x1offDiagonalBlock()
184 template <typename MatrixType>
185 void MatrixSquareRootQuasiTriangular<MatrixType>
186  ::compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
187  typename MatrixType::Index i, typename MatrixType::Index j)
188 {
189  Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
190  if (j-i > 1)
191  rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
192  Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity();
193  A += sqrtT.template block<2,2>(j,j).transpose();
194  sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
195 }
196 
197 // similar to compute1x1offDiagonalBlock()
198 template <typename MatrixType>
199 void MatrixSquareRootQuasiTriangular<MatrixType>
200  ::compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
201  typename MatrixType::Index i, typename MatrixType::Index j)
202 {
203  Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
204  if (j-i > 2)
205  rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
206  Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity();
207  A += sqrtT.template block<2,2>(i,i);
208  sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
209 }
210 
211 // similar to compute1x1offDiagonalBlock()
212 template <typename MatrixType>
213 void MatrixSquareRootQuasiTriangular<MatrixType>
214  ::compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
215  typename MatrixType::Index i, typename MatrixType::Index j)
216 {
217  Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
218  Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
219  Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
220  if (j-i > 2)
221  C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
222  Matrix<Scalar,2,2> X;
223  solveAuxiliaryEquation(X, A, B, C);
224  sqrtT.template block<2,2>(i,j) = X;
225 }
226 
227 // solves the equation A X + X B = C where all matrices are 2-by-2
228 template <typename MatrixType>
229 template <typename SmallMatrixType>
230 void MatrixSquareRootQuasiTriangular<MatrixType>
231  ::solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A,
232  const SmallMatrixType& B, const SmallMatrixType& C)
233 {
234  EIGEN_STATIC_ASSERT((internal::is_same<SmallMatrixType, Matrix<Scalar,2,2> >::value),
235  EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT);
236 
237  Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
238  coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
239  coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
240  coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
241  coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
242  coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
243  coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
244  coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
245  coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
246  coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
247  coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
248  coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
249  coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
250 
251  Matrix<Scalar,4,1> rhs;
252  rhs.coeffRef(0) = C.coeff(0,0);
253  rhs.coeffRef(1) = C.coeff(0,1);
254  rhs.coeffRef(2) = C.coeff(1,0);
255  rhs.coeffRef(3) = C.coeff(1,1);
256 
257  Matrix<Scalar,4,1> result;
258  result = coeffMatrix.fullPivLu().solve(rhs);
259 
260  X.coeffRef(0,0) = result.coeff(0);
261  X.coeffRef(0,1) = result.coeff(1);
262  X.coeffRef(1,0) = result.coeff(2);
263  X.coeffRef(1,1) = result.coeff(3);
264 }
265 
266 
278 template <typename MatrixType>
280 {
281  public:
282  MatrixSquareRootTriangular(const MatrixType& A)
283  : m_A(A)
284  {
285  eigen_assert(A.rows() == A.cols());
286  }
287 
297  template <typename ResultType> void compute(ResultType &result);
298 
299  private:
300  const MatrixType& m_A;
301 };
302 
303 template <typename MatrixType>
304 template <typename ResultType>
306 {
307  // Compute Schur decomposition of m_A
308  const ComplexSchur<MatrixType> schurOfA(m_A);
309  const MatrixType& T = schurOfA.matrixT();
310  const MatrixType& U = schurOfA.matrixU();
311 
312  // Compute square root of T and store it in upper triangular part of result
313  // This uses that the square root of triangular matrices can be computed directly.
314  result.resize(m_A.rows(), m_A.cols());
315  typedef typename MatrixType::Index Index;
316  for (Index i = 0; i < m_A.rows(); i++) {
317  result.coeffRef(i,i) = internal::sqrt(T.coeff(i,i));
318  }
319  for (Index j = 1; j < m_A.cols(); j++) {
320  for (Index i = j-1; i >= 0; i--) {
321  typedef typename MatrixType::Scalar Scalar;
322  // if i = j-1, then segment has length 0 so tmp = 0
323  Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
324  // denominator may be zero if original matrix is singular
325  result.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
326  }
327  }
328 
329  // Compute square root of m_A as U * result * U.adjoint()
330  MatrixType tmp;
331  tmp.noalias() = U * result.template triangularView<Upper>();
332  result.noalias() = tmp * U.adjoint();
333 }
334 
335 
343 template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
345 {
346  public:
347 
355  MatrixSquareRoot(const MatrixType& A);
356 
364  template <typename ResultType> void compute(ResultType &result);
365 };
366 
367 
368 // ********** Partial specialization for real matrices **********
369 
370 template <typename MatrixType>
371 class MatrixSquareRoot<MatrixType, 0>
372 {
373  public:
374 
375  MatrixSquareRoot(const MatrixType& A)
376  : m_A(A)
377  {
378  eigen_assert(A.rows() == A.cols());
379  }
380 
381  template <typename ResultType> void compute(ResultType &result)
382  {
383  // Compute Schur decomposition of m_A
384  const RealSchur<MatrixType> schurOfA(m_A);
385  const MatrixType& T = schurOfA.matrixT();
386  const MatrixType& U = schurOfA.matrixU();
387 
388  // Compute square root of T
389  MatrixSquareRootQuasiTriangular<MatrixType> tmp(T);
390  MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows());
391  tmp.compute(sqrtT);
392 
393  // Compute square root of m_A
394  result = U * sqrtT * U.adjoint();
395  }
396 
397  private:
398  const MatrixType& m_A;
399 };
400 
401 
402 // ********** Partial specialization for complex matrices **********
403 
404 template <typename MatrixType>
405 class MatrixSquareRoot<MatrixType, 1>
406 {
407  public:
408 
409  MatrixSquareRoot(const MatrixType& A)
410  : m_A(A)
411  {
412  eigen_assert(A.rows() == A.cols());
413  }
414 
415  template <typename ResultType> void compute(ResultType &result)
416  {
417  // Compute Schur decomposition of m_A
418  const ComplexSchur<MatrixType> schurOfA(m_A);
419  const MatrixType& T = schurOfA.matrixT();
420  const MatrixType& U = schurOfA.matrixU();
421 
422  // Compute square root of T
423  MatrixSquareRootTriangular<MatrixType> tmp(T);
424  MatrixType sqrtT = MatrixType::Zero(m_A.rows(), m_A.rows());
425  tmp.compute(sqrtT);
426 
427  // Compute square root of m_A
428  result = U * sqrtT * U.adjoint();
429  }
430 
431  private:
432  const MatrixType& m_A;
433 };
434 
435 
448 template<typename Derived> class MatrixSquareRootReturnValue
449 : public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
450 {
451  typedef typename Derived::Index Index;
452  public:
458  MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
459 
465  template <typename ResultType>
466  inline void evalTo(ResultType& result) const
467  {
468  const typename Derived::PlainObject srcEvaluated = m_src.eval();
470  me.compute(result);
471  }
472 
473  Index rows() const { return m_src.rows(); }
474  Index cols() const { return m_src.cols(); }
475 
476  protected:
477  const Derived& m_src;
478  private:
480 };
481 
482 namespace internal {
483 template<typename Derived>
484 struct traits<MatrixSquareRootReturnValue<Derived> >
485 {
486  typedef typename Derived::PlainObject ReturnType;
487 };
488 }
489 
490 template <typename Derived>
491 const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
492 {
493  eigen_assert(rows() == cols());
494  return MatrixSquareRootReturnValue<Derived>(derived());
495 }
496 
497 } // end namespace Eigen
498 
499 #endif // EIGEN_MATRIX_FUNCTION