ops_containers.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_container_ops_H
#define mrpt_math_container_ops_H

#include <mrpt/utils/types_math.h>

#include <mrpt/math/lightweight_geom_data.h>  // forward declarations

#include <functional>
#include <algorithm>
#include <cmath>

/** \addtogroup container_ops_grp Vector and matrices mathematical operations
 * and other utilities
  *  \ingroup mrpt_base_grp
  *  @{ */

/** \file ops_containers.h
  * This file implements several operations that operate element-wise on
 * individual or pairs of containers.
  *  Containers here means any of: mrpt::math::CVectorTemplace,
 * mrpt::math::CArray, mrpt::math::CMatrixFixedNumeric,
 * mrpt::math::CMatrixTemplate.
  *
  *  In general, any container having a type "mrpt_autotype" self-referencing to
 * the type itself, and a dummy struct mrpt_container<>
  *   which is only used as a way to force the compiler to assure that BOTH
 * containers are valid ones in binary operators.
  *   This restrictions
  *   have been designed as a way to provide "polymorphism" at a template level,
 * so the "+,-,..." operators do not
  *   generate ambiguities for ANY type, and limiting them to MRPT containers.
  *
  *   In some cases, the containers provide specializations of some operations,
 * for increased performance.
  */

#include <algorithm>
#include <numeric>
#include <functional>

#include <mrpt/math/CHistogram.h>  // Used in ::histogram()

#include "ops_vectors.h"

