CPointPDFSOG.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/CPointPDFSOG.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/random.h>
#include <mrpt/math/distributions.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/system/os.h>

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

IMPLEMENTS_SERIALIZABLE(CPointPDFSOG, CPosePDF, mrpt::poses)

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPointPDFSOG::CPointPDFSOG(size_t nModes) : m_modes(nModes) {}
/*---------------------------------------------------------------
                        clear
  ---------------------------------------------------------------*/
void CPointPDFSOG::clear() { m_modes.clear(); }
/*---------------------------------------------------------------
        Resize
  ---------------------------------------------------------------*/
void CPointPDFSOG::resize(const size_t N) { m_modes.resize(N); }
/*---------------------------------------------------------------
                                                getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMean(CPoint3D& p) const
{
        size_t N = m_modes.size();
        double X = 0, Y = 0, Z = 0;

        if (N)
        {
                CListGaussianModes::const_iterator it;
                double sumW = 0;

                for (it = m_modes.begin(); it != m_modes.end(); ++it)
                {
                        double w;
                        sumW += w = exp(it->log_w);
                        X += it->val.mean.x() * w;
                        Y += it->val.mean.y() * w;
                        Z += it->val.mean.z() * w;
                }
                if (sumW > 0)
                {
                        X /= sumW;
                        Y /= sumW;
                        Z /= sumW;
                }
        }

        p.x(X);
        p.y(Y);
        p.z(Z);
}

/*---------------------------------------------------------------
                                                getCovarianceAndMean
 ---------------------------------------------------------------*/
