MRPT  2.0.1
MatrixBase_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
13 #include <mrpt/math/MatrixBase.h>
14 #include <Eigen/Eigenvalues> // EigenSolver
15 #include <cstdint>
16 #include <stdexcept>
17 #include <vector>
18 
19 namespace mrpt::math
20 {
21 template <typename Scalar, class Derived>
23  const std::vector<std::size_t>& idxs)
24 {
25  std::size_t k = 1;
26  const auto nR = mbDerived().rows();
27  for (auto it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
28  {
29  const auto nC = mbDerived().cols() - *it - k;
30  if (nC > 0)
31  mbDerived().asEigen().block(0, *it, nR, nC) =
32  mbDerived().asEigen().block(0, *it + 1, nR, nC).eval();
33  }
34  mbDerived().setSize(nR, mbDerived().cols() - idxs.size());
35 }
36 
37 template <typename Scalar, class Derived>
39  const std::vector<std::size_t>& idxsToRemove)
40 {
41  std::vector<std::size_t> idxs = idxsToRemove;
42  std::sort(idxs.begin(), idxs.end());
43  auto itEnd = std::unique(idxs.begin(), idxs.end());
44  idxs.resize(itEnd - idxs.begin());
45  unsafeRemoveColumns(idxs);
46 }
47 
48 template <typename Scalar, class Derived>
50  const std::vector<size_t>& idxs)
51 {
52  std::size_t k = 1;
53  const auto nC = mbDerived().cols();
54  for (auto it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
55  {
56  const auto nR = mbDerived().rows() - *it - k;
57  if (nR > 0)
58  mbDerived().asEigen().block(*it, 0, nR, nC) =
59  mbDerived().asEigen().block(*it + 1, 0, nR, nC).eval();
60  }
61  mbDerived().setSize(mbDerived().rows() - idxs.size(), nC);
62 }
63 
64 template <typename Scalar, class Derived>
66  const std::vector<size_t>& idxsToRemove)
67 {
68  std::vector<std::size_t> idxs = idxsToRemove;
69  std::sort(idxs.begin(), idxs.end());
70  auto itEnd = std::unique(idxs.begin(), idxs.end());
71  idxs.resize(itEnd - idxs.begin());
72  unsafeRemoveRows(idxs);
73 }
74 
75 template <typename Scalar, class Derived>
77 {
78  return mbDerived().asEigen().eval().determinant();
79 }
80 
81 namespace detail
82 {
83 // Aux func to sort by ascending eigenvalues:
84 template <typename Scalar, typename VEC1, typename MATRIX1, typename MATRIX2>
86  const VEC1& eVals, const MATRIX1& eVecs, std::vector<Scalar>& sorted_eVals,
87  MATRIX2& sorted_eVecs)
88 {
89  const int64_t N = static_cast<int64_t>(eVals.size());
90  std::vector<std::pair<Scalar, int64_t>> D;
91  D.reserve(N);
92  for (int64_t i = 0; i < N; i++) D.emplace_back(eVals[i], i);
93  std::sort(D.begin(), D.end());
94 
95  // store:
96  sorted_eVecs.resize(eVecs.rows(), eVecs.cols());
97  sorted_eVals.resize(N);
98  for (int64_t i = 0; i < N; i++)
99  {
100  sorted_eVals[i] = D[i].first;
101  sorted_eVecs.col(i) = eVecs.col(D[i].second);
102  }
103 }
104 } // namespace detail
105 
106 template <typename Scalar, class Derived>
108  Derived& eVecs, std::vector<Scalar>& eVals, bool sorted) const
109 {
110  Eigen::EigenSolver<typename Derived::eigen_t> es(mbDerived().asEigen());
111  if (es.info() != Eigen::Success) return false;
112  const auto eigenVal = es.eigenvalues().real();
113  ASSERT_EQUAL_(eigenVal.rows(), mbDerived().rows());
114  const auto N = eigenVal.rows();
115 
116  if (sorted)
117  {
119  eigenVal, es.eigenvectors().real(), eVals, eVecs);
120  }
121  else
122  {
123  eVals.resize(N);
124  eVecs = es.eigenvectors().real();
125  for (int i = 0; i < N; i++) eVals[i] = eigenVal[i];
126  }
127  return true;
128 }
129 
130 template <typename Scalar, class Derived>
132  Derived& eVecs, std::vector<Scalar>& eVals, bool sorted) const
133 {
135  mbDerived().asEigen());
136  if (es.info() != Eigen::Success) return false;
137  const auto eigenVal = es.eigenvalues().real();
138  ASSERT_EQUAL_(eigenVal.rows(), mbDerived().rows());
139  const auto N = eigenVal.rows();
140 
141  if (sorted)
142  {
144  eigenVal, es.eigenvectors().real(), eVals, eVecs);
145  }
146  else
147  {
148  eVals.resize(N);
149  eVecs = es.eigenvectors().real();
150  for (int i = 0; i < N; i++) eVals[i] = eigenVal[i];
151  }
152  return true;
153 }
154 
155 template <typename Scalar, class Derived>
157 {
158  Eigen::FullPivLU<typename Derived::eigen_t> lu(
159  mbDerived().asEigen().eval());
160  if (threshold > 0) lu.setThreshold(threshold);
161  return lu.rank();
162 }
163 
164 template <typename Scalar, class Derived>
165 bool MatrixBase<Scalar, Derived>::chol(Derived& U) const
166 {
167  Eigen::LLT<typename Derived::eigen_t> Chol =
168  mbDerived().asEigen().template selfadjointView<Eigen::Lower>().llt();
169  if (Chol.info() == Eigen::NoConvergence) return false;
170  U = typename Derived::eigen_t(Chol.matrixU());
171  return true;
172 }
173 
174 template <typename Scalar, class Derived>
176  const Derived& A, const Derived& B)
177 {
178  mbDerived().resize(A.rows(), B.cols());
179  mbDerived().asEigen() = (A.asEigen() * B.asEigen()).eval();
180 }
181 
182 template <typename Scalar, class Derived>
184 {
185  ASSERT_EQUAL_(mbDerived().cols(), mbDerived().rows());
186  const auto N = mbDerived().cols();
187  const auto I = Derived::eigen_t::Identity(N, N);
189  inv.resize(N, N);
190  inv.asEigen() = mbDerived().asEigen().lu().solve(I).eval();
191  return inv;
192 }
193 
194 template <typename Scalar, class Derived>
196 {
197  ASSERT_EQUAL_(mbDerived().cols(), mbDerived().rows());
198  const auto N = mbDerived().cols();
199  const auto I = Derived::eigen_t::Identity(N, N);
201  inv.resize(N, N);
202  inv.asEigen() = mbDerived().asEigen().llt().solve(I).eval();
203  return inv;
204 }
205 
206 template <typename Scalar, class Derived>
208 {
209  return mbDerived().asEigen().diagonal().maxCoeff();
210 }
211 
212 template <typename Scalar, class Derived>
214 {
215  return mbDerived().asEigen().diagonal().minCoeff();
216 }
217 
218 template <typename Scalar, class Derived>
220 {
221  return mbDerived().asEigen().trace();
222 }
223 
224 } // namespace mrpt::math
bool eig(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Computes the eigenvectors and eigenvalues for a square, general matrix.
double Scalar
Definition: KmUtils.h:43
void unsafeRemoveColumns(const std::vector< std::size_t > &idxs)
Removes columns of the matrix.
bool chol(Derived &U) const
Cholesky M=UT * U decomposition for symmetric matrix (upper-half of the matrix is actually ignored...
int rank(Scalar threshold=0) const
Finds the rank of the matrix via LU decomposition.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
This base provides a set of functions for maths stuff.
Derived inverse() const
Returns the inverse of a general matrix using LU.
Scalar minimumDiagonal() const
Returns the minimum value in the diagonal.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
Scalar det() const
Determinant of matrix.
Scalar maximumDiagonal() const
Returns the maximum value in the diagonal.
void matProductOf_AB(const Derived &A, const Derived &B)
this = A*B, with A & B of the same type of this.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
void removeColumns(const std::vector< std::size_t > &idxsToRemove)
Removes columns of the matrix.
void sortEigResults(const VEC1 &eVals, const MATRIX1 &eVecs, std::vector< Scalar > &sorted_eVals, MATRIX2 &sorted_eVecs)
void unsafeRemoveRows(const std::vector< std::size_t > &idxs)
Removes rows of the matrix.
Scalar trace() const
Returns the trace of the matrix (not necessarily square).
void removeRows(const std::vector< std::size_t > &idxsToRemove)
Removes rows of the matrix.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020