CPointPDFParticles.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/utils/CStream.h>
#include <mrpt/poses/CPointPDFParticles.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/ops_containers.h>  // maximum()
#include <mrpt/system/os.h>

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

IMPLEMENTS_SERIALIZABLE( CPointPDFParticles, CPointPDF, mrpt::poses )
IMPLEMENTS_SERIALIZABLE( TSimple3DPoint, CSerializable, mrpt::poses )

CPointPDFParticles::CPointPDFParticles(size_t numParticles)
{
        setSize(numParticles);
}

/** Clear all the particles (free memory) */
void CPointPDFParticles::clear() {
        setSize(0);
}

/*---------------------------------------------------------------
                setSize
  ---------------------------------------------------------------*/
void CPointPDFParticles::setSize(
        size_t  numberParticles,
        const   CPoint3D &defaultValue)
{
        // Free old particles: automatic via smart ptr
        m_particles.resize(numberParticles);
        for (auto &it : m_particles)
        {
                it.log_w = 0;
                it.d.reset(new TSimple3DPoint(defaultValue));
        }
}

/*---------------------------------------------------------------
                                                getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
 ---------------------------------------------------------------*/
void CPointPDFParticles::getMean(CPoint3D &p) const
{
        MRPT_START
        if (m_particles.empty())
                THROW_EXCEPTION("Cannot compute mean since there are zero particles.")

        CParticleList::const_iterator   it;
        double          sumW=0;
        double          x=0,y=0,z=0;
        for (it=m_particles.begin();it!=m_particles.end();it++)
        {
                const double w = exp(it->log_w);
                x+=it->d->x*w;
                y+=it->d->y*w;
                z+=it->d->z*w;
                sumW+=w;
        }

        ASSERT_(sumW!=0)

        sumW = 1.0/sumW;

        p.x( x*sumW );
        p.y( y*sumW );
        p.z( z*sumW );

        MRPT_END
}

/*---------------------------------------------------------------
                                                getEstimatedCovariance
 ---------------------------------------------------------------*/
void CPointPDFParticles::getCovarianceAndMean(CMatrixDouble33 &cov, CPoint3D &mean) const
{
        MRPT_START

        getMean(mean);
        cov.zeros();

        size_t          i,n = m_particles.size();
        double          var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0;

        double lin_w_sum = 0;

        for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w );
        if (lin_w_sum==0) lin_w_sum=1;

        for (i=0;i<n;i++)
        {
                double w = exp( m_particles[i].log_w ) / lin_w_sum;

                double  err_x   = m_particles[i].d->x - mean.x();
                double  err_y   = m_particles[i].d->y - mean.y();
                double  err_phi = m_particles[i].d->z - mean.z();

                var_x+= square(err_x)*w;
                var_y+= square(err_y)*w;
                var_p+= square(err_phi)*w;
                var_xy+= err_x*err_y*w;
                var_xp+= err_x*err_phi*w;
                var_yp+= err_y*err_phi*w;
        }

        if (n>=2)
        {
                // Unbiased estimation of variance:
                cov(0,0) = var_x;
                cov(1,1) = var_y;
                cov(2,2) = var_p;

                cov(1,0) = cov(0,1) = var_xy;
                cov(2,0) = cov(0,2) = var_xp;
                cov(1,2) = cov(2,1) = var_yp;
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void  CPointPDFParticles::writeToStream(mrpt::utils::CStream &out,int *version) const
{
        if (version)
                *version = 0;
        else
        {
                uint32_t        N = size();
                out << N;

                for (CParticleList::const_iterator      it=m_particles.begin();it!=m_particles.end();++it)
                        out << it->log_w << it->d->x << it->d->y << it->d->z;
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void  CPointPDFParticles::readFromStream(mrpt::utils::CStream &in,int version)
{
        switch(version)
        {
        case 0:
                {
                        uint32_t        N;
                        in >> N;
                        setSize(N);

                        for (CParticleList::iterator    it=m_particles.begin();it!=m_particles.end();++it)
                                in >> it->log_w >> it->d->x >> it->d->y >> it->d->z;
                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };
}

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

        // Convert to samples:
        THROW_EXCEPTION("NO");
}

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

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

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

        size_t i,N = m_particles.size();
        for (i=0;i<N;i++)
                os::fprintf(f,"%f %f %f %e\n", m_particles[i].d->x,m_particles[i].d->y,m_particles[i].d->z,m_particles[i].log_w);

        os::fclose(f);

        MRPT_END
}

/*---------------------------------------------------------------
                                                changeCoordinatesReference
 ---------------------------------------------------------------*/
void  CPointPDFParticles::changeCoordinatesReference(const CPose3D &newReferenceBase )
{
        TPoint3D pt;
        for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                newReferenceBase.composePoint(
                        it->d->x, it->d->y, it->d->z,  // In
                        pt.x, pt.y, pt.z // Out
                        );
                it->d->x = pt.x;
                it->d->y = pt.y;
                it->d->z = pt.z;
        }
}

/*---------------------------------------------------------------
                                                computeKurtosis
 ---------------------------------------------------------------*/
double CPointPDFParticles::computeKurtosis()
{
        MRPT_START

        // kurtosis = \mu^4 / (\sigma^2) -3
        CVectorDouble kurts, mu4,m,var;
        kurts.assign(3,.0);
        mu4.assign(3,.0);
        m.assign(3,.0);
        var.assign(3,.0);

        // Means:
        for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                m[0]+=it->d->x;
                m[1]+=it->d->y;
                m[2]+=it->d->z;
        }
        m*=1.0/m_particles.size();

        // variances:
        for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                var[0]+=square(it->d->x-m[0]);
                var[1]+=square(it->d->y-m[1]);
                var[2]+=square(it->d->z-m[2]);
        }
        var*=1.0/m_particles.size();
        var[0]=square(var[0]);
        var[1]=square(var[1]);
        var[2]=square(var[2]);

        // Moment:
        for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                mu4[0]+=pow(it->d->x-m[0],4.0);
                mu4[1]+=pow(it->d->y-m[1],4.0);
                mu4[2]+=pow(it->d->z-m[2],4.0);
        }
        mu4*=1.0/m_particles.size();

        // Kurtosis's
        kurts.array() = mu4.array() / var.array();

        return math::maximum(kurts);

        MRPT_END
}


/*---------------------------------------------------------------
                                        drawSingleSample
  ---------------------------------------------------------------*/
void CPointPDFParticles::drawSingleSample(CPoint3D &outSample) const
{
        MRPT_UNUSED_PARAM(outSample);
        THROW_EXCEPTION("TO DO!")
}

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

        THROW_EXCEPTION("TODO!!!");

        MRPT_END
}


/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void  TSimple3DPoint::writeToStream(mrpt::utils::CStream &out,int *version) const
{
        MRPT_UNUSED_PARAM(out); MRPT_UNUSED_PARAM(version);
        THROW_EXCEPTION("Shouldn't arrive here!");
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void  TSimple3DPoint::readFromStream(mrpt::utils::CStream &in,int version)
{
        MRPT_UNUSED_PARAM(in); MRPT_UNUSED_PARAM(version);
        THROW_EXCEPTION("Shouldn't arrive here!");
}