10 #ifndef EIGEN_MATRIX_FUNCTION_H 11 #define EIGEN_MATRIX_FUNCTION_H 29 template <
typename MatrixType>
52 template <
typename MatrixType>
57 const MatrixType N = MatrixType::Identity(rows, rows) - A;
58 VectorType
e = VectorType::Ones(rows);
59 N.template triangularView<Upper>().solveInPlace(e);
60 return e.cwiseAbs().maxCoeff();
63 template <
typename MatrixType>
69 Index
rows = A.rows();
71 MatrixType Ashifted = A - avgEival * MatrixType::Identity(rows, rows);
73 MatrixType F =
m_f(avgEival, 0) * MatrixType::Identity(rows, rows);
76 for (Index
s = 1;
s < 1.1 * rows + 10;
s++) {
77 Fincr =
m_f(avgEival, static_cast<int>(
s)) *
P;
82 const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
83 const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff();
86 RealScalar rfactorial = 1;
87 for (Index r = 0; r <
rows; r++) {
89 for (Index
i = 0;
i <
rows;
i++)
93 delta = (
std::max)(delta, mx / rfactorial);
95 const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
108 template <
typename Index,
typename ListOfClusters>
111 typename std::list<Index>::iterator
j;
112 for (
typename ListOfClusters::iterator
i = clusters.begin();
i != clusters.end(); ++
i) {
113 j = std::find(
i->begin(),
i->end(),
key);
117 return clusters.end();
131 template <
typename EivalsType,
typename Cluster>
136 for (Index
i=0;
i<eivals.rows(); ++
i) {
139 if (qi == clusters.end()) {
142 clusters.push_back(l);
148 for (Index
j=
i+1;
j<eivals.rows(); ++
j) {
150 && std::find(qi->begin(), qi->end(),
j) == qi->end()) {
152 if (qj == clusters.end()) {
155 qi->insert(qi->end(), qj->begin(), qj->end());
164 template <
typename ListOfClusters,
typename Index>
167 const Index numClusters =
static_cast<Index>(clusters.size());
168 clusterSize.
setZero(numClusters);
169 Index clusterIndex = 0;
170 for (
typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
171 clusterSize[clusterIndex] = cluster->size();
177 template <
typename VectorType>
180 blockStart.resize(clusterSize.rows());
183 blockStart(
i) = blockStart(
i-1) + clusterSize(
i-1);
188 template <
typename EivalsType,
typename ListOfClusters,
typename VectorType>
192 eivalToCluster.resize(eivals.rows());
193 Index clusterIndex = 0;
194 for (
typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
195 for (Index
i = 0;
i < eivals.rows(); ++
i) {
196 if (std::find(cluster->begin(), cluster->end(),
i) != cluster->end()) {
197 eivalToCluster[
i] = clusterIndex;
205 template <
typename DynVectorType,
typename VectorType>
209 DynVectorType indexNextEntry = blockStart;
210 permutation.resize(eivalToCluster.rows());
211 for (Index
i = 0;
i < eivalToCluster.rows();
i++) {
212 Index cluster = eivalToCluster[
i];
213 permutation[
i] = indexNextEntry[cluster];
214 ++indexNextEntry[cluster];
219 template <
typename VectorType,
typename MatrixType>
223 for (Index
i = 0;
i < permutation.rows() - 1;
i++) {
225 for (j =
i; j < permutation.rows(); j++) {
226 if (permutation(j) ==
i)
break;
229 for (Index k = j-1; k >=
i; k--) {
232 T.applyOnTheLeft(k, k+1, rotation.
adjoint());
233 T.applyOnTheRight(k, k+1, rotation);
234 U.applyOnTheRight(k, k+1, rotation);
235 std::swap(permutation.coeffRef(k), permutation.coeffRef(k+1));
246 template <
typename MatrixType,
typename AtomicType,
typename VectorType>
249 fT.setZero(T.rows(), T.cols());
251 fT.block(blockStart(
i), blockStart(
i), clusterSize(
i), clusterSize(
i))
252 = atomic.compute(T.block(blockStart(
i), blockStart(
i), clusterSize(
i), clusterSize(
i)));
278 template <
typename MatrixType>
295 for (Index
i = m - 1;
i >= 0; --
i) {
296 for (Index
j = 0;
j <
n; ++
j) {
316 X(
i,
j) = (
C(
i,
j) - AX - XB) / (A(
i,
i) + B(
j,
j));
328 template <
typename MatrixType,
typename VectorType>
334 static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
335 static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
336 static const int Options = MatrixType::Options;
339 for (Index k = 1; k < clusterSize.rows(); k++) {
340 for (Index
i = 0;
i < clusterSize.rows() - k;
i++) {
342 DynMatrixType
A = T.block(blockStart(
i), blockStart(
i), clusterSize(
i), clusterSize(
i));
343 DynMatrixType
B = -T.block(blockStart(
i+k), blockStart(
i+k), clusterSize(
i+k), clusterSize(
i+k));
344 DynMatrixType
C = fT.block(blockStart(
i), blockStart(
i), clusterSize(
i), clusterSize(
i))
345 * T.block(blockStart(
i), blockStart(
i+k), clusterSize(
i), clusterSize(
i+k));
346 C -= T.block(blockStart(
i), blockStart(
i+k), clusterSize(
i), clusterSize(
i+k))
347 * fT.block(blockStart(
i+k), blockStart(
i+k), clusterSize(
i+k), clusterSize(
i+k));
348 for (Index
m =
i + 1;
m <
i + k;
m++) {
349 C += fT.block(blockStart(
i), blockStart(
m), clusterSize(
i), clusterSize(
m))
350 * T.block(blockStart(
m), blockStart(
i+k), clusterSize(
m), clusterSize(
i+k));
351 C -= T.block(blockStart(
i), blockStart(
m), clusterSize(
i), clusterSize(
m))
352 * fT.block(blockStart(
m), blockStart(
i+k), clusterSize(
m), clusterSize(
i+k));
354 fT.block(blockStart(
i), blockStart(
i+k), clusterSize(
i), clusterSize(
i+k))
388 template <
typename AtomicType,
typename ResultType>
398 template <
typename MatrixType>
401 template <
typename MatA,
typename AtomicType,
typename ResultType>
402 static void run(
const MatA&
A, AtomicType& atomic, ResultType &
result)
406 static const int Rows = Traits::RowsAtCompileTime,
Cols = Traits::ColsAtCompileTime;
407 static const int MaxRows = Traits::MaxRowsAtCompileTime, MaxCols = Traits::MaxColsAtCompileTime;
409 typedef std::complex<Scalar> ComplexScalar;
412 ComplexMatrix CA = A.template cast<ComplexScalar>();
413 ComplexMatrix Cresult;
415 result = Cresult.real();
422 template <
typename MatrixType>
425 template <
typename MatA,
typename AtomicType,
typename ResultType>
426 static void run(
const MatA&
A, AtomicType& atomic, ResultType &
result)
436 std::list<std::list<Index> > clusters;
462 result = U * (fT.template triangularView<Upper>() * U.adjoint());
502 template <
typename ResultType>
508 static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
509 static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
514 AtomicType atomic(
m_f);
519 Index
rows()
const {
return m_A.rows(); }
520 Index
cols()
const {
return m_A.cols(); }
528 template<
typename Derived>
539 template <
typename Derived>
546 template <
typename Derived>
554 template <
typename Derived>
562 template <
typename Derived>
570 template <
typename Derived>
580 #endif // EIGEN_MATRIX_FUNCTION_H Derived::PlainObject ReturnType
void matrix_function_compute_permutation(const DynVectorType &blockStart, const DynVectorType &eivalToCluster, VectorType &permutation)
Compute permutation which groups ei'vals in same cluster together.
void matrix_function_compute_map(const EivalsType &eivals, const ListOfClusters &clusters, VectorType &eivalToCluster)
Compute mapping of eigenvalue indices to cluster indices.
std::complex< typename NumTraits< Scalar >::Real > ComplexScalar
EIGEN_DEVICE_FUNC Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > & setZero(Index size)
cout<< "Here is a random 4x4 matrix, A:"<< endl<< A<< endl<< endl;ComplexSchur< MatrixXcf > schurOfA(A, false)
MatrixType::Scalar Scalar
MatrixType compute(const MatrixType &A)
Compute matrix function of atomic matrix.
internal::ref_selector< Derived >::type DerivedNested
static void run(const MatA &A, AtomicType &atomic, ResultType &result)
EIGEN_DEVICE_FUNC const CoshReturnType cosh() const
Namespace containing all symbols from the Eigen library.
Rotation given by a cosine-sine pair.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
static void run(const MatA &A, AtomicType &atomic, ResultType &result)
Proxy for the matrix function of some matrix (expression).
NumTraits< typename MatrixType::Scalar >::Real matrix_function_compute_mu(const MatrixType &A)
void matrix_function_compute_block_start(const VectorType &clusterSize, VectorType &blockStart)
Compute start of each block using clusterSize.
MatrixFunctionReturnValue(const Derived &A, StemFunction f)
Constructor.
EIGEN_DEVICE_FUNC const CosReturnType cos() const
void matrix_function_compute_block_atomic(const MatrixType &T, AtomicType &atomic, const VectorType &blockStart, const VectorType &clusterSize, MatrixType &fT)
Compute block diagonal part of matrix function.
static const Line3 l(Rot3(), 1, 1)
void matrix_function_partition_eigenvalues(const EivalsType &eivals, std::list< Cluster > &clusters)
Partition eigenvalues in clusters of ei'vals close to each other.
EIGEN_DEVICE_FUNC const SinhReturnType sinh() const
ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters &clusters)
Find cluster in clusters containing some value.
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Class for computing matrix functions.
JacobiRotation adjoint() const
const ComplexMatrixType & matrixT() const
Returns the triangular matrix in the Schur decomposition.
NumTraits< Scalar >::Real RealScalar
void matrix_function_compute_cluster_size(const ListOfClusters &clusters, Matrix< Index, Dynamic, 1 > &clusterSize)
Compute size of each cluster given a partitioning.
Matrix< Scalar, Dynamic, Dynamic > C
MatrixFunctionAtomic(StemFunction f)
Constructor.
void matrix_function_permute_schur(VectorType &permutation, MatrixType &U, MatrixType &T)
Permute Schur decomposition in U and T according to permutation.
internal::stem_function< Scalar >::type StemFunction
void evalTo(ResultType &result) const
Compute the matrix function.
A small structure to hold a non zero as a triplet (i,j,value).
mp::number< mp::cpp_dec_float< 100 >, mp::et_on > Real
static const float matrix_function_separation
Maximum distance allowed between eigenvalues to be considered "close".
void matrix_function_compute_above_diagonal(const MatrixType &T, const VectorType &blockStart, const VectorType &clusterSize, MatrixType &fT)
Compute part of matrix function above block diagonal.
static void run(const MatrixType &A, AtomicType &atomic, ResultType &result)
Compute the matrix function.
Helper class for computing matrix functions of atomic matrices.
const MatrixFunctionReturnValue< Derived > matrixFunction(StemFunction f) const
Helper function for the unsupported MatrixFunctions module.
EIGEN_DEVICE_FUNC const SinReturnType sin() const
stem_function< Scalar >::type StemFunction
The matrix class, also used for vectors and row-vectors.
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
void run(Expr &expr, Dev &dev)
MatrixType matrix_function_solve_triangular_sylvester(const MatrixType &A, const MatrixType &B, const MatrixType &C)
Solve a triangular Sylvester equation AX + XB = C.
const ComplexMatrixType & matrixU() const
Returns the unitary matrix in the Schur decomposition.
void swap(mpfr::mpreal &x, mpfr::mpreal &y)
void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)