CPointPDFGaussian.cpp

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

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

#include <mrpt/poses/CPointPDFGaussian.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/random/RandomGenerators.h>
#include <mrpt/system/os.h>
#include <mrpt/utils/CStream.h>

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

IMPLEMENTS_SERIALIZABLE(CPointPDFGaussian, CPointPDF, mrpt::poses)

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPointPDFGaussian::CPointPDFGaussian() : mean(0, 0, 0), cov() {}
/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPointPDFGaussian::CPointPDFGaussian(
        const CPoint3D& init_Mean, const CMatrixDouble33& init_Cov)
        : mean(init_Mean), cov(init_Cov)
{
}

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPointPDFGaussian::CPointPDFGaussian(const CPoint3D& init_Mean)
        : mean(init_Mean), cov()
{
        cov.zeros();
}

/*---------------------------------------------------------------
                                                getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFGaussian::getMean(CPoint3D& p) const { p = mean; }
/*---------------------------------------------------------------
                                                getCovarianceAndMean
 ---------------------------------------------------------------*/
void CPointPDFGaussian::getCovarianceAndMean(
        CMatrixDouble33& C, CPoint3D& p) const
{
        p = mean;
        C = cov;
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void CPointPDFGaussian::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                out << CPoint3D(mean) << cov;
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void CPointPDFGaussian::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        in >> mean;

                        CMatrix c;
                        in >> c;
                        cov = c.cast<double>();
                }
                break;
                case 1:
                {
                        in >> mean >> cov;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

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

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

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

  ---------------------------------------------------------------*/
void CPointPDFGaussian::saveToTextFile(const std::string& file) const
{
        MRPT_START

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

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

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

        os::fclose(f);

        MRPT_END
}

/*---------------------------------------------------------------
                                                changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFGaussian::changeCoordinatesReference(
        const CPose3D& newReferenceBase)
{
        const CMatrixDouble33& M = newReferenceBase.getRotationMatrix();

        // The mean:
        mean = newReferenceBase + mean;

        // The covariance:
        M.multiply_HCHt(CMatrixDouble33(cov), cov);  // save in cov
}

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

        CMatrixDouble x1(3, 1), x2(3, 1), x(3, 1);
        CMatrixDouble C1(p1.cov);
        CMatrixDouble C2(p2.cov);
        CMatrixDouble C1_inv = C1.inv();
        CMatrixDouble C2_inv = C2.inv();
        CMatrixDouble C;

        x1(0, 0) = p1.mean.x();
        x1(1, 0) = p1.mean.y();
        x1(2, 0) = p1.mean.z();
        x2(0, 0) = p2.mean.x();
        x2(1, 0) = p2.mean.y();
        x2(2, 0) = p2.mean.z();

        C = (C1_inv + C2_inv).inv();
        cov = C;

        x = C * (C1_inv * x1 + C2_inv * x2);

        mean.x(x(0, 0));
        mean.y(x(1, 0));
        mean.z(x(2, 0));

        //      std::cout << "IN1: " << x1 << "\n" << C1 << "\n";
        //      std::cout << "IN2: " << x2 << "\n" << C2 << "\n";
        //      std::cout << "OUT: " << x << "\n" << C << "\n";

        MRPT_END
}

/*---------------------------------------------------------------
                                        productIntegralWith
 ---------------------------------------------------------------*/
double CPointPDFGaussian::productIntegralWith(const CPointPDFGaussian& 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
        // ---------------------------------------------------------------
        CMatrixDouble33 C = cov;
        C += p.cov;  // Sum of covs:
        CMatrixDouble33 C_inv;
        C.inv(C_inv);

        CMatrixDouble31 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();
        MU.get_unsafe(2, 0) = mean.z() - p.mean.z();

        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
}

/*---------------------------------------------------------------
                                        productIntegralWith2D
 ---------------------------------------------------------------*/
double CPointPDFGaussian::productIntegralWith2D(
        const CPointPDFGaussian& 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.block(0, 0, 2, 2);
        C += p.cov.block(0, 0, 2, 2);  // 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)) *
                   (1.0 / std::sqrt(C.det())) *
                   exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));

        MRPT_END
}

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

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

/*---------------------------------------------------------------
                                        drawSingleSample
 ---------------------------------------------------------------*/
void CPointPDFGaussian::drawSingleSample(CPoint3D& outSample) const
{
        MRPT_START

        CVectorDouble vec;
        getRandomGenerator().drawGaussianMultivariate(vec, cov);

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

        MRPT_END
}

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

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

        THROW_EXCEPTION("TODO!!!");

        MRPT_END
}

/*---------------------------------------------------------------
                                        mahalanobisDistanceTo
 ---------------------------------------------------------------*/
double CPointPDFGaussian::mahalanobisDistanceTo(
        const CPointPDFGaussian& other, bool only_2D) const
{
        // The difference in means:
        CMatrixDouble13 deltaX;
        deltaX.get_unsafe(0, 0) = other.mean.x() - mean.x();
        deltaX.get_unsafe(0, 1) = other.mean.y() - mean.y();
        deltaX.get_unsafe(0, 2) = other.mean.z() - mean.z();

        // The inverse of the combined covs:
        CMatrixDouble33 COV = other.cov;
        COV += this->cov;

        if (!only_2D)
        {
                CMatrixDouble33 COV_inv;
                COV.inv(COV_inv);
                return sqrt(deltaX.multiply_HCHt_scalar(COV_inv));
        }
        else
        {
                CMatrixDouble22 C = COV.block(0, 0, 2, 2);
                CMatrixDouble22 COV_inv;
                C.inv(COV_inv);
                CMatrixDouble12 deltaX2 = deltaX.block(0, 0, 1, 2);
                return std::sqrt(deltaX2.multiply_HCHt_scalar(COV_inv));
        }
}