CPoint2DPDFGaussian.cpp

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 |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/poses/CPoint2DPDFGaussian.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/math/CMatrixD.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/random/RandomGenerators.h>
#include <mrpt/system/os.h>

using namespace mrpt::poses;

using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::system;

IMPLEMENTS_SERIALIZABLE(CPoint2DPDFGaussian, CPoint2DPDF, mrpt::poses)

/*---------------------------------------------------------------
    Constructor
  ---------------------------------------------------------------*/
CPoint2DPDFGaussian::CPoint2DPDFGaussian() : mean(0, 0), cov() {}
/*---------------------------------------------------------------
    Constructor
  ---------------------------------------------------------------*/
CPoint2DPDFGaussian::CPoint2DPDFGaussian(
    const CPoint2D& init_Mean, const CMatrixDouble22& init_Cov)
    : mean(init_Mean), cov(init_Cov)
{
}

/*---------------------------------------------------------------
    Constructor
  ---------------------------------------------------------------*/
CPoint2DPDFGaussian::CPoint2DPDFGaussian(const CPoint2D& init_Mean)
    : mean(init_Mean), cov()
{
}

uint8_t CPoint2DPDFGaussian::serializeGetVersion() const { return 0; }
void CPoint2DPDFGaussian::serializeTo(mrpt::serialization::CArchive& out) const
{
    out << CPoint2D(mean) << cov;
}
void CPoint2DPDFGaussian::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            in >> mean >> cov;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
}

void CPoint2DPDFGaussian::copyFrom(const CPoint2DPDF& o)
{
    if (this == &o) return;  // It may be used sometimes

    // Convert to gaussian pdf:
    o.getCovarianceAndMean(cov, mean);
}

/*---------------------------------------------------------------

  ---------------------------------------------------------------*/
bool CPoint2DPDFGaussian::saveToTextFile(const std::string& file) const
{
    MRPT_START

    FILE* f = os::fopen(file.c_str(), "wt");
    if (!f) return false;

    os::fprintf(f, "%f %f\n", mean.x(), mean.y());

    os::fprintf(f, "%f %f\n", cov(0, 0), cov(0, 1));
    os::fprintf(f, "%f %f\n", cov(1, 0), cov(1, 1));

    os::fclose(f);
    return true;
    MRPT_END
}

/*---------------------------------------------------------------
                        changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPoint2DPDFGaussian::changeCoordinatesReference(
    const CPose3D& newReferenceBase)
{
    // Clip the 3x3 rotation matrix
    const CMatrixDouble22 M =
        newReferenceBase.getRotationMatrix().block(0, 0, 2, 2);

    // The mean:
    mean = CPoint2D(newReferenceBase + mean);

    // The covariance:
    cov = M * cov * M.transpose();
}

/*---------------------------------------------------------------
                    bayesianFusion
 ---------------------------------------------------------------*/
void CPoint2DPDFGaussian::bayesianFusion(
    const CPoint2DPDFGaussian& p1, const CPoint2DPDFGaussian& p2)
{
    MRPT_START

    CMatrixDouble22 C1_inv;
    p1.cov.inv(C1_inv);

    CMatrixDouble22 C2_inv;
    p2.cov.inv(C2_inv);

    CMatrixDouble22 L = C1_inv;
    L += C2_inv;

    L.inv(cov);  // The new cov.

    const Eigen::Vector2d x1{p1.mean.x(), p1.mean.y()};
    const Eigen::Vector2d x2{p2.mean.x(), p2.mean.y()};
    CMatrixDouble21 x = cov * (C1_inv * x1 + C2_inv * x2);

    mean.x(x.get_unsafe(0, 0));
    mean.y(x.get_unsafe(1, 0));

    MRPT_END
}

/*---------------------------------------------------------------
                    productIntegralWith
 ---------------------------------------------------------------*/
double CPoint2DPDFGaussian::productIntegralWith(
    const CPoint2DPDFGaussian& p) const
{
    MRPT_START
    // --------------------------------------------------------------
    // 12/APR/2009 - Jose Luis Blanco:
    //  The integral over all the variable space of the product of two
    //   Gaussians variables amounts to simply the evaluation of
    //   a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
    // ---------------------------------------------------------------
    CMatrixDouble22 C = cov + p.cov;  // Sum of covs:

    CMatrixDouble22 C_inv;
    C.inv(C_inv);

    CMatrixDouble21 MU(UNINITIALIZED_MATRIX);  // Diff. of means
    MU.get_unsafe(0, 0) = mean.x() - p.mean.x();
    MU.get_unsafe(1, 0) = mean.y() - p.mean.y();

    return std::pow(M_2PI, -0.5 * state_length) * (1.0 / std::sqrt(C.det())) *
           exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));

    MRPT_END
}

/*---------------------------------------------------------------
                    productIntegralNormalizedWith
 ---------------------------------------------------------------*/
double CPoint2DPDFGaussian::productIntegralNormalizedWith(
    const CPoint2DPDFGaussian& p) const
{
    return std::exp(-0.5 * square(mahalanobisDistanceTo(p)));
}

/*---------------------------------------------------------------
                    drawSingleSample
 ---------------------------------------------------------------*/
void CPoint2DPDFGaussian::drawSingleSample(CPoint2D& outSample) const
{
    MRPT_START

    // Eigen3 emits an out-of-array warning here, but it seems to be a false
    // warning? (WTF)
    CVectorDouble vec;
    getRandomGenerator().drawGaussianMultivariate(vec, cov);

    ASSERT_(vec.size() == 2);
    outSample.x(mean.x() + vec[0]);
    outSample.y(mean.y() + vec[1]);

    MRPT_END
}

/*---------------------------------------------------------------
                    bayesianFusion
 ---------------------------------------------------------------*/
void CPoint2DPDFGaussian::bayesianFusion(
    const CPoint2DPDF& p1_, const CPoint2DPDF& p2_,
    const double minMahalanobisDistToDrop)
{
    MRPT_UNUSED_PARAM(minMahalanobisDistToDrop);
    MRPT_START

    // p1: CPoint2DPDFGaussian, p2: CPosePDFGaussian:
    ASSERT_(p1_.GetRuntimeClass() == CLASS_ID(CPoint2DPDFGaussian));
    ASSERT_(p2_.GetRuntimeClass() == CLASS_ID(CPoint2DPDFGaussian));

    THROW_EXCEPTION("TODO!!!");

    MRPT_END
}

/*---------------------------------------------------------------
                    mahalanobisDistanceTo
 ---------------------------------------------------------------*/
double CPoint2DPDFGaussian::mahalanobisDistanceTo(
    const CPoint2DPDFGaussian& other) const
{
    // The difference in means:
    Eigen::Matrix<double, 2, 1> deltaX;
    deltaX[0] = other.mean.x() - mean.x();
    deltaX[1] = other.mean.y() - mean.y();

    // The inverse of the combined covs:
    return std::sqrt(
        deltaX.multiply_HtCH_scalar((other.cov + this->cov).inverse()));
}

/** Returns the Mahalanobis distance from this PDF to some point */
double CPoint2DPDFGaussian::mahalanobisDistanceToPoint(
    const double x, const double y) const
{
    // The difference in means:
    Eigen::Matrix<double, 2, 1> deltaX;
    deltaX[0] = x - mean.x();
    deltaX[1] = y - mean.y();

    // The inverse of the combined covs:
    return std::sqrt(deltaX.multiply_HtCH_scalar(this->cov.inverse()));
}