eigen_plugins.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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.
  *  @{ */
using iterator = Scalar*;
using const_iterator = const Scalar*;
 
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);
}
 
/** 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);
#else
    return derived().coeffRef(row, col);
#endif
}
/** 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;
#else
    derived().coeffRef(row, col) = val;
#endif
}
 
/** 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 = nullptr);
// 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 = nullptr,
    float* out_estimatedResolution = nullptr) 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 (decltype(cols()) 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 (decltype(cols()) 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 (decltype(cols()) 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 (auto 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->cols() == 0 || this->rows() == 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;
}
 
/** @} */  // 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
  *  \return false on error
  */
template <class MATRIX1, class MATRIX2>
EIGEN_STRONG_INLINE bool 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
  *  \return false on error
  */
template <class MATRIX1, class VECTOR1>
EIGEN_STRONG_INLINE bool 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]);
}