ops_matrices.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 |
   +------------------------------------------------------------------------+ */
#ifndef mrpt_math_matrix_ops_H
#define mrpt_math_matrix_ops_H

#include <mrpt/math/math_frwds.h>  // forward declarations
#include <mrpt/math/eigen_frwds.h>  // forward declarations

#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>

#include <mrpt/math/ops_containers.h>  // Many generic operations

/** \file ops_matrices.h
  * This file implements miscelaneous matrix and matrix/vector operations, and
 * internal functions in mrpt::math::detail
  */
namespace mrpt
{
namespace math
{
/** \addtogroup container_ops_grp
  *  @{ */

/** Transpose operator for matrices */
template <class Derived>
inline const typename Eigen::MatrixBase<Derived>::AdjointReturnType operator~(
        const Eigen::MatrixBase<Derived>& m)
{
        return m.adjoint();
}

/** Unary inversion operator. */
template <class Derived>
inline typename Eigen::MatrixBase<Derived>::PlainObject operator!(
        const Eigen::MatrixBase<Derived>& m)
{
        return m.inv();
}

/** @} */  // end MRPT matrices operators

/** R = H * C * H^t (with C symmetric) */
template <typename MAT_H, typename MAT_C, typename MAT_R>
inline void multiply_HCHt(
        const MAT_H& H, const MAT_C& C, MAT_R& R,
        bool accumResultInOutput)  //   bool allow_submatrix_mult)
{
        if (accumResultInOutput)
                R += ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
                          H.adjoint())
                                 .eval()
                                 .template selfadjointView<Eigen::Lower>();
        else
                R = ((H * C.template selfadjointView<Eigen::Lower>()).eval() *
                         H.adjoint())
                                .eval()
                                .template selfadjointView<Eigen::Lower>();
}

/** r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C) */
template <typename VECTOR_H, typename MAT_C>
typename MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H& H, const MAT_C& C)
{
        return (H.matrix().adjoint() * C * H.matrix()).eval()(0, 0);
}