namespace mrpt
{
namespace math
{
/** Computes the normalized or normal histogram of a sequence of numbers given
 * the number of bins and the limits.
  *  In any case this is a "linear" histogram, i.e. for matrices, all the
 * elements are taken as if they were a plain sequence, not taking into account
 * they were in columns or rows.
  *  If desired, out_bin_centers can be set to receive the bins centers.
  */
template <class CONTAINER>
std::vector<double> histogram(
        const CONTAINER& v, double limit_min, double limit_max, size_t number_bins,
        bool do_normalization = false,
        std::vector<double>* out_bin_centers = nullptr)
{
        mrpt::math::CHistogram H(limit_min, limit_max, number_bins);
        std::vector<double> ret(number_bins);
        std::vector<double> dummy_ret_bins;
        H.add(v);
        if (do_normalization)
                H.getHistogramNormalized(
                        out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret);
        else
                H.getHistogram(
                        out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret);
        return ret;
}

template <class EIGEN_CONTAINER>
void resizeLike(EIGEN_CONTAINER& trg, const EIGEN_CONTAINER& src)
{
        trg.resizeLike(src);
}
template <typename T>
void resizeLike(std::vector<T>& trg, const std::vector<T>& src)
{
        trg.resize(src.size());
}

/** Computes the cumulative sum of all the elements, saving the result in
 * another container.
  *  This works for both matrices (even mixing their types) and vectores/arrays
 * (even mixing types),
  *  and even to store the cumsum of any matrix into any vector/array, but not
 * in opposite direction.
  * \sa sum */
template <class CONTAINER1, class CONTAINER2, typename VALUE>
inline void cumsum_tmpl(const CONTAINER1& in_data, CONTAINER2& out_cumsum)
{
        resizeLike(out_cumsum, in_data);
        VALUE last = 0;
        const size_t N = in_data.size();
        for (size_t i = 0; i < N; i++) last = out_cumsum[i] = last + in_data[i];
}

template <class CONTAINER1, class CONTAINER2>
inline void cumsum(const CONTAINER1& in_data, CONTAINER2& out_cumsum)
{
        cumsum_tmpl<CONTAINER1, CONTAINER2,
                                typename mrpt::math::ContainerType<CONTAINER2>::element_t>(
                in_data, out_cumsum);
}

/** Computes the cumulative sum of all the elements
  * \sa sum  */
template <class CONTAINER>
inline CONTAINER cumsum(const CONTAINER& in_data)
{
        CONTAINER ret;
        cumsum(in_data, ret);
        return ret;
}

template <class CONTAINER>
inline typename CONTAINER::Scalar norm_inf(const CONTAINER& v)
{
        return v.norm_inf();
}
template <class CONTAINER>
inline typename CONTAINER::Scalar norm(const CONTAINER& v)
{
        return v.norm();
}
template <class CONTAINER>
inline typename CONTAINER::Scalar maximum(const CONTAINER& v)
{
        return v.maxCoeff();
}
template <class CONTAINER>
inline typename CONTAINER::Scalar minimum(const CONTAINER& v)
{
        return v.minimum();
}

template <typename T>
inline T maximum(const std::vector<T>& v)
{
        ASSERT_(!v.empty())
        T m = v[0];
        for (size_t i = 0; i < v.size(); i++) mrpt::utils::keep_max(m, v[i]);
        return m;
}
template <typename T>
inline T minimum(const std::vector<T>& v)
{
        ASSERT_(!v.empty())
        T m = v[0];
        for (size_t i = 0; i < v.size(); i++) mrpt::utils::keep_min(m, v[i]);
        return m;
}

/** \name Generic container element-wise operations - Miscelaneous
  * @{
  */

/** Accumulate the squared-norm of a vector/array/matrix into "total" (this
 * function is compatible with std::accumulate). */
template <class CONTAINER, typename VALUE>
VALUE squareNorm_accum(const VALUE total, const CONTAINER& v)
{
        return total + v.squaredNorm();
}

/** Compute the square norm of anything implementing [].
  \sa norm */
template <size_t N, class T, class U>
inline T squareNorm(const U& v)
{
        T res = 0;
        for (size_t i = 0; i < N; i++) res += square(v[i]);
        return res;
}

/** v1*v2: The dot product of two containers (vectors/arrays/matrices) */
template <class CONTAINER1, class CONTAINER2>
inline typename CONTAINER1::Scalar dotProduct(
        const CONTAINER1& v1, const CONTAINER1& v2)
{
        return v1.dot(v2);
}

/** v1*v2: The dot product of any two objects supporting []  */
template <size_t N, class T, class U, class V>
inline T dotProduct(const U& v1, const V& v2)
{
        T res = 0;
        for (size_t i = 0; i < N; i++) res += v1[i] * v2[i];
        return res;
}

/** Computes the sum of all the elements.
  * \note If used with containers of integer types (uint8_t, int, etc...) this
  could overflow. In those cases, use sumRetType the second argument RET to
  specify a larger type to hold the sum.
   \sa cumsum  */
template <class CONTAINER>
inline typename CONTAINER::Scalar sum(const CONTAINER& v)
{
        return v.sum();
}

/// \overload
template <typename T>
inline T sum(const std::vector<T>& v)
{
        return std::accumulate(v.begin(), v.end(), T(0));
}

/** Computes the sum of all the elements, with a custom return type.
   \sa sum, cumsum  */
template <class CONTAINER, typename RET>
inline RET sumRetType(const CONTAINER& v)
{
        return v.template sumRetType<RET>();
}

/** Computes the mean value of a vector  \return The mean, as a double number.
  * \sa math::stddev,math::meanAndStd  */
template <class CONTAINER>
inline double mean(const CONTAINER& v)
{
        if (v.empty())
                return 0;
        else
                return sum(v) / static_cast<double>(v.size());
}

/** Return the maximum and minimum values of a std::vector */
template <typename T>
inline void minimum_maximum(const std::vector<T>& V, T& curMin, T& curMax)
{
        ASSERT_(V.size() != 0)
        const size_t N = V.size();
        curMin = curMax = V[0];
        for (size_t i = 1; i < N; i++)
        {
                mrpt::utils::keep_min(curMin, V[i]);
                mrpt::utils::keep_max(curMax, V[i]);
        }
}

/** Return the maximum and minimum values of a Eigen-based vector or matrix */
template <class Derived>
inline void minimum_maximum(
        const Eigen::MatrixBase<Derived>& V,
        typename Eigen::MatrixBase<Derived>::Scalar& curMin,
        typename Eigen::MatrixBase<Derived>::Scalar& curMax)
{
        V.minimum_maximum(curMin, curMax);
}

/** Counts the number of elements that appear in both STL-like containers
 * (comparison through the == operator)
  *  It is assumed that no repeated elements appear within each of the
 * containers.  */
template <class CONTAINER1, class CONTAINER2>
size_t countCommonElements(const CONTAINER1& a, const CONTAINER2& b)
{
        size_t ret = 0;
        for (typename CONTAINER1::const_iterator it1 = a.begin(); it1 != a.end();
                 ++it1)
                for (typename CONTAINER2::const_iterator it2 = b.begin();
                         it2 != b.end(); ++it2)
                        if ((*it1) == (*it2)) ret++;
        return ret;
}

/** Adjusts the range of all the elements such as the minimum and maximum values
 * being those supplied by the user.  */
template <class CONTAINER>
void adjustRange(
        CONTAINER& m, const typename CONTAINER::Scalar minVal,
        const typename CONTAINER::Scalar maxVal)
{
        if (size_t(m.size()) == 0) return;
        typename CONTAINER::Scalar curMin, curMax;
        minimum_maximum(m, curMin, curMax);
        const typename CONTAINER::Scalar curRan = curMax - curMin;
        m -= (curMin + minVal);
        if (curRan != 0) m *= (maxVal - minVal) / curRan;
}

/** Computes the standard deviation of a vector
  * \param v The set of data
  * \param out_mean The output for the estimated mean
  * \param out_std The output for the estimated standard deviation
  * \param unbiased If set to true or false the std is normalized by "N-1" or
 * "N", respectively.
  * \sa math::mean,math::stddev
  */
template <class VECTORLIKE>
void meanAndStd(
        const VECTORLIKE& v, double& out_mean, double& out_std,
        bool unbiased = true)
{
        if (v.size() < 2)
        {
                out_std = 0;
                out_mean = (v.size() == 1) ? *v.begin() : 0;
        }
        else
        {
                // Compute the mean:
                const size_t N = v.size();
                out_mean = mrpt::math::sum(v) / static_cast<double>(N);
                // Compute the std:
                double vector_std = 0;
                for (size_t i = 0; i < N; i++)
                        vector_std += mrpt::math::square(v[i] - out_mean);
                out_std =
                        std::sqrt(vector_std / static_cast<double>(N - (unbiased ? 1 : 0)));
        }
}

/** Computes the standard deviation of a vector
  * \param v The set of data
  * \param unbiased If set to true or false the std is normalized by "N-1" or
 * "N", respectively.
  * \sa math::mean,math::meanAndStd
  */
template <class VECTORLIKE>
inline double stddev(const VECTORLIKE& v, bool unbiased = true)
{
        double m, s;
        meanAndStd(v, m, s, unbiased);
        return s;
}

/** Computes the mean vector and covariance from a list of values given as a
 * vector of vectors, where each row is a sample.
  * \param v The set of data, as a vector of N vectors of M elements.
  * \param out_mean The output M-vector for the estimated mean.
  * \param out_cov The output MxM matrix for the estimated covariance matrix.
  * \sa mrpt::math::meanAndCovMat, math::mean,math::stddev, math::cov
  */
template <class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE>
void meanAndCovVec(
        const VECTOR_OF_VECTOR& v, VECTORLIKE& out_mean, MATRIXLIKE& out_cov)
{
        const size_t N = v.size();
        ASSERTMSG_(N > 0, "The input vector contains no elements");
        const double N_inv = 1.0 / N;

        const size_t M = v[0].size();
        ASSERTMSG_(M > 0, "The input vector 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[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[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[i][j] - out_mean[j]) * (v[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 values given as a vector of
 * vectors, where each row is a sample.
  * \param v The set of data, as a vector of N vectors of M elements.
  * \param out_cov The output MxM matrix for the estimated covariance matrix.
  * \tparam RETURN_MATRIX The type of the returned matrix, e.g. Eigen::MatrixXd
  * \sa math::mean,math::stddev, math::cov, meanAndCovVec
  */
template <class VECTOR_OF_VECTOR, class RETURN_MATRIX>
inline RETURN_MATRIX covVector(const VECTOR_OF_VECTOR& v)
{
        std::vector<double> m;
        RETURN_MATRIX C;
        meanAndCovVec(v, m, C);
        return C;
}

/** Normalised Cross Correlation between two vector patches
  * The Matlab code for this is
  * a = a - mean2(a);
  * b = b - mean2(b);
  * r = sum(sum(a.*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
  */
template <class CONT1, class CONT2>
double ncc_vector(const CONT1& patch1, const CONT2& patch2)
{
        ASSERT_(patch1.size() == patch2.size())

        double numerator = 0, sum_a = 0, sum_b = 0, result, a_mean, b_mean;
        a_mean = patch1.mean();
        b_mean = patch2.mean();

        const size_t N = patch1.size();
        for (size_t i = 0; i < N; ++i)
        {
                numerator += (patch1[i] - a_mean) * (patch2[i] - b_mean);
                sum_a += mrpt::math::square(patch1[i] - a_mean);
                sum_b += mrpt::math::square(patch2[i] - b_mean);
        }
        ASSERTMSG_(sum_a * sum_b != 0, "Divide by zero when normalizing.")
        result = numerator / std::sqrt(sum_a * sum_b);
        return result;
}

/** @} Misc ops */

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

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

#endif