Main MRPT website > C++ reference
MRPT logo
eigen_plugins.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef EIGEN_PLUGIN_H
29 #define EIGEN_PLUGIN_H
30 // -------------------------------------------------------------------------
31 // Note: This file will be included within the body of Eigen::MatrixBase
32 // -------------------------------------------------------------------------
33 public:
34  /** @name MRPT plugin: Types
35  * @{ */
36  typedef Scalar value_type; //!< Type of the elements
37  // size is constant
38  enum { static_size = RowsAtCompileTime*ColsAtCompileTime };
39  /** @} */
40 
41 
42  /** @name MRPT plugin: Basic iterators. These iterators are intended for 1D matrices only, i.e. column or row vectors.
43  * @{ */
44  typedef Scalar* iterator;
45  typedef const Scalar* const_iterator;
46 
47  EIGEN_STRONG_INLINE iterator begin() { return derived().data(); }
48  EIGEN_STRONG_INLINE iterator end() { return &(derived().data()[size()-1]); }
49  EIGEN_STRONG_INLINE const_iterator begin() const { return derived().data(); }
50  EIGEN_STRONG_INLINE const_iterator end() const { return &(derived().data()[size()-1]); }
51 
52  /** @} */
53 
54 
55  /** @name MRPT plugin: Set/get/load/save and other miscelaneous methods
56  * @{ */
57 
58  /*! Fill all the elements with a given value */
59  EIGEN_STRONG_INLINE void fill(const Scalar v) { derived().setConstant(v); }
60 
61  /*! Fill all the elements with a given value */
62  EIGEN_STRONG_INLINE void assign(const Scalar v) { derived().setConstant(v); }
63  /*! Resize to N and set all the elements to a given value */
64  EIGEN_STRONG_INLINE void assign(size_t N, const Scalar v) { derived().resize(N); derived().setConstant(v); }
65 
66  /** Get number of rows */
67  EIGEN_STRONG_INLINE size_t getRowCount() const { return rows(); }
68  /** Get number of columns */
69  EIGEN_STRONG_INLINE size_t getColCount() const { return cols(); }
70 
71  /** Make the matrix an identity matrix (the diagonal values can be 1.0 or any other value) */
72  EIGEN_STRONG_INLINE void unit(const size_t nRows, const Scalar diag_vals) {
73  if (diag_vals==1)
74  derived().setIdentity(nRows,nRows);
75  else {
76  derived().setZero(nRows,nRows);
77  derived().diagonal().setConstant(diag_vals);
78  }
79  }
80 
81  /** Make the matrix an identity matrix */
82  EIGEN_STRONG_INLINE void unit() { derived().setIdentity(); }
83  /** Make the matrix an identity matrix */
84  EIGEN_STRONG_INLINE void eye() { derived().setIdentity(); }
85 
86  /** Set all elements to zero */
87  EIGEN_STRONG_INLINE void zeros() { derived().setZero(); }
88  /** Resize and set all elements to zero */
89  EIGEN_STRONG_INLINE void zeros(const size_t row,const size_t col) { derived().setZero(row,col); }
90 
91  /** Resize matrix and set all elements to one */
92  EIGEN_STRONG_INLINE void ones(const size_t row, const size_t col) { derived().setOnes(row,col); }
93  /** Set all elements to one */
94  EIGEN_STRONG_INLINE void ones() { derived().setOnes(); }
95 
96  /** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical applications)
97  * VERY IMPORTANT WARNING: You must be aware of the memory layout, either Column or Row-major ordering.
98  */
99  EIGEN_STRONG_INLINE Scalar * get_unsafe_row(size_t row) { return &derived().coeffRef(row,0); }
100  EIGEN_STRONG_INLINE const Scalar* get_unsafe_row(size_t row) const { return &derived().coeff(row,0); }
101 
102  /** Read-only access to one element (Use with caution, bounds are not checked!) */
103  EIGEN_STRONG_INLINE Scalar get_unsafe(const size_t row, const size_t col) const {
104 #ifdef _DEBUG
105  return derived()(row,col);
106 #else
107  return derived().coeff(row,col);
108 #endif
109  }
110  /** Reference access to one element (Use with caution, bounds are not checked!) */
111  EIGEN_STRONG_INLINE Scalar& get_unsafe(const size_t row, const size_t col) {
112 #ifdef _DEBUG
113  return derived()(row,col);
114 #endif
115  return derived().coeffRef(row,col);
116  }
117  /** Sets an element (Use with caution, bounds are not checked!) */
118  EIGEN_STRONG_INLINE void set_unsafe(const size_t row, const size_t col, const Scalar val) {
119 #ifdef _DEBUG
120  derived()(row,col) = val;
121 #endif
122  derived().coeffRef(row,col) = val;
123  }
124 
125  /** Insert an element at the end of the container (for 1D vectors/arrays) */
126  EIGEN_STRONG_INLINE void push_back(Scalar val)
127  {
128  const Index N = size();
129  derived().conservativeResize(N+1);
130  coeffRef(N) = val;
131  }
132 
133  EIGEN_STRONG_INLINE bool isSquare() const { return cols()==rows(); }
134  EIGEN_STRONG_INLINE bool isSingular(const Scalar absThreshold = 0) const { return std::abs(derived().determinant())<absThreshold; }
135 
136  /** Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]"
137  * The string must start with '[' and end with ']'. Rows are separated by semicolons ';' and
138  * columns in each row by one or more whitespaces ' ', commas ',' or tabs '\t'.
139  *
140  * This format is also used for CConfigFile::read_matrix.
141  *
142  * This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
143  *
144  * \return true on success. false if the string is malformed, and then the matrix will be resized to 0x0.
145  * \sa inMatlabFormat, CConfigFile::read_matrix
146  */
147  bool fromMatlabStringFormat(const std::string &s, bool dumpErrorMsgToStdErr = true);
148  // Method implemented in eigen_plugins_impl.h
149 
150  /** Dump matrix in matlab format.
151  * This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
152  * \sa fromMatlabStringFormat
153  */
154  std::string inMatlabFormat(const size_t decimal_digits = 6) const;
155  // Method implemented in eigen_plugins_impl.h
156 
157  /** Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classes themselves).
158  * \param theMatrix It can be a CMatrixTemplate or a CMatrixFixedNumeric.
159  * \param file The target filename.
160  * \param fileFormat See TMatrixTextFileFormat. The format of the numbers in the text file.
161  * \param appendMRPTHeader Insert this header to the file "% File generated by MRPT. Load with MATLAB with: VAR=load(FILENAME);"
162  * \param userHeader Additional text to be written at the head of the file. Typically MALAB comments "% This file blah blah". Final end-of-line is not needed.
163  * \sa loadFromTextFile, CMatrixTemplate::inMatlabFormat, SAVE_MATRIX
164  */
165  void saveToTextFile(
166  const std::string &file,
167  mrpt::math::TMatrixTextFileFormat fileFormat = mrpt::math::MATRIX_FORMAT_ENG,
168  bool appendMRPTHeader = false,
169  const std::string &userHeader = std::string()
170  ) const;
171  // Method implemented in eigen_plugins_impl.h
172 
173  /** Load matrix from a text file, compatible with MATLAB text format.
174  * Lines starting with '%' or '#' are interpreted as comments and ignored.
175  * \sa saveToTextFile, fromMatlabStringFormat
176  */
177  void loadFromTextFile(const std::string &file);
178  // Method implemented in eigen_plugins_impl.h
179 
180  //! \overload
181  void loadFromTextFile(std::istream &_input_text_stream);
182  // Method implemented in eigen_plugins_impl.h
183 
184  EIGEN_STRONG_INLINE void multiplyColumnByScalar(size_t c, Scalar s) { derived().col(c)*=s; }
185  EIGEN_STRONG_INLINE void multiplyRowByScalar(size_t r, Scalar s) { derived().row(r)*=s; }
186 
187  EIGEN_STRONG_INLINE void swapCols(size_t i1,size_t i2) { derived().col(i1).swap( derived().col(i2) ); }
188  EIGEN_STRONG_INLINE void swapRows(size_t i1,size_t i2) { derived().row(i1).swap( derived().row(i2) ); }
189 
190  EIGEN_STRONG_INLINE size_t countNonZero() const { return ((*static_cast<const Derived*>(this))!= 0).count(); }
191 
192  /** [VECTORS OR MATRICES] Finds the maximum value
193  * \exception std::exception On an empty input container
194  */
195  EIGEN_STRONG_INLINE Scalar maximum() const
196  {
197  if (size()==0) throw std::runtime_error("maximum: container is empty");
198  return derived().maxCoeff();
199  }
200 
201  /** [VECTORS OR MATRICES] Finds the minimum value
202  * \sa maximum, minimum_maximum
203  * \exception std::exception On an empty input container */
204  EIGEN_STRONG_INLINE Scalar minimum() const
205  {
206  if (size()==0) throw std::runtime_error("minimum: container is empty");
207  return derived().minCoeff();
208  }
209 
210  /** [VECTORS OR MATRICES] Compute the minimum and maximum of a container at once
211  * \sa maximum, minimum
212  * \exception std::exception On an empty input container */
213  EIGEN_STRONG_INLINE void minimum_maximum(
214  Scalar & out_min,
215  Scalar & out_max) const
216  {
217  out_min = minimum();
218  out_max = maximum();
219  }
220 
221 
222  /** [VECTORS ONLY] Finds the maximum value (and the corresponding zero-based index) from a given container.
223  * \exception std::exception On an empty input vector
224  */
225  EIGEN_STRONG_INLINE Scalar maximum(size_t *maxIndex) const
226  {
227  if (size()==0) throw std::runtime_error("maximum: container is empty");
228  Index idx;
229  const Scalar m = derived().maxCoeff(&idx);
230  if (maxIndex) *maxIndex = idx;
231  return m;
232  }
233 
234  /** [VECTORS OR MATRICES] Finds the maximum value (and the corresponding zero-based index) from a given container.
235  * \exception std::exception On an empty input vector
236  */
237  void find_index_max_value(size_t &u,size_t &v,Scalar &valMax) const
238  {
239  if (cols()==0 || rows()==0) throw std::runtime_error("find_index_max_value: container is empty");
240  Index idx1,idx2;
241  valMax = derived().maxCoeff(&idx1,&idx2);
242  u = idx1; v = idx2;
243  }
244 
245 
246  /** [VECTORS ONLY] Finds the minimum value (and the corresponding zero-based index) from a given container.
247  * \sa maximum, minimum_maximum
248  * \exception std::exception On an empty input vector */
249  EIGEN_STRONG_INLINE Scalar minimum(size_t *minIndex) const
250  {
251  if (size()==0) throw std::runtime_error("minimum: container is empty");
252  Index idx;
253  const Scalar m =derived().minCoeff(&idx);
254  if (minIndex) *minIndex = idx;
255  return m;
256  }
257 
258  /** [VECTORS ONLY] Compute the minimum and maximum of a container at once
259  * \sa maximum, minimum
260  * \exception std::exception On an empty input vector */
261  EIGEN_STRONG_INLINE void minimum_maximum(
262  Scalar & out_min,
263  Scalar & out_max,
264  size_t *minIndex,
265  size_t *maxIndex) const
266  {
267  out_min = minimum(minIndex);
268  out_max = maximum(maxIndex);
269  }
270 
271  /** Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements. */
272  EIGEN_STRONG_INLINE Scalar norm_inf() const { return lpNorm<Eigen::Infinity>(); }
273 
274  /** Compute the square norm of a vector/array/matrix (the Euclidean distance to the origin, taking all the elements as a single vector). \sa norm */
275  EIGEN_STRONG_INLINE Scalar squareNorm() const { return squaredNorm(); }
276 
277  /*! Sum all the elements, returning a value of the same type than the container */
278  EIGEN_STRONG_INLINE Scalar sumAll() const { return derived().sum(); }
279 
280  /** Computes the laplacian of this square graph weight matrix.
281  * The laplacian matrix is L = D - W, with D a diagonal matrix with the degree of each node, W the
282  */
283  template<typename OtherDerived>
284  EIGEN_STRONG_INLINE void laplacian(Eigen::MatrixBase <OtherDerived>& ret) const
285  {
286  if (rows()!=cols()) throw std::runtime_error("laplacian: Defined for square matrixes only");
287  const Index N = rows();
288  ret = -(*this);
289  for (Index i=0;i<N;i++)
290  {
291  Scalar deg = 0;
292  for (Index j=0;j<N;j++) deg+= derived().coeff(j,i);
293  ret.coeffRef(i,i)+=deg;
294  }
295  }
296 
297 
298  /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable.
299  * **WARNING**: MRPT's add-on method \a setSize() pads with zeros, while Eigen's \a resize() does NOT (new elements are undefined).
300  */
301  EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
302  {
303 #ifdef _DEBUG
304  if ((Derived::RowsAtCompileTime!=Eigen::Dynamic && Derived::RowsAtCompileTime!=int(row)) || (Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(col))) {
305  std::stringstream ss; ss << "setSize: Trying to change a fixed sized matrix from " << rows() << "x" << cols() << " to " << row << "x" << col;
306  throw std::runtime_error(ss.str());
307  }
308 #endif
309  const Index oldCols = cols();
310  const Index oldRows = rows();
311  const int nNewCols = int(col) - int(cols());
312  const int nNewRows = int(row) - int(rows());
313  ::mrpt::math::detail::TAuxResizer<Eigen::MatrixBase<Derived>,SizeAtCompileTime>::internal_resize(*this,row,col);
314  if (nNewCols>0) derived().block(0,oldCols,row,nNewCols).setZero();
315  if (nNewRows>0) derived().block(oldRows,0,nNewRows,col).setZero();
316  }
317 
318  /** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it in the passed vector "x". */
319  template <class OUTVECT>
320  void largestEigenvector(
321  OUTVECT &x,
322  Scalar resolution = Scalar(0.01),
323  size_t maxIterations = 6,
324  int *out_Iterations = NULL,
325  float *out_estimatedResolution = NULL ) const
326  {
327  // Apply the iterative Power Method:
328  size_t iter=0;
329  const Index n = rows();
330  x.resize(n);
331  x.setConstant(1); // Initially, set to all ones, for example...
332  Scalar dif;
333  do // Iterative loop:
334  {
335  Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,1> xx = (*this) * x;
336  xx *= Scalar(1.0/xx.norm());
337  dif = (x-xx).array().abs().sum(); // Compute diference between iterations:
338  x = xx; // Set as current estimation:
339  iter++; // Iteration counter:
340  } while (iter<maxIterations && dif>resolution);
341  if (out_Iterations) *out_Iterations=static_cast<int>(iter);
342  if (out_estimatedResolution) *out_estimatedResolution=dif;
343  }
344 
345  /** Combined matrix power and assignment operator */
346  MatrixBase<Derived>& operator ^= (const unsigned int pow)
347  {
348  if (pow==0)
349  derived().setIdentity();
350  else
351  for (unsigned int i=1;i<pow;i++)
352  derived() *= derived();
353  return *this;
354  }
355 
356  /** Scalar power of all elements to a given power, this is diferent of ^ operator. */
357  EIGEN_STRONG_INLINE void scalarPow(const Scalar s) { (*this)=array().pow(s); }
358 
359  /** Checks for matrix type */
360  EIGEN_STRONG_INLINE bool isDiagonal() const
361  {
362  for (Index c=0;c<cols();c++)
363  for (Index r=0;r<rows();r++)
364  if (r!=c && coeff(r,c)!=0)
365  return false;
366  return true;
367  }
368 
369  /** Finds the maximum value in the diagonal of the matrix. */
370  EIGEN_STRONG_INLINE Scalar maximumDiagonal() const { return diagonal().maximum(); }
371 
372  /** Computes the mean of the entire matrix
373  * \sa meanAndStdAll */
374  EIGEN_STRONG_INLINE double mean() const
375  {
376  if ( size()==0) throw std::runtime_error("mean: Empty container.");
377  return derived().sum()/static_cast<double>(size());
378  }
379 
380  /** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
381  * \sa mean,meanAndStdAll \exception std::exception If the matrix/vector is empty.
382  * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
383  */
384  template <class VEC>
385  void meanAndStd(
386  VEC &outMeanVector,
387  VEC &outStdVector,
388  const bool unbiased_variance = true ) const
389  {
390  const double N = rows();
391  if (N==0) throw std::runtime_error("meanAndStd: Empty container.");
392  const double N_inv = 1.0/N;
393  const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
394  outMeanVector.resize(cols());
395  outStdVector.resize(cols());
396  for (Index i=0;i<cols();i++)
397  {
398  outMeanVector[i]= this->col(i).array().sum() * N_inv;
399  outStdVector[i] = std::sqrt( (this->col(i).array()-outMeanVector[i]).square().sum() * N_ );
400  }
401  }
402 
403  /** Computes the mean and standard deviation of all the elements in the matrix as a whole.
404  * \sa mean,meanAndStd \exception std::exception If the matrix/vector is empty.
405  * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
406  */
407  void meanAndStdAll(
408  double &outMean,
409  double &outStd,
410  const bool unbiased_variance = true ) const
411  {
412  const double N = size();
413  if (N==0) throw std::runtime_error("meanAndStdAll: Empty container.");
414  const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
415  outMean = derived().array().sum()/static_cast<double>(size());
416  outStd = std::sqrt( (this->array() - outMean).square().sum()*N_);
417  }
418 
419  /** Insert matrix "m" into this matrix at indices (r,c), that is, (*this)(r,c)=m(0,0) and so on */
420  template <typename MAT>
421  EIGEN_STRONG_INLINE void insertMatrix(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.rows(),m.cols())=m; }
422 
423  template <typename MAT>
424  EIGEN_STRONG_INLINE void insertMatrixTranspose(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.cols(),m.rows())=m.adjoint(); }
425 
426  template <typename MAT> EIGEN_STRONG_INLINE void insertRow(size_t nRow, const MAT & aRow) { this->row(nRow) = aRow; }
427  template <typename MAT> EIGEN_STRONG_INLINE void insertCol(size_t nCol, const MAT & aCol) { this->col(nCol) = aCol; }
428 
429  template <typename R> void insertRow(size_t nRow, const std::vector<R> & aRow)
430  {
431  if (static_cast<Index>(aRow.size())!=cols()) throw std::runtime_error("insertRow: Row size doesn't fit the size of this matrix.");
432  for (Index j=0;j<cols();j++)
433  coeffRef(nRow,j) = aRow[j];
434  }
435  template <typename R> void insertCol(size_t nCol, const std::vector<R> & aCol)
436  {
437  if (static_cast<Index>(aCol.size())!=rows()) throw std::runtime_error("insertRow: Row size doesn't fit the size of this matrix.");
438  for (Index j=0;j<rows();j++)
439  coeffRef(j,nCol) = aCol[j];
440  }
441 
442  /** Remove columns of the matrix.*/
443  EIGEN_STRONG_INLINE void removeColumns(const std::vector<size_t> &idxsToRemove)
444  {
445  std::vector<size_t> idxs = idxsToRemove;
446  std::sort( idxs.begin(), idxs.end() );
447  std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
448  idxs.resize( itEnd - idxs.begin() );
449 
450  unsafeRemoveColumns( idxs );
451  }
452 
453  /** Remove columns of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
454  EIGEN_STRONG_INLINE void unsafeRemoveColumns(const std::vector<size_t> &idxs)
455  {
456  size_t k = 1;
457  for (std::vector<size_t>::const_reverse_iterator it = idxs.rbegin(); it != idxs.rend(); it++, k++)
458  {
459  const size_t nC = cols() - *it - k;
460  if( nC > 0 )
461  derived().block(0,*it,rows(),nC) = derived().block(0,*it+1,rows(),nC).eval();
462  }
463  derived().conservativeResize(NoChange,cols()-idxs.size());
464  }
465 
466  /** Remove rows of the matrix. */
467  EIGEN_STRONG_INLINE void removeRows(const std::vector<size_t> &idxsToRemove)
468  {
469  std::vector<size_t> idxs = idxsToRemove;
470  std::sort( idxs.begin(), idxs.end() );
471  std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
472  idxs.resize( itEnd - idxs.begin() );
473 
474  unsafeRemoveRows( idxs );
475  }
476 
477  /** Remove rows of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
478  EIGEN_STRONG_INLINE void unsafeRemoveRows(const std::vector<size_t> &idxs)
479  {
480  size_t k = 1;
481  for (std::vector<size_t>::reverse_iterator it = idxs.rbegin(); it != idxs.rend(); it++, k++)
482  {
483  const size_t nR = rows() - *it - k;
484  if( nR > 0 )
485  derived().block(*it,0,nR,cols()) = derived().block(*it+1,0,nR,cols()).eval();
486  }
487  derived().conservativeResize(rows()-idxs.size(),NoChange);
488  }
489 
490  /** Transpose */
491  EIGEN_STRONG_INLINE const AdjointReturnType t() const { return derived().adjoint(); }
492 
493  EIGEN_STRONG_INLINE PlainObject inv() const { PlainObject outMat = derived().inverse().eval(); return outMat; }
494  template <class MATRIX> EIGEN_STRONG_INLINE void inv(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
495  template <class MATRIX> EIGEN_STRONG_INLINE void inv_fast(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
496  EIGEN_STRONG_INLINE Scalar det() const { return derived().determinant(); }
497 
498  /** @} */ // end miscelaneous
499 
500 
501  /** @name MRPT plugin: Multiply and extra addition functions
502  @{ */
503 
504  EIGEN_STRONG_INLINE bool empty() const { return this->getColCount()==0 || this->getRowCount()==0; }
505 
506  /*! Add c (scalar) times A to this matrix: this += A * c */
507  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_Ac(const OTHERMATRIX &m,const Scalar c) { (*this)+=c*m; }
508  /*! Substract c (scalar) times A to this matrix: this -= A * c */
509  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_Ac(const OTHERMATRIX &m,const Scalar c) { (*this) -= c*m; }
510 
511  /*! Substract A transposed to this matrix: this -= A.adjoint() */
512  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_At(const OTHERMATRIX &m) { (*this) -= m.adjoint(); }
513 
514  /*! Substract n (integer) times A to this matrix: this -= A * n */
515  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_An(const OTHERMATRIX& m, const size_t n) { this->noalias() -= n * m; }
516 
517  /*! this += A + A<sup>T</sup> */
518  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_AAt(const OTHERMATRIX &A) { this->noalias() += A; this->noalias() += A.adjoint(); }
519 
520  /*! this -= A + A<sup>T</sup> */ \
521  template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_AAt(const OTHERMATRIX &A) { this->noalias() -= A; this->noalias() -= A.adjoint(); }
522 
523 
524  template <class MATRIX1,class MATRIX2> EIGEN_STRONG_INLINE void multiply( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { (*this)= A*B; }
525 
526  template <class MATRIX1,class MATRIX2>
527  EIGEN_STRONG_INLINE void multiply_AB( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ {
528  (*this)= A*B;
529  }
530 
531  template <typename MATRIX1,typename MATRIX2>
532  EIGEN_STRONG_INLINE void multiply_AtB(const MATRIX1 &A,const MATRIX2 &B) /*!< this=A^t * B */ {
533  *this = A.adjoint() * B;
534  }
535 
536  /*! Computes the vector vOut = this * vIn, where "vIn" is a column vector of the appropriate length. */
537  template<typename OTHERVECTOR1,typename OTHERVECTOR2>
538  EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
539  if (accumToOutput) vOut.noalias() += (*this) * vIn;
540  else vOut = (*this) * vIn;
541  }
542 
543  /*! Computes the vector vOut = this<sup>T</sup> * vIn, where "vIn" is a column vector of the appropriate length. */ \
544  template<typename OTHERVECTOR1,typename OTHERVECTOR2>
545  EIGEN_STRONG_INLINE void multiply_Atb(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
546  if (accumToOutput) vOut.noalias() += this->adjoint() * vIn;
547  else vOut = this->adjoint() * vIn;
548  }
549 
550  template <typename MAT_C, typename MAT_R>
551  EIGEN_STRONG_INLINE void multiply_HCHt(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this * C * this<sup>T</sup> */ {
552  if (accumResultInOutput)
553  R.noalias() += (*this) * C * this->adjoint();
554  else R.noalias() = (*this) * C * this->adjoint();
555  }
556 
557  template <typename MAT_C, typename MAT_R>
558  EIGEN_STRONG_INLINE void multiply_HtCH(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this<sup>T</sup> * C * this */ {
559  if (accumResultInOutput)
560  R.noalias() += this->adjoint() * C * (*this);
561  else R.noalias() = this->adjoint() * C * (*this);
562  }
563 
564  /*! R = H * C * H<sup>T</sup> (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */
565  template <typename MAT_C>
566  EIGEN_STRONG_INLINE Scalar multiply_HCHt_scalar(const MAT_C &C) const {
567  return ( (*this) * C * this->adjoint() ).eval()(0,0);
568  }
569 
570  /*! R = H<sup>T</sup> * C * H (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */
571  template <typename MAT_C>
572  EIGEN_STRONG_INLINE Scalar multiply_HtCH_scalar(const MAT_C &C) const {
573  return ( this->adjoint() * C * (*this) ).eval()(0,0);
574  }
575 
576  /*! this = C * C<sup>T</sup> * f (with a matrix C and a scalar f). */
577  template<typename MAT_A>
578  EIGEN_STRONG_INLINE void multiply_AAt_scalar(const MAT_A &A,typename MAT_A::value_type f) {
579  *this = (A * A.adjoint()) * f;
580  }
581 
582  /*! this = C<sup>T</sup> * C * f (with a matrix C and a scalar f). */
583  template<typename MAT_A> EIGEN_STRONG_INLINE void multiply_AtA_scalar(const MAT_A &A,typename MAT_A::value_type f) {
584  *this = (A.adjoint() * A) * f;
585  }
586 
587  /*! this = A * skew(v), with \a v being a 3-vector (or 3-array) and skew(v) the skew symmetric matrix of v (see mrpt::math::skew_symmetric3) */
588  template <class MAT_A,class SKEW_3VECTOR> void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v) {
589  mrpt::math::multiply_A_skew3(A,v,*this); }
590 
591  /*! this = skew(v)*A, with \a v being a 3-vector (or 3-array) and skew(v) the skew symmetric matrix of v (see mrpt::math::skew_symmetric3) */
592  template <class SKEW_3VECTOR,class MAT_A> void multiply_skew3_A(const SKEW_3VECTOR &v,const MAT_A &A) {
593  mrpt::math::multiply_skew3_A(v,A,*this); }
594 
595  /** outResult = this * A
596  */
597  template <class MAT_A,class MAT_OUT>
598  EIGEN_STRONG_INLINE void multiply_subMatrix(const MAT_A &A,MAT_OUT &outResult,const size_t A_cols_offset,const size_t A_rows_offset,const size_t A_col_count) const {
599  outResult = derived() * A.block(A_rows_offset,A_cols_offset,derived().cols(),A_col_count);
600  }
601 
602  template <class MAT_A,class MAT_B,class MAT_C>
603  void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*C */ {
604  *this = A*B*C;
605  }
606 
607  template <class MAT_A,class MAT_B,class MAT_C>
608  void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*(C<sup>T</sup>) */ {
609  *this = A*B*C.adjoint();
610  }
611 
612  template <class MAT_A,class MAT_B,class MAT_C>
613  void multiply_AtBC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A(<sup>T</sup>)*B*C */ {
614  *this = A.adjoint()*B*C;
615  }
616 
617  template <class MAT_A,class MAT_B>
618  EIGEN_STRONG_INLINE void multiply_ABt(const MAT_A &A,const MAT_B &B) /*!< this = A * B<sup>T</sup> */ {
619  *this = A*B.adjoint();
620  }
621 
622  template <class MAT_A>
623  EIGEN_STRONG_INLINE void multiply_AAt(const MAT_A &A) /*!< this = A * A<sup>T</sup> */ {
624  *this = A*A.adjoint();
625  }
626 
627  template <class MAT_A>
628  EIGEN_STRONG_INLINE void multiply_AtA(const MAT_A &A) /*!< this = A<sup>T</sup> * A */ {
629  *this = A.adjoint()*A;
630  }
631 
632  template <class MAT_A,class MAT_B>
633  EIGEN_STRONG_INLINE void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B) /*!< this = A * B (result is symmetric) */ {
634  *this = A*B;
635  }
636 
637 
638  /** Matrix left divide: RES = A<sup>-1</sup> * this , with A being squared (using the Eigen::ColPivHouseholderQR method) */
639  template<class MAT2,class MAT3 >
640  EIGEN_STRONG_INLINE void leftDivideSquare(const MAT2 &A, MAT3 &RES) const
641  {
642  Eigen::ColPivHouseholderQR<PlainObject> QR = A.colPivHouseholderQr();
643  if (!QR.isInvertible()) throw std::runtime_error("leftDivideSquare: Matrix A is not invertible");
644  RES = QR.inverse() * (*this);
645  }
646 
647  /** Matrix right divide: RES = this * B<sup>-1</sup>, with B being squared (using the Eigen::ColPivHouseholderQR method) */
648  template<class MAT2,class MAT3 >
649  EIGEN_STRONG_INLINE void rightDivideSquare(const MAT2 &B, MAT3 &RES) const
650  {
651  Eigen::ColPivHouseholderQR<PlainObject> QR = B.colPivHouseholderQr();
652  if (!QR.isInvertible()) throw std::runtime_error("rightDivideSquare: Matrix B is not invertible");
653  RES = (*this) * QR.inverse();
654  }
655 
656  /** @} */ // end multiply functions
657 
658 
659  /** @name MRPT plugin: Eigenvalue / Eigenvectors
660  @{ */
661 
662  /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), both returned as matrices: eigenvectors are the columns in "eVecs", and eigenvalues in ascending order as the diagonal of "eVals".
663  * \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
664  * \sa eigenVectorsSymmetric, eigenVectorsVec
665  */
666  template <class MATRIX1,class MATRIX2>
667  EIGEN_STRONG_INLINE void eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
668  // Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.
669 
670  /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), eigenvectors are the columns in "eVecs", and eigenvalues are returned in in ascending order in the vector "eVals".
671  * \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
672  * \sa eigenVectorsSymmetric, eigenVectorsVec
673  */
674  template <class MATRIX1,class VECTOR1>
675  EIGEN_STRONG_INLINE void eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
676  // Implemented in eigen_plugins_impl.h
677 
678  /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), and return only the eigenvalues in the vector "eVals".
679  * \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
680  * \sa eigenVectorsSymmetric, eigenVectorsVec
681  */
682  template <class VECTOR>
683  EIGEN_STRONG_INLINE void eigenValues( VECTOR & eVals ) const
684  {
685  PlainObject eVecs;
686  eVecs.resizeLike(*this);
687  this->eigenVectorsVec(eVecs,eVals);
688  }
689 
690  /** [For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order), both returned as matrices: eigenvectors are the columns, and eigenvalues \sa eigenVectors
691  */
692  template <class MATRIX1,class MATRIX2>
693  EIGEN_STRONG_INLINE void eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
694  // Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.
695 
696  /** [For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order), both returned as matrices: eigenvectors are the columns, and eigenvalues \sa eigenVectorsVec
697  */
698  template <class MATRIX1,class VECTOR1>
699  EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
700  // Implemented in eigen_plugins_impl.h
701 
702 
703  /** @} */ // end eigenvalues
704 
705 
706 
707  /** @name MRPT plugin: Linear algebra & decomposition-based methods
708  @{ */
709 
710  /** Cholesky M=U<sup>T</sup> * U decomposition for simetric matrix (upper-half of the matrix will be actually ignored) */
711  template <class MATRIX> EIGEN_STRONG_INLINE bool chol(MATRIX &U) const
712  {
713  Eigen::LLT<PlainObject> Chol = derived().selfadjointView<Eigen::Lower>().llt();
714  if (Chol.info()==Eigen::NoConvergence)
715  return false;
716  U = PlainObject(Chol.matrixU());
717  return true;
718  }
719 
720  /** Gets the rank of the matrix via the Eigen::ColPivHouseholderQR method
721  * \param threshold If set to >0, it's used as threshold instead of Eigen's default one.
722  */
723  EIGEN_STRONG_INLINE size_t rank(double threshold=0) const
724  {
725  Eigen::ColPivHouseholderQR<PlainObject> QR = this->colPivHouseholderQr();
726  if (threshold>0) QR.setThreshold(threshold);
727  return QR.rank();
728  }
729 
730  /** @} */ // end linear algebra
731 
732 
733 
734  /** @name MRPT plugin: Scalar and element-wise extra operators
735  @{ */
736 
737  EIGEN_STRONG_INLINE MatrixBase<Derived>& Sqrt() { (*this) = this->array().sqrt(); return *this; }
738  EIGEN_STRONG_INLINE PlainObject Sqrt() const { PlainObject res = this->array().sqrt(); return res; }
739 
740  EIGEN_STRONG_INLINE MatrixBase<Derived>& Abs() { (*this) = this->array().abs(); return *this; }
741  EIGEN_STRONG_INLINE PlainObject Abs() const { PlainObject res = this->array().abs(); return res; }
742 
743  EIGEN_STRONG_INLINE MatrixBase<Derived>& Log() { (*this) = this->array().log(); return *this; }
744  EIGEN_STRONG_INLINE PlainObject Log() const { PlainObject res = this->array().log(); return res; }
745 
746  EIGEN_STRONG_INLINE MatrixBase<Derived>& Exp() { (*this) = this->array().exp(); return *this; }
747  EIGEN_STRONG_INLINE PlainObject Exp() const { PlainObject res = this->array().exp(); return res; }
748 
749  EIGEN_STRONG_INLINE MatrixBase<Derived>& Square() { (*this) = this->array().square(); return *this; }
750  EIGEN_STRONG_INLINE PlainObject Square() const { PlainObject res = this->array().square(); return res; }
751 
752  /** Scales all elements such as the minimum & maximum values are shifted to the given values */
753  void normalize(Scalar valMin, Scalar valMax)
754  {
755  if (size()==0) return;
756  Scalar curMin,curMax;
757  minimum_maximum(curMin,curMax);
758  Scalar minMaxDelta = curMax - curMin;
759  if (minMaxDelta==0) minMaxDelta = 1;
760  const Scalar minMaxDelta_ = (valMax-valMin)/minMaxDelta;
761  this->array() = (this->array()-curMin)*minMaxDelta_+valMin;
762  }
763  //! \overload
764  inline void adjustRange(Scalar valMin, Scalar valMax) { normalize(valMin,valMax); }
765 
766  /** @} */ // end Scalar
767 
768 
769  /** Extract one row from the matrix into a row vector */
770  template <class OtherDerived> EIGEN_STRONG_INLINE void extractRow(size_t nRow, Eigen::EigenBase<OtherDerived> &v, size_t startingCol = 0) const {
771  v = derived().block(nRow,startingCol,1,cols()-startingCol);
772  }
773  //! \overload
774  template <typename T> inline void extractRow(size_t nRow, std::vector<T> &v, size_t startingCol = 0) const {
775  const size_t N = cols()-startingCol;
776  v.resize(N);
777  for (size_t i=0;i<N;i++) v[i]=(*this)(nRow,startingCol+i);
778  }
779  /** Extract one row from the matrix into a column vector */
780  template <class VECTOR> EIGEN_STRONG_INLINE void extractRowAsCol(size_t nRow, VECTOR &v, size_t startingCol = 0) const
781  {
782  v = derived().adjoint().block(startingCol,nRow,cols()-startingCol,1);
783  }
784 
785 
786  /** Extract one column from the matrix into a column vector */
787  template <class VECTOR> EIGEN_STRONG_INLINE void extractCol(size_t nCol, VECTOR &v, size_t startingRow = 0) const {
788  v = derived().block(startingRow,nCol,rows()-startingRow,1);
789  }
790  //! \overload
791  template <typename T> inline void extractCol(size_t nCol, std::vector<T> &v, size_t startingRow = 0) const {
792  const size_t N = rows()-startingRow;
793  v.resize(N);
794  for (size_t i=0;i<N;i++) v[i]=(*this)(startingRow+i,nCol);
795  }
796 
797  template <class MATRIX> EIGEN_STRONG_INLINE void extractMatrix(const size_t firstRow, const size_t firstCol, MATRIX &m) const
798  {
799  m = derived().block(firstRow,firstCol,m.rows(),m.cols());
800  }
801  template <class MATRIX> EIGEN_STRONG_INLINE void extractMatrix(const size_t firstRow, const size_t firstCol, const size_t nRows, const size_t nCols, MATRIX &m) const
802  {
803  m.resize(nRows,nCols);
804  m = derived().block(firstRow,firstCol,nRows,nCols);
805  }
806 
807  /** Get a submatrix, given its bounds: first & last column and row (inclusive). */
808  template <class MATRIX>
809  EIGEN_STRONG_INLINE void extractSubmatrix(const size_t row_first,const size_t row_last,const size_t col_first,const size_t col_last,MATRIX &out) const
810  {
811  out.resize(row_last-row_first+1,col_last-col_first+1);
812  out = derived().block(row_first,col_first,row_last-row_first+1,col_last-col_first+1);
813  }
814 
815  /** Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is a sequence {block_indices(i):block_indices(i)+block_size-1} for all "i" up to the size of block_indices.
816  * A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
817  * \sa extractSubmatrix, extractSubmatrixSymmetrical
818  */
819  template <class MATRIX>
820  void extractSubmatrixSymmetricalBlocks(
821  const size_t block_size,
822  const std::vector<size_t> &block_indices,
823  MATRIX& out) const
824  {
825  if (block_size<1) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: block_size must be >=1");
826  if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Matrix is not square.");
827 
828  const size_t N = block_indices.size();
829  const size_t nrows_out=N*block_size;
830  out.resize(nrows_out,nrows_out);
831  if (!N) return; // Done
832  for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
833  {
834  for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
835  {
836 #if defined(_DEBUG)
837  if (block_indices[dst_col_blk]*block_size + block_size-1>=size_t(cols())) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Indices out of range!");
838 #endif
839  out.block(dst_row_blk * block_size,dst_col_blk * block_size, block_size,block_size)
840  =
841  derived().block(block_indices[dst_row_blk] * block_size, block_indices[dst_col_blk] * block_size, block_size,block_size);
842  }
843  }
844  }
845 
846 
847  /** Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequence of indices passed as argument.
848  * A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
849  * \sa extractSubmatrix, extractSubmatrixSymmetricalBlocks
850  */
851  template <class MATRIX>
852  void extractSubmatrixSymmetrical(
853  const std::vector<size_t> &indices,
854  MATRIX& out) const
855  {
856  if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetrical: Matrix is not square.");
857 
858  const size_t N = indices.size();
859  const size_t nrows_out=N;
860  out.resize(nrows_out,nrows_out);
861  if (!N) return; // Done
862  for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
863  for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
864  out.coeffRef(dst_row_blk,dst_col_blk) = this->coeff(indices[dst_row_blk],indices[dst_col_blk]);
865  }
866 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013