/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

// -------------------------------------------------------------------------
// Note: This file will be included within the body of Eigen::MatrixBase
// -------------------------------------------------------------------------
public:
	/** @name MRPT plugin: Types
	  *  @{ */
	// size is constant
	enum { static_size = RowsAtCompileTime*ColsAtCompileTime };
	/** @} */


	/** @name MRPT plugin: Basic iterators. These iterators are intended for 1D matrices only, i.e. column or row vectors.
	  *  @{ */
	typedef Scalar* iterator;
	typedef const Scalar* const_iterator;

	EIGEN_STRONG_INLINE iterator begin() { return derived().data(); }
	EIGEN_STRONG_INLINE iterator end()   { return (&(derived().data()[size()-1]))+1; }
	EIGEN_STRONG_INLINE const_iterator begin() const { return derived().data(); }
	EIGEN_STRONG_INLINE const_iterator end() const   { return (&(derived().data()[size()-1]))+1; }

	/** @} */


	/** @name MRPT plugin: Set/get/load/save and other miscelaneous methods
	  *  @{ */

	/*! Fill all the elements with a given value */
	EIGEN_STRONG_INLINE void fill(const Scalar v) { derived().setConstant(v); }

	/*! Fill all the elements with a given value */
	EIGEN_STRONG_INLINE void assign(const Scalar v) { derived().setConstant(v); }
	/*! Resize to N and set all the elements to a given value */
	EIGEN_STRONG_INLINE void assign(size_t N, const Scalar v) { derived().resize(N); derived().setConstant(v); }

	/** Get number of rows */
	EIGEN_STRONG_INLINE size_t getRowCount() const { return rows(); }
	/** Get number of columns */
	EIGEN_STRONG_INLINE size_t getColCount() const { return cols(); }

	/** Make the matrix an identity matrix (the diagonal values can be 1.0 or any other value) */
	EIGEN_STRONG_INLINE void unit(const size_t nRows, const Scalar diag_vals) {
		if (diag_vals==1)
			derived().setIdentity(nRows,nRows);
		else {
			derived().setZero(nRows,nRows);
			derived().diagonal().setConstant(diag_vals);
		}
	}

	/** Make the matrix an identity matrix  */
	EIGEN_STRONG_INLINE void unit() { derived().setIdentity(); }
	/** Make the matrix an identity matrix  */
	EIGEN_STRONG_INLINE void eye() { derived().setIdentity(); }

	/** Set all elements to zero */
	EIGEN_STRONG_INLINE void zeros() { derived().setZero(); }
	/** Resize and set all elements to zero */
	EIGEN_STRONG_INLINE void zeros(const size_t row,const size_t col) { derived().setZero(row,col); }

	/** Resize matrix and set all elements to one */
	EIGEN_STRONG_INLINE void  ones(const size_t row, const size_t col) { derived().setOnes(row,col); }
	/** Set all elements to one */
	EIGEN_STRONG_INLINE void  ones() { derived().setOnes(); }

	/** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical applications)
	  * VERY IMPORTANT WARNING: You must be aware of the memory layout, either Column or Row-major ordering.
	  */
	EIGEN_STRONG_INLINE Scalar      * get_unsafe_row(size_t row)       { return &derived().coeffRef(row,0); }
	EIGEN_STRONG_INLINE const Scalar* get_unsafe_row(size_t row) const { return &derived().coeff(row,0); }

	/** Read-only access to one element (Use with caution, bounds are not checked!) */
	EIGEN_STRONG_INLINE Scalar get_unsafe(const size_t row, const size_t col) const {
#ifdef _DEBUG
		return derived()(row,col);
#else
		return derived().coeff(row,col);
#endif
	}
	/** Reference access to one element (Use with caution, bounds are not checked!) */
	EIGEN_STRONG_INLINE Scalar& get_unsafe(const size_t row, const size_t col) { //-V659
#ifdef _DEBUG
		return derived()(row,col);
#endif
		return derived().coeffRef(row,col);
	}
	/** Sets an element  (Use with caution, bounds are not checked!) */
	EIGEN_STRONG_INLINE void set_unsafe(const size_t row, const size_t col, const Scalar val) {
#ifdef _DEBUG
		derived()(row,col) = val;
#endif
		derived().coeffRef(row,col) = val;
	}

	/** Insert an element at the end of the container (for 1D vectors/arrays) */
	EIGEN_STRONG_INLINE void push_back(Scalar val)
	{
		const Index N = size();
		derived().conservativeResize(N+1);
		coeffRef(N) = val;
	}

	EIGEN_STRONG_INLINE bool isSquare() const { return cols()==rows(); }
	EIGEN_STRONG_INLINE bool isSingular(const Scalar absThreshold = 0) const { return std::abs(derived().determinant())<absThreshold; }

	/** Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]"
	  *  The string must start with '[' and end with ']'. Rows are separated by semicolons ';' and
	  *  columns in each row by one or more whitespaces ' ', commas ',' or tabs '\t'.
	  *
	  *  This format is also used for CConfigFile::read_matrix.
	  *
	  *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
	  *
	  * \return true on success. false if the string is malformed, and then the matrix will be resized to 0x0.
	  * \sa inMatlabFormat, CConfigFile::read_matrix
	  */
	bool fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here = NULL);
	// Method implemented in eigen_plugins_impl.h

	/** Dump matrix in matlab format.
	  *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
	  * \sa fromMatlabStringFormat
	  */
	std::string  inMatlabFormat(const size_t decimal_digits = 6) const;
	// Method implemented in eigen_plugins_impl.h

	/** Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classes themselves).
		* \param theMatrix It can be a CMatrixTemplate or a CMatrixFixedNumeric.
		* \param file The target filename.
		* \param fileFormat See TMatrixTextFileFormat. The format of the numbers in the text file.
		* \param appendMRPTHeader Insert this header to the file "% File generated by MRPT. Load with MATLAB with: VAR=load(FILENAME);"
		* \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.
		* \sa loadFromTextFile, CMatrixTemplate::inMatlabFormat, SAVE_MATRIX
		*/
	void saveToTextFile(
		const std::string &file,
		mrpt::math::TMatrixTextFileFormat fileFormat = mrpt::math::MATRIX_FORMAT_ENG,
		bool    appendMRPTHeader = false,
		const std::string &userHeader = std::string()
		) const;
	// Method implemented in eigen_plugins_impl.h

	/** Load matrix from a text file, compatible with MATLAB text format.
	  *  Lines starting with '%' or '#' are interpreted as comments and ignored.
	  * \sa saveToTextFile, fromMatlabStringFormat
	  */
	void  loadFromTextFile(const std::string &file);
	// Method implemented in eigen_plugins_impl.h

	//! \overload
	void  loadFromTextFile(std::istream &_input_text_stream);
	// Method implemented in eigen_plugins_impl.h

	EIGEN_STRONG_INLINE void multiplyColumnByScalar(size_t c, Scalar s) { derived().col(c)*=s; }
	EIGEN_STRONG_INLINE void multiplyRowByScalar(size_t r, Scalar s)    { derived().row(r)*=s; }

	EIGEN_STRONG_INLINE void swapCols(size_t i1,size_t i2) { derived().col(i1).swap( derived().col(i2) ); }
	EIGEN_STRONG_INLINE void swapRows(size_t i1,size_t i2) { derived().row(i1).swap( derived().row(i2) ); }

	EIGEN_STRONG_INLINE size_t countNonZero() const { return ((*static_cast<const Derived*>(this))!= 0).count(); }

	/** [VECTORS OR MATRICES] Finds the maximum value
	  * \exception std::exception On an empty input container
	  */
	EIGEN_STRONG_INLINE Scalar maximum() const
	{
		if (size()==0) throw std::runtime_error("maximum: container is empty");
		return derived().maxCoeff();
	}

	/** [VECTORS OR MATRICES] Finds the minimum value
	  * \sa maximum, minimum_maximum
	  * \exception std::exception On an empty input container */
	EIGEN_STRONG_INLINE Scalar minimum() const
	{
		if (size()==0) throw std::runtime_error("minimum: container is empty");
		return derived().minCoeff();
	}

	/** [VECTORS OR MATRICES] Compute the minimum and maximum of a container at once
	  * \sa maximum, minimum
	  * \exception std::exception On an empty input container */
	EIGEN_STRONG_INLINE void minimum_maximum(
		Scalar & out_min,
		Scalar & out_max) const
	{
		out_min = minimum();
		out_max = maximum();
	}


	/** [VECTORS ONLY] Finds the maximum value (and the corresponding zero-based index) from a given container.
	  * \exception std::exception On an empty input vector
	  */
	EIGEN_STRONG_INLINE Scalar maximum(size_t *maxIndex) const
	{
		if (size()==0) throw std::runtime_error("maximum: container is empty");
		Index idx;
		const Scalar m = derived().maxCoeff(&idx);
		if (maxIndex) *maxIndex = idx;
		return m;
	}

	/** [VECTORS OR MATRICES] Finds the maximum value (and the corresponding zero-based index) from a given container.
	  * \exception std::exception On an empty input vector
	  */
	void find_index_max_value(size_t &u,size_t &v,Scalar &valMax) const
	{
		if (cols()==0 || rows()==0) throw std::runtime_error("find_index_max_value: container is empty");
		Index idx1,idx2;
		valMax = derived().maxCoeff(&idx1,&idx2);
		u = idx1; v = idx2;
	}


	/** [VECTORS ONLY] Finds the minimum value (and the corresponding zero-based index) from a given container.
	  * \sa maximum, minimum_maximum
	  * \exception std::exception On an empty input vector  */
	EIGEN_STRONG_INLINE Scalar minimum(size_t *minIndex) const
	{
		if (size()==0) throw std::runtime_error("minimum: container is empty");
		Index idx;
		const Scalar m =derived().minCoeff(&idx);
		if (minIndex) *minIndex = idx;
		return m;
	}

	/** [VECTORS ONLY] Compute the minimum and maximum of a container at once
	  * \sa maximum, minimum
	  * \exception std::exception On an empty input vector */
	EIGEN_STRONG_INLINE void minimum_maximum(
		Scalar & out_min,
		Scalar & out_max,
		size_t *minIndex,
		size_t *maxIndex) const
	{
		out_min = minimum(minIndex);
		out_max = maximum(maxIndex);
	}

	/** Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements. */
	EIGEN_STRONG_INLINE Scalar norm_inf() const { return lpNorm<Eigen::Infinity>(); }

	/** 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 */
	EIGEN_STRONG_INLINE Scalar squareNorm() const { return squaredNorm(); }

	/*! Sum all the elements, returning a value of the same type than the container  */
	EIGEN_STRONG_INLINE Scalar sumAll() const { return derived().sum(); }

	/** Computes the laplacian of this square graph weight matrix.
	  *  The laplacian matrix is L = D - W, with D a diagonal matrix with the degree of each node, W the
	  */
	template<typename OtherDerived>
	EIGEN_STRONG_INLINE void laplacian(Eigen::MatrixBase <OtherDerived>& ret) const
	{
		if (rows()!=cols()) throw std::runtime_error("laplacian: Defined for square matrixes only");
		const Index N = rows();
		ret = -(*this);
		for (Index i=0;i<N;i++)
		{
			Scalar deg = 0;
			for (Index j=0;j<N;j++) deg+= derived().coeff(j,i);
			ret.coeffRef(i,i)+=deg;
		}
	}


	/** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable.
	  *  **WARNING**: MRPT's add-on method \a setSize() pads with zeros, while Eigen's \a resize() does NOT (new elements are undefined).
	  */
	EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
	{
#ifdef _DEBUG
		if ((Derived::RowsAtCompileTime!=Eigen::Dynamic && Derived::RowsAtCompileTime!=int(row)) || (Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(col))) {
			std::stringstream ss; ss << "setSize: Trying to change a fixed sized matrix from " << rows() << "x" << cols() << " to " << row << "x" << col;
			throw std::runtime_error(ss.str());
		}
#endif
		const Index oldCols = cols();
		const Index oldRows = rows();
		const int nNewCols = int(col) - int(cols());
		const int nNewRows = int(row) - int(rows());
		::mrpt::math::detail::TAuxResizer<Eigen::MatrixBase<Derived>,SizeAtCompileTime>::internal_resize(*this,row,col);
		if (nNewCols>0) derived().block(0,oldCols,row,nNewCols).setZero();
		if (nNewRows>0) derived().block(oldRows,0,nNewRows,col).setZero();
	}

	/** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it in the passed vector "x". */
	template <class OUTVECT>
	void largestEigenvector(
		OUTVECT &x,
		Scalar resolution = Scalar(0.01),
		size_t maxIterations = 6,
		int    *out_Iterations = NULL,
		float  *out_estimatedResolution = NULL ) const
	{
		// Apply the iterative Power Method:
		size_t iter=0;
		const Index n = rows();
		x.resize(n);
		x.setConstant(1);  // Initially, set to all ones, for example...
		Scalar dif;
		do  // Iterative loop:
		{
			Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,1> xx = (*this) * x;
			xx *= Scalar(1.0/xx.norm());
			dif = (x-xx).array().abs().sum();	// Compute diference between iterations:
			x = xx;	// Set as current estimation:
			iter++; // Iteration counter:
		} while (iter<maxIterations && dif>resolution);
		if (out_Iterations) *out_Iterations=static_cast<int>(iter);
		if (out_estimatedResolution) *out_estimatedResolution=dif;
	}

	/** Combined matrix power and assignment operator */
	MatrixBase<Derived>& operator ^= (const unsigned int pow)
	{
		if (pow==0)
			derived().setIdentity();
		else
		for (unsigned int i=1;i<pow;i++)
			derived() *= derived();
		return *this;
	}

	/** Scalar power of all elements to a given power, this is diferent of ^ operator. */
	EIGEN_STRONG_INLINE void scalarPow(const Scalar s) { (*this)=array().pow(s); }

	/** Checks for matrix type */
	EIGEN_STRONG_INLINE bool isDiagonal() const
	{
		for (Index c=0;c<cols();c++)
			for (Index r=0;r<rows();r++)
				if (r!=c && coeff(r,c)!=0)
					return false;
		return true;
	}

	/** Finds the maximum value in the diagonal of the matrix. */
	EIGEN_STRONG_INLINE Scalar maximumDiagonal() const { return diagonal().maxCoeff(); }

	/** Computes the mean of the entire matrix
	  * \sa meanAndStdAll */
	EIGEN_STRONG_INLINE double mean() const
	{
		if ( size()==0) throw std::runtime_error("mean: Empty container.");
		return derived().sum()/static_cast<double>(size());
	}

	/** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
	  * \sa mean,meanAndStdAll \exception std::exception If the matrix/vector is empty.
	  * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
	  */
	template <class VEC>
	void  meanAndStd(
		VEC &outMeanVector,
		VEC &outStdVector,
		const bool unbiased_variance = true ) const
	{
		const size_t N = rows();
		if (N==0) throw std::runtime_error("meanAndStd: Empty container.");
		const double N_inv = 1.0/N;
		const double N_    = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
		outMeanVector.resize(cols());
		outStdVector.resize(cols());
		for (Index i=0;i<cols();i++)
		{
			outMeanVector[i]= this->col(i).array().sum() * N_inv;
			outStdVector[i] = std::sqrt( (this->col(i).array()-outMeanVector[i]).square().sum() * N_ );
		}
	}

	/** Computes the mean and standard deviation of all the elements in the matrix as a whole.
	  * \sa mean,meanAndStd  \exception std::exception If the matrix/vector is empty.
	  * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
	  */
	void  meanAndStdAll(
		double &outMean,
		double &outStd,
		const bool unbiased_variance = true )  const
	{
		const size_t N = size();
		if (N==0) throw std::runtime_error("meanAndStdAll: Empty container.");
		const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
		outMean = derived().array().sum()/static_cast<double>(size());
		outStd  = std::sqrt( (this->array() - outMean).square().sum()*N_);
	}

	/** Insert matrix "m" into this matrix at indices (r,c), that is, (*this)(r,c)=m(0,0) and so on */
	template <typename MAT>
	EIGEN_STRONG_INLINE void insertMatrix(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.rows(),m.cols())=m; }

	template <typename MAT>
	EIGEN_STRONG_INLINE void insertMatrixTranspose(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.cols(),m.rows())=m.adjoint(); }

	template <typename MAT> EIGEN_STRONG_INLINE void insertRow(size_t nRow, const MAT & aRow) { this->row(nRow) = aRow; }
	template <typename MAT> EIGEN_STRONG_INLINE void insertCol(size_t nCol, const MAT & aCol) { this->col(nCol) = aCol; }

	template <typename R> void insertRow(size_t nRow, const std::vector<R> & aRow)
	{
		if (static_cast<Index>(aRow.size())!=cols()) throw std::runtime_error("insertRow: Row size doesn't fit the size of this matrix.");
		for (Index j=0;j<cols();j++)
			coeffRef(nRow,j) = aRow[j];
	}
	template <typename R> void insertCol(size_t nCol, const std::vector<R> & aCol)
	{
		if (static_cast<Index>(aCol.size())!=rows()) throw std::runtime_error("insertRow: Row size doesn't fit the size of this matrix.");
		for (Index j=0;j<rows();j++)
			coeffRef(j,nCol) = aCol[j];
	}

	/** Remove columns of the matrix.*/
	EIGEN_STRONG_INLINE void removeColumns(const std::vector<size_t> &idxsToRemove)
	{
		std::vector<size_t> idxs = idxsToRemove;
		std::sort( idxs.begin(), idxs.end() );
		std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
		idxs.resize( itEnd - idxs.begin() );

		unsafeRemoveColumns( idxs );
	}

	/** Remove columns of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
	EIGEN_STRONG_INLINE void unsafeRemoveColumns(const std::vector<size_t> &idxs)
	{
		size_t k = 1;
		for (std::vector<size_t>::const_reverse_iterator it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
		{
			const size_t nC = cols() - *it - k;
			if( nC > 0 )
				derived().block(0,*it,rows(),nC) = derived().block(0,*it+1,rows(),nC).eval();
		}
		derived().conservativeResize(NoChange,cols()-idxs.size());
	}

	/** Remove rows of the matrix. */
	EIGEN_STRONG_INLINE void removeRows(const std::vector<size_t> &idxsToRemove)
	{
		std::vector<size_t> idxs = idxsToRemove;
		std::sort( idxs.begin(), idxs.end() );
		std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
		idxs.resize( itEnd - idxs.begin() );

		unsafeRemoveRows( idxs );
	}

	/** Remove rows of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
	EIGEN_STRONG_INLINE void unsafeRemoveRows(const std::vector<size_t> &idxs)
	{
		size_t k = 1;
		for (std::vector<size_t>::reverse_iterator it = idxs.rbegin(); it != idxs.rend(); ++it, ++k)
		{
			const size_t nR = rows() - *it - k;
			if( nR > 0 )
				derived().block(*it,0,nR,cols()) = derived().block(*it+1,0,nR,cols()).eval();
		}
		derived().conservativeResize(rows()-idxs.size(),NoChange);
	}

	/** Transpose */
	EIGEN_STRONG_INLINE const AdjointReturnType t() const { return derived().adjoint(); }

	EIGEN_STRONG_INLINE PlainObject inv() const { PlainObject outMat = derived().inverse().eval(); return outMat; }
	template <class MATRIX> EIGEN_STRONG_INLINE void inv(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
	template <class MATRIX> EIGEN_STRONG_INLINE void inv_fast(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
	EIGEN_STRONG_INLINE Scalar det() const { return derived().determinant(); }

	/** @} */  // end miscelaneous


	/** @name MRPT plugin: Multiply and extra addition functions
		@{ */

	EIGEN_STRONG_INLINE bool empty() const { return this->getColCount()==0 || this->getRowCount()==0; }

	/*! Add c (scalar) times A to this matrix: this += A * c  */
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_Ac(const OTHERMATRIX &m,const Scalar c)	{ (*this)+=c*m; }
	/*! Substract c (scalar) times A to this matrix: this -= A * c  */
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_Ac(const OTHERMATRIX &m,const Scalar c)	{ (*this) -= c*m; }

	/*! Substract A transposed to this matrix: this -= A.adjoint() */
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_At(const OTHERMATRIX &m)	{ (*this) -= m.adjoint(); }

	/*! Substract n (integer) times A to this matrix: this -= A * n  */
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_An(const OTHERMATRIX& m, const size_t n)	{ this->noalias() -= n * m; }

	/*! this += A + A<sup>T</sup>  */
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_AAt(const OTHERMATRIX &A) { this->noalias() += A; this->noalias() += A.adjoint(); }

	/*! this -= A + A<sup>T</sup>  */ \
	template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_AAt(const OTHERMATRIX &A)	{ this->noalias() -= A; this->noalias() -= A.adjoint(); }


	template <class MATRIX1,class MATRIX2> EIGEN_STRONG_INLINE void multiply( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { (*this)= A*B; }

	template <class MATRIX1,class MATRIX2>
	EIGEN_STRONG_INLINE void multiply_AB( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ {
		(*this)= A*B;
	}

	template <typename MATRIX1,typename MATRIX2>
	EIGEN_STRONG_INLINE void multiply_AtB(const MATRIX1 &A,const MATRIX2 &B) /*!< this=A^t * B */ {
		*this = A.adjoint() * B;
	}

	/*! Computes the vector vOut = this * vIn, where "vIn" is a column vector of the appropriate length. */
	template<typename OTHERVECTOR1,typename OTHERVECTOR2>
	EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
		if (accumToOutput) vOut.noalias() += (*this) * vIn;
		else vOut = (*this) * vIn;
	}

	/*! Computes the vector vOut = this<sup>T</sup> * vIn, where "vIn" is a column vector of the appropriate length. */ \
	template<typename OTHERVECTOR1,typename OTHERVECTOR2>
	EIGEN_STRONG_INLINE void multiply_Atb(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
		if (accumToOutput) vOut.noalias() += this->adjoint() * vIn;
		else vOut = this->adjoint() * vIn;
	}

	template <typename MAT_C, typename MAT_R>
	EIGEN_STRONG_INLINE void multiply_HCHt(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this * C * this<sup>T</sup>  */ {
		if (accumResultInOutput)
		      R.noalias() += (*this) * C * this->adjoint();
		else  R.noalias()  = (*this) * C * this->adjoint();
	}

	template <typename MAT_C, typename MAT_R>
	EIGEN_STRONG_INLINE void multiply_HtCH(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this<sup>T</sup> * C * this  */ {
		if (accumResultInOutput)
		      R.noalias() += this->adjoint() * C * (*this);
		else  R.noalias()  = this->adjoint() * C * (*this);
	}

	/*! 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 */
	template <typename MAT_C>
	EIGEN_STRONG_INLINE Scalar multiply_HCHt_scalar(const MAT_C &C) const {
		return ( (*this) * C * this->adjoint() ).eval()(0,0);
	}

	/*! 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 */
	template <typename MAT_C>
	EIGEN_STRONG_INLINE Scalar multiply_HtCH_scalar(const MAT_C &C) const {
		return ( this->adjoint() * C * (*this) ).eval()(0,0);
	}

	/*! this = C * C<sup>T</sup> * f (with a matrix C and a scalar f). */
	template<typename MAT_A>
	EIGEN_STRONG_INLINE void multiply_AAt_scalar(const MAT_A &A,typename MAT_A::Scalar f)	{
		*this = (A * A.adjoint()) * f;
	}

	/*! this = C<sup>T</sup> * C * f (with a matrix C and a scalar f). */
	template<typename MAT_A> EIGEN_STRONG_INLINE void multiply_AtA_scalar(const MAT_A &A,typename MAT_A::Scalar f)	{
		*this = (A.adjoint() * A) * f;
	}

	/*! 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) */
	template <class MAT_A,class SKEW_3VECTOR> void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v) {
		mrpt::math::multiply_A_skew3(A,v,*this); }

	/*! 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) */
	template <class SKEW_3VECTOR,class MAT_A> void multiply_skew3_A(const SKEW_3VECTOR &v,const MAT_A &A) {
		mrpt::math::multiply_skew3_A(v,A,*this); }

	/** outResult = this * A
	  */
	template <class MAT_A,class MAT_OUT>
	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  {
		outResult = derived() * A.block(A_rows_offset,A_cols_offset,derived().cols(),A_col_count);
	}

	template <class MAT_A,class MAT_B,class MAT_C>
	void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*C  */ {
		*this = A*B*C;
	}

	template <class MAT_A,class MAT_B,class MAT_C>
	void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*(C<sup>T</sup>) */ {
		*this = A*B*C.adjoint();
	}

	template <class MAT_A,class MAT_B,class MAT_C>
	void multiply_AtBC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A(<sup>T</sup>)*B*C */ {
		*this = A.adjoint()*B*C;
	}

	template <class MAT_A,class MAT_B>
	EIGEN_STRONG_INLINE void multiply_ABt(const MAT_A &A,const MAT_B &B) /*!< this = A * B<sup>T</sup> */ {
		*this = A*B.adjoint();
	}

	template <class MAT_A>
	EIGEN_STRONG_INLINE void multiply_AAt(const MAT_A &A) /*!< this = A * A<sup>T</sup> */ {
		*this = A*A.adjoint();
	}

	template <class MAT_A>
	EIGEN_STRONG_INLINE void multiply_AtA(const MAT_A &A) /*!< this = A<sup>T</sup> * A */ {
		*this = A.adjoint()*A;
	}

	template <class MAT_A,class MAT_B>
	EIGEN_STRONG_INLINE void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B) /*!< this = A * B (result is symmetric) */ {
		*this = A*B;
	}


	/** Matrix left divide: RES = A<sup>-1</sup> * this , with A being squared (using the Eigen::ColPivHouseholderQR method) */
	template<class MAT2,class MAT3 >
	EIGEN_STRONG_INLINE void leftDivideSquare(const MAT2 &A, MAT3 &RES) const
	{
		Eigen::ColPivHouseholderQR<PlainObject> QR = A.colPivHouseholderQr();
		if (!QR.isInvertible()) throw std::runtime_error("leftDivideSquare: Matrix A is not invertible");
		RES = QR.inverse() * (*this);
	}

	/** Matrix right divide: RES = this * B<sup>-1</sup>, with B being squared  (using the Eigen::ColPivHouseholderQR method)  */
	template<class MAT2,class MAT3 >
	EIGEN_STRONG_INLINE void rightDivideSquare(const MAT2 &B, MAT3 &RES) const
	{
		Eigen::ColPivHouseholderQR<PlainObject> QR = B.colPivHouseholderQr();
		if (!QR.isInvertible()) throw std::runtime_error("rightDivideSquare: Matrix B is not invertible");
		RES = (*this) * QR.inverse();
	}

	/** @} */  // end multiply functions


	/** @name MRPT plugin: Eigenvalue / Eigenvectors
	    @{  */

	/** [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".
	  *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
	  *   \sa eigenVectorsSymmetric, eigenVectorsVec
	  */
	template <class MATRIX1,class MATRIX2>
	EIGEN_STRONG_INLINE void eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
	// Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.

	/** [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".
	  *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
	  *   \sa eigenVectorsSymmetric, eigenVectorsVec
	  */
	template <class MATRIX1,class VECTOR1>
	EIGEN_STRONG_INLINE void eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
	// Implemented in eigen_plugins_impl.h

	/** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), and return only the eigenvalues in the vector "eVals".
	  *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
	  *   \sa eigenVectorsSymmetric, eigenVectorsVec
	  */
	template <class VECTOR>
	EIGEN_STRONG_INLINE void eigenValues( VECTOR & eVals ) const
	{
		PlainObject eVecs;
		eVecs.resizeLike(*this);
		this->eigenVectorsVec(eVecs,eVals);
	}

	/** [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
	  */
	template <class MATRIX1,class MATRIX2>
	EIGEN_STRONG_INLINE void eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
	// Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.

	/** [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
	  */
	template <class MATRIX1,class VECTOR1>
	EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
	// Implemented in eigen_plugins_impl.h


	/** @} */  // end eigenvalues



	/** @name MRPT plugin: Linear algebra & decomposition-based methods
	    @{ */

	/** Cholesky M=U<sup>T</sup> * U decomposition for simetric matrix (upper-half of the matrix will be actually ignored) */
	template <class MATRIX> EIGEN_STRONG_INLINE bool chol(MATRIX &U) const
	{
		Eigen::LLT<PlainObject> Chol = derived().template selfadjointView<Eigen::Lower>().llt();
		if (Chol.info()==Eigen::NoConvergence)
			return false;
		U = PlainObject(Chol.matrixU());
		return true;
	}

	/** Gets the rank of the matrix via the Eigen::ColPivHouseholderQR method
	  * \param threshold If set to >0, it's used as threshold instead of Eigen's default one.
	  */
	EIGEN_STRONG_INLINE size_t rank(double threshold=0) const
	{
		Eigen::ColPivHouseholderQR<PlainObject> QR = this->colPivHouseholderQr();
		if (threshold>0) QR.setThreshold(threshold);
		return QR.rank();
	}

	/** @} */   // end linear algebra



	/** @name MRPT plugin: Scalar and element-wise extra operators
	    @{ */

	/** Scales all elements such as the minimum & maximum values are shifted to the given values */
	void normalize(Scalar valMin, Scalar valMax)
	{
		if (size()==0) return;
		Scalar curMin,curMax;
		minimum_maximum(curMin,curMax);
		Scalar minMaxDelta = curMax - curMin;
		if (minMaxDelta==0) minMaxDelta = 1;
		const Scalar minMaxDelta_ = (valMax-valMin)/minMaxDelta;
		this->array() = (this->array()-curMin)*minMaxDelta_+valMin;
	}
	//! \overload
	inline void adjustRange(Scalar valMin, Scalar valMax) { normalize(valMin,valMax); }

	/** @} */  // end Scalar


	/** Extract one row from the matrix into a row vector */
	template <class OtherDerived> EIGEN_STRONG_INLINE void extractRow(size_t nRow, Eigen::EigenBase<OtherDerived> &v, size_t startingCol = 0) const {
		v = derived().block(nRow,startingCol,1,cols()-startingCol);
	}
	//! \overload
	template <typename T> inline void extractRow(size_t nRow, std::vector<T> &v, size_t startingCol = 0) const {
		const size_t N = cols()-startingCol;
		v.resize(N);
		for (size_t i=0;i<N;i++) v[i]=(*this)(nRow,startingCol+i);
	}
	/** Extract one row from the matrix into a column vector */
	template <class VECTOR> EIGEN_STRONG_INLINE void extractRowAsCol(size_t nRow, VECTOR &v, size_t startingCol = 0) const
	{
		v = derived().adjoint().block(startingCol,nRow,cols()-startingCol,1);
	}


	/** Extract one column from the matrix into a column vector */
	template <class VECTOR> EIGEN_STRONG_INLINE void extractCol(size_t nCol, VECTOR &v, size_t startingRow = 0) const {
		v = derived().block(startingRow,nCol,rows()-startingRow,1);
	}
	//! \overload
	template <typename T> inline void extractCol(size_t nCol, std::vector<T> &v, size_t startingRow = 0) const {
		const size_t N = rows()-startingRow;
		v.resize(N);
		for (size_t i=0;i<N;i++) v[i]=(*this)(startingRow+i,nCol);
	}

	template <class MATRIX> EIGEN_STRONG_INLINE void extractMatrix(const size_t firstRow, const size_t firstCol, MATRIX &m) const
	{
		m = derived().block(firstRow,firstCol,m.rows(),m.cols());
	}
	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
	{
		m.resize(nRows,nCols);
		m = derived().block(firstRow,firstCol,nRows,nCols);
	}

	/** Get a submatrix, given its bounds: first & last column and row (inclusive). */
	template <class MATRIX>
	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
	{
		out.resize(row_last-row_first+1,col_last-col_first+1);
		out = derived().block(row_first,col_first,row_last-row_first+1,col_last-col_first+1);
	}

	/** 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.
	  *  A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
	  * \sa extractSubmatrix, extractSubmatrixSymmetrical
	  */
	template <class MATRIX>
	void extractSubmatrixSymmetricalBlocks(
		const size_t 			block_size,
		const std::vector<size_t>  	&block_indices,
		MATRIX& out) const
	{
		if (block_size<1) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: block_size must be >=1");
		if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Matrix is not square.");

		const size_t N = block_indices.size();
		const size_t nrows_out=N*block_size;
		out.resize(nrows_out,nrows_out);
		if (!N) return; // Done
		for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
		{
			for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
			{
#if defined(_DEBUG)
				if (block_indices[dst_col_blk]*block_size + block_size-1>=size_t(cols())) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Indices out of range!");
#endif
				out.block(dst_row_blk * block_size,dst_col_blk * block_size, block_size,block_size)
				=
				derived().block(block_indices[dst_row_blk] * block_size, block_indices[dst_col_blk] * block_size, block_size,block_size);
			}
		}
	}


	/** Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequence of indices passed as argument.
	  *  A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
	  * \sa extractSubmatrix, extractSubmatrixSymmetricalBlocks
	  */
	template <class MATRIX>
	void extractSubmatrixSymmetrical(
		const std::vector<size_t>  	&indices,
		MATRIX& out) const
	{
		if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetrical: Matrix is not square.");

		const size_t N = indices.size();
		const size_t nrows_out=N;
		out.resize(nrows_out,nrows_out);
		if (!N) return; // Done
		for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
			for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
				out.coeffRef(dst_row_blk,dst_col_blk) = this->coeff(indices[dst_row_blk],indices[dst_col_blk]);
	}