/** R = H^t * C * H  (with C symmetric) */
template <typename MAT_H, typename MAT_C, typename MAT_R>
void multiply_HtCH(
        const MAT_H& H, const MAT_C& C, MAT_R& R,
        bool accumResultInOutput)  // bool allow_submatrix_mult)
{
        if (accumResultInOutput)
                R +=
                        ((H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() *
                         H)
                                .eval()
                                .template selfadjointView<Eigen::Lower>();
        else
                R = ((H.adjoint() * C.template selfadjointView<Eigen::Lower>()).eval() *
                         H)
                                .eval()
                                .template selfadjointView<Eigen::Lower>();
}

/** Computes the mean vector and covariance from a list of samples in an NxM
 * matrix, where each row is a sample, so the covariance is MxM.
  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric
 * or CMatrixFixedNumeric
  * \param out_mean The output M-vector for the estimated mean.
  * \param out_cov The output MxM matrix for the estimated covariance matrix,
 * this can also be either a fixed-size of dynamic size matrix.
  * \sa mrpt::math::meanAndCovVec, math::mean,math::stddev, math::cov
  */
template <class MAT_IN, class VECTOR, class MAT_OUT>
void meanAndCovMat(const MAT_IN& v, VECTOR& out_mean, MAT_OUT& out_cov)
{
        const size_t N = v.rows();
        ASSERTMSG_(N > 0, "The input matrix contains no elements");
        const double N_inv = 1.0 / N;

        const size_t M = v.cols();
        ASSERTMSG_(M > 0, "The input matrix contains rows of length 0");

        // First: Compute the mean
        out_mean.assign(M, 0);
        for (size_t i = 0; i < N; i++)
                for (size_t j = 0; j < M; j++) out_mean[j] += v.coeff(i, j);
        out_mean *= N_inv;

        // Second: Compute the covariance
        //  Save only the above-diagonal part, then after averaging
        //  duplicate that part to the other half.
        out_cov.zeros(M, M);
        for (size_t i = 0; i < N; i++)
        {
                for (size_t j = 0; j < M; j++)
                        out_cov.get_unsafe(j, j) +=
                                square(v.get_unsafe(i, j) - out_mean[j]);

                for (size_t j = 0; j < M; j++)
                        for (size_t k = j + 1; k < M; k++)
                                out_cov.get_unsafe(j, k) += (v.get_unsafe(i, j) - out_mean[j]) *
                                                                                        (v.get_unsafe(i, k) - out_mean[k]);
        }
        for (size_t j = 0; j < M; j++)
                for (size_t k = j + 1; k < M; k++)
                        out_cov.get_unsafe(k, j) = out_cov.get_unsafe(j, k);
        out_cov *= N_inv;
}

/** Computes the covariance matrix from a list of samples in an NxM matrix,
 * where each row is a sample, so the covariance is MxM.
  * \param v The set of data, as a NxM matrix.
  * \param out_cov The output MxM matrix for the estimated covariance matrix.
  * \sa math::mean,math::stddev, math::cov
  */
template <class MATRIX>
inline Eigen::Matrix<typename MATRIX::Scalar, MATRIX::ColsAtCompileTime,
                                         MATRIX::ColsAtCompileTime>
        cov(const MATRIX& v)
{
        Eigen::Matrix<double, MATRIX::ColsAtCompileTime, 1> m;
        Eigen::Matrix<typename MATRIX::Scalar, MATRIX::ColsAtCompileTime,
                                  MATRIX::ColsAtCompileTime>
                C;
        meanAndCovMat(v, m, C);
        return C;
}

/** A useful macro for saving matrixes to a file while debugging. */
#define SAVE_MATRIX(M) M.saveToTextFile(#M ".txt");

/** Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where
 * Skew(v) is the skew symmetric matric generated from \a v (see
 * mrpt::math::skew_symmetric3)
  */
template <class MAT_A, class SKEW_3VECTOR, class MAT_OUT>
void multiply_A_skew3(const MAT_A& A, const SKEW_3VECTOR& v, MAT_OUT& out)
{
        MRPT_START
        ASSERT_EQUAL_(size(A, 2), 3)
        ASSERT_EQUAL_(v.size(), 3)
        const size_t N = size(A, 1);
        out.setSize(N, 3);
        for (size_t i = 0; i < N; i++)
        {
                out.set_unsafe(
                        i, 0, A.get_unsafe(i, 1) * v[2] - A.get_unsafe(i, 2) * v[1]);
                out.set_unsafe(
                        i, 1, -A.get_unsafe(i, 0) * v[2] + A.get_unsafe(i, 2) * v[0]);
                out.set_unsafe(
                        i, 2, A.get_unsafe(i, 0) * v[1] - A.get_unsafe(i, 1) * v[0]);
        }
        MRPT_END
}

/** Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where
 * Skew(v) is the skew symmetric matric generated from \a v (see
 * mrpt::math::skew_symmetric3)
  */
template <class SKEW_3VECTOR, class MAT_A, class MAT_OUT>
void multiply_skew3_A(const SKEW_3VECTOR& v, const MAT_A& A, MAT_OUT& out)
{
        MRPT_START
        ASSERT_EQUAL_(size(A, 1), 3)
        ASSERT_EQUAL_(v.size(), 3)
        const size_t N = size(A, 2);
        out.setSize(3, N);
        for (size_t i = 0; i < N; i++)
        {
                out.set_unsafe(
                        0, i, -A.get_unsafe(1, i) * v[2] + A.get_unsafe(2, i) * v[1]);
                out.set_unsafe(
                        1, i, A.get_unsafe(0, i) * v[2] - A.get_unsafe(2, i) * v[0]);
                out.set_unsafe(
                        2, i, -A.get_unsafe(0, i) * v[1] + A.get_unsafe(1, i) * v[0]);
        }
        MRPT_END
}

// ------ Implementatin of detail functions -------------
namespace detail
{
/** Extract a submatrix - The output matrix must be set to the required size
 * before call. */
template <class MATORG, class MATDEST>
void extractMatrix(
        const MATORG& M, const size_t first_row, const size_t first_col,
        MATDEST& outMat)
{
        const size_t NR = outMat.getRowCount();
        const size_t NC = outMat.getColCount();
        ASSERT_BELOWEQ_(first_row + NR, M.getRowCount())
        ASSERT_BELOWEQ_(first_col + NC, M.getColCount())
        for (size_t r = 0; r < NR; r++)
                for (size_t c = 0; c < NC; c++)
                        outMat.get_unsafe(r, c) =
                                M.get_unsafe(first_row + r, first_col + c);
}

}  // end of detail namespace

/**  @} */  // end of grouping

}  // End of math namespace
}  // End of mrpt namespace

#endif