38 EIGEN_STRONG_INLINE
void fill(
const Scalar v) { derived().setConstant(v); }
41 EIGEN_STRONG_INLINE
void assign(
const Scalar v) { derived().setConstant(v); }
43 EIGEN_STRONG_INLINE
void assign(
size_t N,
const Scalar v) { derived().resize(N); derived().setConstant(v); }
46 EIGEN_STRONG_INLINE
size_t getRowCount()
const {
return rows(); }
48 EIGEN_STRONG_INLINE
size_t getColCount()
const {
return cols(); }
51 EIGEN_STRONG_INLINE
void unit(
const size_t nRows,
const Scalar diag_vals) {
53 derived().setIdentity(nRows,nRows);
55 derived().setZero(nRows,nRows);
56 derived().diagonal().setConstant(diag_vals);
61 EIGEN_STRONG_INLINE
void unit() { derived().setIdentity(); }
63 EIGEN_STRONG_INLINE
void eye() { derived().setIdentity(); }
66 EIGEN_STRONG_INLINE
void zeros() { derived().setZero(); }
68 EIGEN_STRONG_INLINE
void zeros(
const size_t row,
const size_t col) { derived().setZero(row,col); }
71 EIGEN_STRONG_INLINE
void ones(
const size_t row,
const size_t col) { derived().setOnes(row,col); }
73 EIGEN_STRONG_INLINE
void ones() { derived().setOnes(); }
78 EIGEN_STRONG_INLINE Scalar *
get_unsafe_row(
size_t row) {
return &derived().coeffRef(row,0); }
79 EIGEN_STRONG_INLINE
const Scalar*
get_unsafe_row(
size_t row)
const {
return &derived().coeff(row,0); }
82 EIGEN_STRONG_INLINE Scalar
get_unsafe(
const size_t row,
const size_t col)
const {
84 return derived()(row,col);
86 return derived().coeff(row,col);
90 EIGEN_STRONG_INLINE Scalar&
get_unsafe(
const size_t row,
const size_t col) {
92 return derived()(row,col);
94 return derived().coeffRef(row,col);
97 EIGEN_STRONG_INLINE
void set_unsafe(
const size_t row,
const size_t col,
const Scalar val) {
99 derived()(row,col) = val;
101 derived().coeffRef(row,col) = val;
107 const Index N =
size();
108 derived().conservativeResize(N+1);
112 EIGEN_STRONG_INLINE
bool isSquare()
const {
return cols()==rows(); }
113 EIGEN_STRONG_INLINE
bool isSingular(
const Scalar absThreshold = 0)
const {
return std::abs(derived().determinant())<absThreshold; }
133 std::string
inMatlabFormat(
const size_t decimal_digits = 6)
const;
145 const std::string &file,
147 bool appendMRPTHeader =
false,
148 const std::string &userHeader = std::string()
166 EIGEN_STRONG_INLINE
void swapCols(
size_t i1,
size_t i2) { derived().col(i1).swap( derived().col(i2) ); }
167 EIGEN_STRONG_INLINE
void swapRows(
size_t i1,
size_t i2) { derived().row(i1).swap( derived().row(i2) ); }
169 EIGEN_STRONG_INLINE
size_t countNonZero()
const {
return ((*static_cast<const Derived*>(
this))!= 0).count(); }
176 if (
size()==0)
throw std::runtime_error(
"maximum: container is empty");
177 return derived().maxCoeff();
185 if (
size()==0)
throw std::runtime_error(
"minimum: container is empty");
186 return derived().minCoeff();
194 Scalar & out_max)
const
204 EIGEN_STRONG_INLINE Scalar
maximum(
size_t *maxIndex)
const
206 if (
size()==0)
throw std::runtime_error(
"maximum: container is empty");
208 const Scalar m = derived().maxCoeff(&idx);
209 if (maxIndex) *maxIndex = idx;
218 if (cols()==0 || rows()==0)
throw std::runtime_error(
"find_index_max_value: container is empty");
220 valMax = derived().maxCoeff(&idx1,&idx2);
228 EIGEN_STRONG_INLINE Scalar
minimum(
size_t *minIndex)
const
230 if (
size()==0)
throw std::runtime_error(
"minimum: container is empty");
232 const Scalar m =derived().minCoeff(&idx);
233 if (minIndex) *minIndex = idx;
244 size_t *maxIndex)
const
251 EIGEN_STRONG_INLINE Scalar
norm_inf()
const {
return lpNorm<Eigen::Infinity>(); }
254 EIGEN_STRONG_INLINE Scalar
squareNorm()
const {
return squaredNorm(); }
257 EIGEN_STRONG_INLINE Scalar
sumAll()
const {
return derived().sum(); }
262 template<
typename OtherDerived>
263 EIGEN_STRONG_INLINE
void laplacian(Eigen::MatrixBase <OtherDerived>& ret)
const
265 if (rows()!=cols())
throw std::runtime_error(
"laplacian: Defined for square matrixes only");
266 const Index N = rows();
268 for (Index i=0;i<N;i++)
271 for (Index j=0;j<N;j++) deg+= derived().coeff(j,i);
272 ret.coeffRef(i,i)+=deg;
280 EIGEN_STRONG_INLINE
void setSize(
size_t row,
size_t col)
283 if ((Derived::RowsAtCompileTime!=Eigen::Dynamic && Derived::RowsAtCompileTime!=
int(row)) || (Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=
int(col))) {
284 std::stringstream ss; ss <<
"setSize: Trying to change a fixed sized matrix from " << rows() <<
"x" << cols() <<
" to " << row <<
"x" << col;
285 throw std::runtime_error(ss.str());
288 const Index oldCols = cols();
289 const Index oldRows = rows();
290 const int nNewCols = int(col) - int(cols());
291 const int nNewRows = int(row) - int(rows());
293 if (nNewCols>0) derived().block(0,oldCols,row,nNewCols).setZero();
294 if (nNewRows>0) derived().block(oldRows,0,nNewRows,col).setZero();
298 template <
class OUTVECT>
301 Scalar resolution = Scalar(0.01),
302 size_t maxIterations = 6,
303 int *out_Iterations = NULL,
304 float *out_estimatedResolution = NULL )
const
308 const Index n = rows();
314 Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,1> xx = (*this) * x;
315 xx *= Scalar(1.0/xx.norm());
316 dif = (x-xx).array().abs().sum();
319 }
while (iter<maxIterations && dif>resolution);
320 if (out_Iterations) *out_Iterations=static_cast<int>(iter);
321 if (out_estimatedResolution) *out_estimatedResolution=dif;
328 derived().setIdentity();
330 for (
unsigned int i=1;i<pow;i++)
331 derived() *= derived();
336 EIGEN_STRONG_INLINE
void scalarPow(
const Scalar s) { (*this)=array().pow(s); }
341 for (Index c=0;c<cols();c++)
342 for (Index r=0;r<rows();r++)
343 if (r!=c && coeff(r,c)!=0)
353 EIGEN_STRONG_INLINE
double mean()
const
355 if (
size()==0)
throw std::runtime_error(
"mean: Empty container.");
356 return derived().sum()/static_cast<double>(
size());
367 const bool unbiased_variance =
true )
const
369 const size_t N = rows();
370 if (N==0)
throw std::runtime_error(
"meanAndStd: Empty container.");
371 const double N_inv = 1.0/N;
372 const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
373 outMeanVector.resize(cols());
374 outStdVector.resize(cols());
375 for (Index i=0;i<cols();i++)
377 outMeanVector[i]= this->col(i).array().sum() * N_inv;
378 outStdVector[i] = std::sqrt( (this->col(i).array()-outMeanVector[i]).
square().
sum() * N_ );
389 const bool unbiased_variance =
true )
const
391 const size_t N =
size();
392 if (N==0)
throw std::runtime_error(
"meanAndStdAll: Empty container.");
393 const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
394 outMean = derived().array().sum()/static_cast<double>(
size());
395 outStd = std::sqrt( (this->array() - outMean).
square().
sum()*N_);
399 template <
typename MAT>
400 EIGEN_STRONG_INLINE
void insertMatrix(
size_t r,
size_t c,
const MAT &m) { derived().block(r,c,m.rows(),m.cols())=m; }
402 template <
typename MAT>
403 EIGEN_STRONG_INLINE
void insertMatrixTranspose(
size_t r,
size_t c,
const MAT &m) { derived().block(r,c,m.cols(),m.rows())=m.adjoint(); }
405 template <
typename MAT> EIGEN_STRONG_INLINE
void insertRow(
size_t nRow,
const MAT & aRow) { this->row(nRow) = aRow; }
406 template <
typename MAT> EIGEN_STRONG_INLINE
void insertCol(
size_t nCol,
const MAT & aCol) { this->col(nCol) = aCol; }
408 template <
typename R>
void insertRow(
size_t nRow,
const std::vector<R> & aRow)
410 if (static_cast<Index>(aRow.size())!=cols())
throw std::runtime_error(
"insertRow: Row size doesn't fit the size of this matrix.");
411 for (Index j=0;j<cols();j++)
412 coeffRef(nRow,j) = aRow[j];
414 template <
typename R>
void insertCol(
size_t nCol,
const std::vector<R> & aCol)
416 if (static_cast<Index>(aCol.size())!=rows())
throw std::runtime_error(
"insertRow: Row size doesn't fit the size of this matrix.");
417 for (Index j=0;j<rows();j++)
418 coeffRef(j,nCol) = aCol[j];
422 EIGEN_STRONG_INLINE
void removeColumns(
const std::vector<size_t> &idxsToRemove)
424 std::vector<size_t> idxs = idxsToRemove;
425 std::sort( idxs.begin(), idxs.end() );
427 idxs.resize( itEnd - idxs.begin() );
436 for (std::vector<size_t>::const_reverse_iterator it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
438 const size_t nC = cols() - *it - k;
440 derived().block(0,*it,rows(),nC) = derived().block(0,*it+1,rows(),nC).eval();
442 derived().conservativeResize(NoChange,cols()-idxs.size());
446 EIGEN_STRONG_INLINE
void removeRows(
const std::vector<size_t> &idxsToRemove)
448 std::vector<size_t> idxs = idxsToRemove;
449 std::sort( idxs.begin(), idxs.end() );
451 idxs.resize( itEnd - idxs.begin() );
460 for (std::vector<size_t>::reverse_iterator it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
462 const size_t nR = rows() - *it - k;
464 derived().block(*it,0,nR,cols()) = derived().block(*it+1,0,nR,cols()).eval();
466 derived().conservativeResize(rows()-idxs.size(),NoChange);
470 EIGEN_STRONG_INLINE
const AdjointReturnType
t()
const {
return derived().adjoint(); }
472 EIGEN_STRONG_INLINE PlainObject
inv()
const { PlainObject outMat = derived().inverse().eval();
return outMat; }
473 template <
class MATRIX> EIGEN_STRONG_INLINE
void inv(MATRIX &outMat)
const { outMat = derived().inverse().eval(); }
474 template <
class MATRIX> EIGEN_STRONG_INLINE
void inv_fast(MATRIX &outMat)
const { outMat = derived().inverse().eval(); }
475 EIGEN_STRONG_INLINE Scalar
det()
const {
return derived().determinant(); }
486 template<
typename OTHERMATRIX> EIGEN_STRONG_INLINE
void add_Ac(
const OTHERMATRIX &m,
const Scalar c) { (*this)+=c*m; }
488 template<
typename OTHERMATRIX> EIGEN_STRONG_INLINE
void substract_Ac(
const OTHERMATRIX &m,
const Scalar c) { (*this) -= c*m; }
491 template<
typename OTHERMATRIX> EIGEN_STRONG_INLINE
void substract_At(
const OTHERMATRIX &m) { (*this) -= m.adjoint(); }
494 template<
typename OTHERMATRIX> EIGEN_STRONG_INLINE
void substract_An(
const OTHERMATRIX& m,
const size_t n) { this->noalias() -= n * m; }
497 template<
typename OTHERMATRIX> EIGEN_STRONG_INLINE
void add_AAt(
const OTHERMATRIX &A) { this->noalias() += A; this->noalias() += A.adjoint(); }
500 template<typename OTHERMATRIX> EIGEN_STRONG_INLINE
void substract_AAt(
const OTHERMATRIX &A) { this->noalias() -= A; this->noalias() -= A.adjoint(); }
503 template <
class MATRIX1,
class MATRIX2> EIGEN_STRONG_INLINE
void multiply(
const MATRIX1& A,
const MATRIX2 &B ) { (*this)= A*B; }
505 template <
class MATRIX1,
class MATRIX2>
506 EIGEN_STRONG_INLINE
void multiply_AB(
const MATRIX1& A,
const MATRIX2 &B ) {
510 template <
typename MATRIX1,
typename MATRIX2>
511 EIGEN_STRONG_INLINE
void multiply_AtB(
const MATRIX1 &A,
const MATRIX2 &B) {
512 *
this = A.adjoint() * B;
516 template<
typename OTHERVECTOR1,
typename OTHERVECTOR2>
517 EIGEN_STRONG_INLINE
void multiply_Ab(
const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,
bool accumToOutput =
false)
const {
518 if (accumToOutput) vOut.noalias() += (*this) * vIn;
519 else vOut = (*this) * vIn;
523 template<typename OTHERVECTOR1,typename OTHERVECTOR2>
524 EIGEN_STRONG_INLINE
void multiply_Atb(
const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,
bool accumToOutput =
false)
const {
525 if (accumToOutput) vOut.noalias() += this->adjoint() * vIn;
526 else vOut = this->adjoint() * vIn;
529 template <
typename MAT_C,
typename MAT_R>
530 EIGEN_STRONG_INLINE
void multiply_HCHt(
const MAT_C &C,MAT_R &R,
bool accumResultInOutput=
false) const {
531 if (accumResultInOutput)
532 R.noalias() += (*this) * C * this->adjoint();
533 else R.noalias() = (*this) * C * this->adjoint();
536 template <
typename MAT_C,
typename MAT_R>
537 EIGEN_STRONG_INLINE
void multiply_HtCH(
const MAT_C &C,MAT_R &R,
bool accumResultInOutput=
false) const {
538 if (accumResultInOutput)
539 R.noalias() += this->adjoint() * C * (*this);
540 else R.noalias() = this->adjoint() * C * (*this);
544 template <
typename MAT_C>
546 return ( (*
this) * C * this->adjoint() ).eval()(0,0);
550 template <
typename MAT_C>
552 return ( this->adjoint() * C * (*
this) ).eval()(0,0);
556 template<
typename MAT_A>
558 *
this = (A * A.adjoint()) * f;
562 template<
typename MAT_A> EIGEN_STRONG_INLINE
void multiply_AtA_scalar(
const MAT_A &A,
typename MAT_A::Scalar f) {
563 *
this = (A.adjoint() * A) * f;
567 template <
class MAT_A,
class SKEW_3VECTOR>
void multiply_A_skew3(
const MAT_A &A,
const SKEW_3VECTOR &v) {
571 template <
class SKEW_3VECTOR,
class MAT_A>
void multiply_skew3_A(
const SKEW_3VECTOR &v,
const MAT_A &A) {
576 template <
class MAT_A,
class MAT_OUT>
577 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 {
578 outResult = derived() * A.block(A_rows_offset,A_cols_offset,derived().cols(),A_col_count);
581 template <
class MAT_A,
class MAT_B,
class MAT_C>
586 template <
class MAT_A,
class MAT_B,
class MAT_C>
588 *
this = A*B*C.adjoint();
591 template <
class MAT_A,
class MAT_B,
class MAT_C>
593 *
this = A.adjoint()*B*C;
596 template <
class MAT_A,
class MAT_B>
598 *
this = A*B.adjoint();
601 template <
class MAT_A>
603 *
this = A*A.adjoint();
606 template <
class MAT_A>
608 *
this = A.adjoint()*A;
611 template <
class MAT_A,
class MAT_B>
618 template<
class MAT2,
class MAT3 >
621 Eigen::ColPivHouseholderQR<PlainObject> QR = A.colPivHouseholderQr();
622 if (!QR.isInvertible())
throw std::runtime_error(
"leftDivideSquare: Matrix A is not invertible");
623 RES = QR.inverse() * (*this);
627 template<
class MAT2,
class MAT3 >
630 Eigen::ColPivHouseholderQR<PlainObject> QR = B.colPivHouseholderQr();
631 if (!QR.isInvertible())
throw std::runtime_error(
"rightDivideSquare: Matrix B is not invertible");
632 RES = (*this) * QR.inverse();
645 template <
class MATRIX1,
class MATRIX2>
646 EIGEN_STRONG_INLINE
void eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals )
const;
653 template <
class MATRIX1,
class VECTOR1>
654 EIGEN_STRONG_INLINE
void eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals )
const;
661 template <
class VECTOR>
665 eVecs.resizeLike(*
this);
671 template <
class MATRIX1,
class MATRIX2>
677 template <
class MATRIX1,
class VECTOR1>
690 template <
class MATRIX> EIGEN_STRONG_INLINE
bool chol(MATRIX &U)
const
692 Eigen::LLT<PlainObject> Chol = derived().template selfadjointView<Eigen::Lower>().llt();
693 if (Chol.info()==Eigen::NoConvergence)
695 U = PlainObject(Chol.matrixU());
702 EIGEN_STRONG_INLINE
size_t rank(
double threshold=0)
const
704 Eigen::ColPivHouseholderQR<PlainObject> QR = this->colPivHouseholderQr();
705 if (threshold>0) QR.setThreshold(threshold);
719 if (
size()==0)
return;
720 Scalar curMin,curMax;
722 Scalar minMaxDelta = curMax - curMin;
723 if (minMaxDelta==0) minMaxDelta = 1;
724 const Scalar minMaxDelta_ = (valMax-valMin)/minMaxDelta;
725 this->array() = (this->array()-curMin)*minMaxDelta_+valMin;
735 v = derived().block(nRow,startingCol,1,cols()-startingCol);
738 template <
typename T>
inline void extractRow(
size_t nRow, std::vector<T> &v,
size_t startingCol = 0)
const {
739 const size_t N = cols()-startingCol;
741 for (
size_t i=0;i<N;i++) v[i]=(*
this)(nRow,startingCol+i);
744 template <
class VECTOR> EIGEN_STRONG_INLINE
void extractRowAsCol(
size_t nRow, VECTOR &v,
size_t startingCol = 0)
const
746 v = derived().adjoint().block(startingCol,nRow,cols()-startingCol,1);
751 template <
class VECTOR> EIGEN_STRONG_INLINE
void extractCol(
size_t nCol, VECTOR &v,
size_t startingRow = 0)
const {
752 v = derived().block(startingRow,nCol,rows()-startingRow,1);
755 template <
typename T>
inline void extractCol(
size_t nCol, std::vector<T> &v,
size_t startingRow = 0)
const {
756 const size_t N = rows()-startingRow;
758 for (
size_t i=0;i<N;i++) v[i]=(*
this)(startingRow+i,nCol);
761 template <
class MATRIX> EIGEN_STRONG_INLINE
void extractMatrix(
const size_t firstRow,
const size_t firstCol, MATRIX &m)
const
763 m = derived().block(firstRow,firstCol,m.rows(),m.cols());
765 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
767 m.resize(nRows,nCols);
768 m = derived().block(firstRow,firstCol,nRows,nCols);
772 template <
class MATRIX>
773 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
775 out.resize(row_last-row_first+1,col_last-col_first+1);
776 out = derived().block(row_first,col_first,row_last-row_first+1,col_last-col_first+1);
783 template <
class MATRIX>
785 const size_t block_size,
786 const std::vector<size_t> &block_indices,
789 if (block_size<1)
throw std::runtime_error(
"extractSubmatrixSymmetricalBlocks: block_size must be >=1");
790 if (cols()!=rows())
throw std::runtime_error(
"extractSubmatrixSymmetricalBlocks: Matrix is not square.");
792 const size_t N = block_indices.size();
793 const size_t nrows_out=N*block_size;
794 out.resize(nrows_out,nrows_out);
796 for (
size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
798 for (
size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
801 if (block_indices[dst_col_blk]*block_size + block_size-1>=
size_t(cols()))
throw std::runtime_error(
"extractSubmatrixSymmetricalBlocks: Indices out of range!");
803 out.block(dst_row_blk * block_size,dst_col_blk * block_size, block_size,block_size)
805 derived().block(block_indices[dst_row_blk] * block_size, block_indices[dst_col_blk] * block_size, block_size,block_size);
815 template <
class MATRIX>
817 const std::vector<size_t> &indices,
820 if (cols()!=rows())
throw std::runtime_error(
"extractSubmatrixSymmetrical: Matrix is not square.");
822 const size_t N = indices.size();
823 const size_t nrows_out=N;
824 out.resize(nrows_out,nrows_out);
826 for (
size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
827 for (
size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
828 out.coeffRef(dst_row_blk,dst_col_blk) = this->coeff(indices[dst_row_blk],indices[dst_col_blk]);