void CPointPDFSOG::getCovarianceAndMean(
        CMatrixDouble33& estCov, CPoint3D& p) const
{
        size_t N = m_modes.size();

        getMean(p);
        estCov.zeros();

        if (N)
        {
                // 1) Get the mean:
                double sumW = 0;
                CMatrixDouble31 estMean = CMatrixDouble31(p);

                CListGaussianModes::const_iterator it;

                CMatrixDouble33 partCov;

                for (it = m_modes.begin(); it != m_modes.end(); ++it)
                {
                        double w;
                        sumW += w = exp(it->log_w);

                        // estCov += w * ( it->val.cov +
                        // ((estMean_i-estMean)*(~(estMean_i-estMean))) );
                        CMatrixDouble31 estMean_i = CMatrixDouble31(it->val.mean);
                        estMean_i -= estMean;
                        partCov.multiply_AAt(estMean_i);
                        partCov += it->val.cov;
                        partCov *= w;
                        estCov += partCov;
                }

                if (sumW != 0) estCov *= (1.0 / sumW);
        }
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void CPointPDFSOG::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                uint32_t N = m_modes.size();
                CListGaussianModes::const_iterator it;

                out << N;

                for (it = m_modes.begin(); it != m_modes.end(); ++it)
                {
                        out << it->log_w;
                        out << it->val.mean;
                        out << it->val.cov(0, 0) << it->val.cov(1, 1) << it->val.cov(2, 2);
                        out << it->val.cov(0, 1) << it->val.cov(0, 2) << it->val.cov(1, 2);
                }
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void CPointPDFSOG::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                {
                        uint32_t N;
                        CListGaussianModes::iterator it;
                        double x;

                        in >> N;

                        this->resize(N);

                        for (it = m_modes.begin(); it != m_modes.end(); ++it)
                        {
                                in >> it->log_w;

                                // In version 0, weights were linear!!
                                if (version == 0) it->log_w = log(max(1e-300, it->log_w));

                                in >> it->val.mean;

                                in >> x;
                                it->val.cov(0, 0) = x;
                                in >> x;
                                it->val.cov(1, 1) = x;
                                in >> x;
                                it->val.cov(2, 2) = x;

                                in >> x;
                                it->val.cov(1, 0) = x;
                                it->val.cov(0, 1) = x;
                                in >> x;
                                it->val.cov(2, 0) = x;
                                it->val.cov(0, 2) = x;
                                in >> x;
                                it->val.cov(1, 2) = x;
                                it->val.cov(2, 1) = x;
                        }
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

void CPointPDFSOG::copyFrom(const CPointPDF& o)
{
        MRPT_START

        if (this == &o) return;  // It may be used sometimes

        if (o.GetRuntimeClass() == CLASS_ID(CPointPDFSOG))
        {
                m_modes = static_cast<const CPointPDFSOG*>(&o)->m_modes;
        }
        else
        {
                // Approximate as a mono-modal gaussian pdf:
                this->resize(1);
                m_modes[0].log_w = 0;
                o.getCovarianceAndMean(m_modes[0].val.cov, m_modes[0].val.mean);
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                                saveToTextFile
  ---------------------------------------------------------------*/
void CPointPDFSOG::saveToTextFile(const std::string& file) const
{
        FILE* f = os::fopen(file.c_str(), "wt");
        if (!f) return;

        for (CListGaussianModes::const_iterator it = m_modes.begin();
                 it != m_modes.end(); ++it)
                os::fprintf(
                        f, "%e %e %e %e %e %e %e %e %e %e\n", exp(it->log_w),
                        it->val.mean.x(), it->val.mean.y(), it->val.mean.z(),
                        it->val.cov(0, 0), it->val.cov(1, 1), it->val.cov(2, 2),
                        it->val.cov(0, 1), it->val.cov(0, 2), it->val.cov(1, 2));
        os::fclose(f);
}

/*---------------------------------------------------------------
                                                changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFSOG::changeCoordinatesReference(const CPose3D& newReferenceBase)
{
        for (CListGaussianModes::iterator it = m_modes.begin(); it != m_modes.end();
                 ++it)
                it->val.changeCoordinatesReference(newReferenceBase);
}

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

        ASSERT_(m_modes.size() > 0);

        // 1st: Select a mode with a probability proportional to its weight:
        vector<double> logWeights(m_modes.size());
        vector<size_t> outIdxs;
        vector<double>::iterator itW;
        CListGaussianModes::const_iterator it;
        for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
                 ++it, ++itW)
                *itW = it->log_w;

        CParticleFilterCapable::computeResampling(
                CParticleFilter::prMultinomial,  // Resampling algorithm
                logWeights,  // input: log weights
                outIdxs  // output: indexes
                );

        // we need just one: take the first (arbitrary)
        size_t selectedIdx = outIdxs[0];
        ASSERT_(selectedIdx < m_modes.size());
        const CPointPDFGaussian* selMode = &m_modes[selectedIdx].val;

        // 2nd: Draw a position from the selected Gaussian:
        CVectorDouble vec;
        getRandomGenerator().drawGaussianMultivariate(vec, selMode->cov);

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

        MRPT_END
}

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

        // p1: CPointPDFSOG, p2: CPosePDFGaussian:

        ASSERT_(p1_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));
        ASSERT_(p2_.GetRuntimeClass() == CLASS_ID(CPointPDFSOG));

        const CPointPDFSOG* p1 = static_cast<const CPointPDFSOG*>(&p1_);
        const CPointPDFSOG* p2 = static_cast<const CPointPDFSOG*>(&p2_);

        // Compute the new kernel means, covariances, and weights after multiplying
        // to the Gaussian "p2":
        CPointPDFGaussian auxGaussianProduct, auxSOG_Kernel_i;

        float minMahalanobisDistToDrop2 = square(minMahalanobisDistToDrop);

        this->m_modes.clear();
        bool is2D =
                false;  // to detect & avoid errors in 3x3 matrix inversions of range=2.

        for (CListGaussianModes::const_iterator it1 = p1->m_modes.begin();
                 it1 != p1->m_modes.end(); ++it1)
        {
                CMatrixDouble33 c = it1->val.cov;

                // Is a 2D covariance??
                if (c.get_unsafe(2, 2) == 0)
                {
                        is2D = true;
                        c.set_unsafe(2, 2, 1);
                }

                ASSERT_(c(0, 0) != 0 && c(0, 0) != 0)

                CMatrixDouble33 covInv(UNINITIALIZED_MATRIX);
                c.inv(covInv);

                CMatrixDouble31 eta = covInv * CMatrixDouble31(it1->val.mean);

                // Normal distribution canonical form constant:
                // See: http://www-static.cc.gatech.edu/~wujx/paper/Gaussian.pdf
                double a = -0.5 * (3 * log(M_2PI) - log(covInv.det()) +
                                                   eta.multiply_HtCH_scalar(
                                                           c));  // (~eta * (*it1).val.cov * eta)(0,0) );

                for (CListGaussianModes::const_iterator it2 = p2->m_modes.begin();
                         it2 != p2->m_modes.end(); ++it2)
                {
                        auxSOG_Kernel_i = (*it2).val;
                        if (auxSOG_Kernel_i.cov.get_unsafe(2, 2) == 0)
                        {
                                auxSOG_Kernel_i.cov.set_unsafe(2, 2, 1);
                                is2D = true;
                        }
                        ASSERT_(
                                auxSOG_Kernel_i.cov(0, 0) > 0 && auxSOG_Kernel_i.cov(1, 1) > 0)

                        // Should we drop this product term??
                        bool reallyComputeThisOne = true;
                        if (minMahalanobisDistToDrop > 0)
                        {
                                // Approximate (fast) mahalanobis distance (square):
                                float mahaDist2;

                                float stdX2 =
                                        max(auxSOG_Kernel_i.cov.get_unsafe(0, 0),
                                                (*it1).val.cov.get_unsafe(0, 0));
                                mahaDist2 =
                                        square(auxSOG_Kernel_i.mean.x() - (*it1).val.mean.x()) /
                                        stdX2;

                                float stdY2 =
                                        max(auxSOG_Kernel_i.cov.get_unsafe(1, 1),
                                                (*it1).val.cov.get_unsafe(1, 1));
                                mahaDist2 +=
                                        square(auxSOG_Kernel_i.mean.y() - (*it1).val.mean.y()) /
                                        stdY2;

                                if (!is2D)
                                {
                                        float stdZ2 =
                                                max(auxSOG_Kernel_i.cov.get_unsafe(2, 2),
                                                        (*it1).val.cov.get_unsafe(2, 2));
                                        mahaDist2 +=
                                                square(auxSOG_Kernel_i.mean.z() - (*it1).val.mean.z()) /
                                                stdZ2;
                                }

                                reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
                        }

                        if (reallyComputeThisOne)
                        {
                                auxGaussianProduct.bayesianFusion(auxSOG_Kernel_i, (*it1).val);

                                // ----------------------------------------------------------------------
                                // The new weight is given by:
                                //
                                //   w'_i = w_i * exp( a + a_i - a' )
                                //
                                //      a = -1/2 ( dimensionality * log(2pi) - log(det(Cov^-1))
                                //      + (Cov^-1 * mu)^t * Cov^-1 * (Cov^-1 * mu) )
                                //
                                // ----------------------------------------------------------------------
                                TGaussianMode newKernel;

                                newKernel.val = auxGaussianProduct;  // Copy mean & cov

                                CMatrixDouble33 covInv_i = auxSOG_Kernel_i.cov.inv();
                                CMatrixDouble31 eta_i = CMatrixDouble31(auxSOG_Kernel_i.mean);
                                eta_i = covInv_i * eta_i;

                                CMatrixDouble33 new_covInv_i = newKernel.val.cov.inv();
                                CMatrixDouble31 new_eta_i = CMatrixDouble31(newKernel.val.mean);
                                new_eta_i = new_covInv_i * new_eta_i;

                                double a_i =
                                        -0.5 *
                                        (3 * log(M_2PI) - log(new_covInv_i.det()) +
                                         (eta_i.adjoint() * auxSOG_Kernel_i.cov * eta_i)(0, 0));
                                double new_a_i =
                                        -0.5 * (3 * log(M_2PI) - log(new_covInv_i.det()) +
                                                        (new_eta_i.adjoint() * newKernel.val.cov *
                                                         new_eta_i)(0, 0));

                                newKernel.log_w =
                                        (it1)->log_w + (it2)->log_w + a + a_i - new_a_i;

                                // Fix 2D case:
                                if (is2D) newKernel.val.cov(2, 2) = 0;

                                // Add to the results (in "this") the new kernel:
                                this->m_modes.push_back(newKernel);
                        }  // end if reallyComputeThisOne
                }  // end for it2

        }  // end for it1

        normalizeWeights();

        MRPT_END
}

/*---------------------------------------------------------------
                                                assureSymmetry
 ---------------------------------------------------------------*/
void CPointPDFSOG::assureSymmetry()
{
        MRPT_START
        // Differences, when they exist, appear in the ~15'th significant
        //  digit, so... just take one of them arbitrarily!
        for (CListGaussianModes::iterator it = m_modes.begin(); it != m_modes.end();
                 ++it)
        {
                it->val.cov(0, 1) = it->val.cov(1, 0);
                it->val.cov(0, 2) = it->val.cov(2, 0);
                it->val.cov(1, 2) = it->val.cov(2, 1);
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                                normalizeWeights
 ---------------------------------------------------------------*/
void CPointPDFSOG::normalizeWeights()
{
        MRPT_START

        if (!m_modes.size()) return;

        CListGaussianModes::iterator it;
        double maxW = m_modes[0].log_w;
        for (it = m_modes.begin(); it != m_modes.end(); ++it)
                maxW = max(maxW, it->log_w);

        for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;

        MRPT_END
}

/*---------------------------------------------------------------
                                                ESS
 ---------------------------------------------------------------*/
double CPointPDFSOG::ESS() const
{
        MRPT_START
        CListGaussianModes::const_iterator it;
        double cum = 0;

        /* Sum of weights: */
        double sumLinearWeights = 0;
        for (it = m_modes.begin(); it != m_modes.end(); ++it)
                sumLinearWeights += exp(it->log_w);

        /* Compute ESS: */
        for (it = m_modes.begin(); it != m_modes.end(); ++it)
                cum += square(exp(it->log_w) / sumLinearWeights);

        if (cum == 0)
                return 0;
        else
                return 1.0 / (m_modes.size() * cum);
        MRPT_END
}

/*---------------------------------------------------------------
                                                evaluatePDFInArea
 ---------------------------------------------------------------*/
void CPointPDFSOG::evaluatePDFInArea(
        float x_min, float x_max, float y_min, float y_max, float resolutionXY,
        float z, CMatrixD& outMatrix, bool sumOverAllZs)
{
        MRPT_START

        ASSERT_(x_max > x_min);
        ASSERT_(y_max > y_min);
        ASSERT_(resolutionXY > 0);

        const size_t Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
        const size_t Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
        outMatrix.setSize(Ny, Nx);

        for (size_t i = 0; i < Ny; i++)
        {
                const float y = y_min + i * resolutionXY;
                for (size_t j = 0; j < Nx; j++)
                {
                        float x = x_min + j * resolutionXY;
                        outMatrix(i, j) = evaluatePDF(CPoint3D(x, y, z), sumOverAllZs);
                }
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                                evaluatePDF
 ---------------------------------------------------------------*/
double CPointPDFSOG::evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const
{
        if (!sumOverAllZs)
        {
                // Normal evaluation:
                CMatrixDouble31 X = CMatrixDouble31(x);
                double ret = 0;

                CMatrixDouble31 MU;

                for (CListGaussianModes::const_iterator it = m_modes.begin();
                         it != m_modes.end(); ++it)
                {
                        MU = CMatrixDouble31(it->val.mean);
                        ret += exp(it->log_w) * math::normalPDF(X, MU, it->val.cov);
                }

                return ret;
        }
        else
        {
                // Only X,Y:
                CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
                double ret = 0;

                X(0, 0) = x.x();
                X(1, 0) = x.y();

                for (CListGaussianModes::const_iterator it = m_modes.begin();
                         it != m_modes.end(); ++it)
                {
                        MU(0, 0) = it->val.mean.x();
                        MU(1, 0) = it->val.mean.y();

                        COV(0, 0) = it->val.cov(0, 0);
                        COV(1, 1) = it->val.cov(1, 1);
                        COV(0, 1) = COV(1, 0) = it->val.cov(0, 1);

                        ret += exp(it->log_w) * math::normalPDF(X, MU, COV);
                }

                return ret;
        }
}

/*---------------------------------------------------------------
                                                getMostLikelyMode
 ---------------------------------------------------------------*/
void CPointPDFSOG::getMostLikelyMode(CPointPDFGaussian& outVal) const
{
        if (this->empty())
        {
                outVal = CPointPDFGaussian();
        }
        else
        {
                const_iterator it_best = m_modes.end();
                for (const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
                        if (it_best == m_modes.end() || it->log_w > it_best->log_w)
                                it_best = it;

                outVal = it_best->val;
        }
}

/*---------------------------------------------------------------
                                                getAs3DObject
 ---------------------------------------------------------------*/
// void  CPointPDFSOG::getAs3DObject( mrpt::opengl::CSetOfObjects::Ptr  &outObj
// )
// const
//{
//      // For each gaussian node
//      for (CListGaussianModes::const_iterator it = m_modes.begin(); it!=
// m_modes.end();++it)
//      {
//              opengl::CEllipsoid::Ptr obj =
// mrpt::make_aligned_shared<opengl::CEllipsoid>();
//
//              obj->setPose( it->val.mean);
//              obj->setCovMatrix(it->val.cov,  it->val.cov(2,2)==0  ?  2:3);
//
//              obj->setQuantiles(3);
//              obj->enableDrawSolid3D(false);
//              obj->setColor(1,0,0, 0.5);
//
//              outObj->insert( obj );
//      } // end for each gaussian node
//}