CPose3DPDFParticles.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/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/poses/SO_SE_average.h>
#include <mrpt/system/os.h>

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

IMPLEMENTS_SERIALIZABLE( CPose3DPDFParticles, CPose3DPDF, mrpt::poses )

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFParticles::CPose3DPDFParticles( size_t M )
{
        m_particles.resize(M);

        for (auto &it : m_particles)
        {
                it.log_w = .0;
                it.d.reset(new CPose3D());
        }

        CPose3D nullPose(0,0,0);
        resetDeterministic( nullPose );
}

void  CPose3DPDFParticles::copyFrom(const CPose3DPDF &o)
{
        MRPT_START

        CPose3DPDFParticles::CParticleList::const_iterator      itSrc;
        CPose3DPDFParticles::CParticleList::iterator            itDest;

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

        if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFParticles))
        {
                const CPose3DPDFParticles       *pdf = dynamic_cast<const CPose3DPDFParticles*>( &o );
                ASSERT_(pdf);

                m_particles = pdf->m_particles;
        }
        else
        if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFGaussian))
        {
                THROW_EXCEPTION("TO DO!!");
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                                getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed
                as a weighted average over all m_particles.
 ---------------------------------------------------------------*/
void CPose3DPDFParticles::getMean(CPose3D &p) const
{
        MRPT_START

        // Default to (0,..,0)
        p = CPose3D();
        if (m_particles.empty())
                return;

        // Calc average on SE(3)
        mrpt::poses::SE_average<3> se_averager;
        for (CPose3DPDFParticles::CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                const double w  = exp(it->log_w);
                se_averager.append( *it->d,w );
        }
        se_averager.get_average(p);

        MRPT_END
}

/*---------------------------------------------------------------
                                                getCovarianceAndMean
  ---------------------------------------------------------------*/
void CPose3DPDFParticles::getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean) const
{
        MRPT_START

        getMean(mean);  // First! the mean value:

        // Now the covariance:
        cov.zeros();
        CVectorDouble   vars; vars.assign(6,0.0);       // The diagonal of the final covariance matrix

        // Elements off the diagonal of the covariance matrix:
        double                                                                                  std_xy = 0,std_xz = 0,std_xya= 0,std_xp = 0,std_xr = 0;
        double                                                                                  std_yz = 0,std_yya = 0,std_yp = 0,std_yr = 0;
        double                                                                                  std_zya = 0,std_zp = 0,std_zr = 0;
        double                                                                                  std_yap = 0,std_yar = 0;
        double                                                                                  std_pr = 0;

        // Mean values in [0, 2pi] range:
        double          mean_yaw = mean.yaw();
        double          mean_pitch = mean.pitch();
        double          mean_roll = mean.roll();
        if (mean_yaw<0) mean_yaw += M_2PI;
        if (mean_pitch<0) mean_pitch += M_2PI;
        if (mean_roll<0) mean_roll += M_2PI;

        // Enought information to estimate the covariance?
        if (m_particles.size()<2)       return;


        // Sum all weight values:
        double          W = 0;
        for (CPose3DPDFParticles::CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
                W += exp(it->log_w);

        ASSERT_(W>0);

        // Compute covariance:
        for (CPose3DPDFParticles::CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it)
        {
                double w = exp( it->log_w ) / W;

                // Manage 1 PI range:
                double  err_yaw   = wrapToPi( fabs(it->d->yaw() - mean_yaw) );
                double  err_pitch = wrapToPi( fabs(it->d->pitch() - mean_pitch) );
                double  err_roll  = wrapToPi( fabs(it->d->roll() - mean_roll) );

                double  err_x     = it->d->x() - mean.x();
                double  err_y     = it->d->y() - mean.y();
                double  err_z     = it->d->z() - mean.z();

                vars[0] += square(err_x)*w;
                vars[1] += square(err_y)*w;
                vars[2] += square(err_z)*w;
                vars[3] += square(err_yaw)*w;
                vars[4] += square(err_pitch)*w;
                vars[5] += square(err_roll)*w;

                std_xy  += err_x*err_y * w;
                std_xz  += err_x*err_z * w;
                std_xya += err_x*err_yaw * w;
                std_xp  += err_x*err_pitch * w;
                std_xr  += err_x*err_roll * w;

                std_yz  += err_y*err_z * w;
                std_yya += err_y*err_yaw * w;
                std_yp  += err_y*err_pitch * w;
                std_yr  += err_y*err_roll * w;

                std_zya += err_z*err_yaw * w;
                std_zp  += err_z*err_pitch * w;
                std_zr  += err_z*err_roll * w;

                std_yap += err_yaw*err_pitch * w;
                std_yar += err_yaw*err_roll * w;

                std_pr  += err_pitch*err_roll * w;
        } // end for it

        // Unbiased estimation of variance:
        cov(0,0) = vars[0];
        cov(1,1) = vars[1];
        cov(2,2) = vars[2];
        cov(3,3) = vars[3];
        cov(4,4) = vars[4];
        cov(5,5) = vars[5];

        cov(1,0) = cov(0,1) = std_xy;
        cov(2,0) = cov(0,2) = std_xz;
        cov(3,0) = cov(0,3) = std_xya;
        cov(4,0) = cov(0,4) = std_xp;
        cov(5,0) = cov(0,5) = std_xr;

        cov(2,1) = cov(1,2) = std_yz;
        cov(3,1) = cov(1,3) = std_yya;
        cov(4,1) = cov(1,4) = std_yp;
        cov(5,1) = cov(1,5) = std_yr;

        cov(3,2) = cov(2,3) = std_zya;
        cov(4,2) = cov(2,4) = std_zp;
        cov(5,2) = cov(2,5) = std_zr;

        cov(4,3) = cov(3,4) = std_yap;
        cov(5,3) = cov(3,5) = std_yar;

        cov(5,4) = cov(4,5) = std_pr;


        MRPT_END
}


/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void  CPose3DPDFParticles::writeToStream(mrpt::utils::CStream &out,int *version) const
{
        if (version)
                *version = 0;
        else
        {
                writeParticlesToStream(out);
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void  CPose3DPDFParticles::readFromStream(mrpt::utils::CStream &in, int version)
{
        switch(version)
        {
        case 0:
                {
                        readParticlesFromStream(in);
                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };
}

/*---------------------------------------------------------------
                                                saveToTextFile
   Save PDF's m_particles to a text file. In each line it
      will go: "x y phi weight"
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::saveToTextFile(const std::string &file) const
{
        using namespace mrpt::system;

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

        os::fprintf(f,"%% x  y  z  yaw[rad] pitch[rad] roll[rad] log_weight\n");

        for (const auto &p : m_particles)
                os::fprintf(f,"%f %f %f %f %f %f %e\n",
                                p.d->x(),
                                p.d->y(),
                                p.d->z(),
                                p.d->yaw(),
                                p.d->pitch(),
                                p.d->roll(),
                                p.log_w );

        os::fclose(f);
}


/*---------------------------------------------------------------
                                                getParticlePose
 ---------------------------------------------------------------*/
CPose3D  CPose3DPDFParticles::getParticlePose(int i) const
{
        return *m_particles[i].d;
}

/*---------------------------------------------------------------
                                                changeCoordinatesReference
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::changeCoordinatesReference( const CPose3D &newReferenceBase )
{
        for (CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
                it->d->composeFrom(newReferenceBase, *it->d);
}

/*---------------------------------------------------------------
                                        drawSingleSample
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::drawSingleSample( CPose3D &outPart ) const
{
        MRPT_UNUSED_PARAM(outPart);
        THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
                                        drawManySamples
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::drawManySamples(
        size_t                                          N,
        std::vector<CVectorDouble>      &outSamples ) const
{
        MRPT_UNUSED_PARAM(N); MRPT_UNUSED_PARAM(outSamples);
        THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
                                                +=
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::operator += ( const CPose3D &Ap)
{
        MRPT_UNUSED_PARAM(Ap);
        THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
                                        append
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::append( CPose3DPDFParticles &o )
{
        MRPT_UNUSED_PARAM(o);
        THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
                                        inverse
 ---------------------------------------------------------------*/
void CPose3DPDFParticles::inverse(CPose3DPDF  &o) const
{
        MRPT_START
        ASSERT_( o.GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles) );
        CPose3DPDFParticles * out = static_cast<CPose3DPDFParticles*> (&o);

        // Prepare the output:
        out->copyFrom(*this);

        CPose3DPDFParticles::CParticleList::iterator it;
        CPose3D   zero(0,0,0);

        for (it=out->m_particles.begin();it!=out->m_particles.end();++it)
                *it->d = zero - *it->d;

        MRPT_END
}

/*---------------------------------------------------------------
                                        getMostLikelyParticle
 ---------------------------------------------------------------*/
CPose3D  CPose3DPDFParticles::getMostLikelyParticle() const
{
        CPose3DPDFParticles::CParticleList::const_iterator      it, itMax=m_particles.begin();
        double          max_w = -1e300;

        for (it=m_particles.begin();it!=m_particles.end();++it)
        {
                if (it->log_w > max_w)
                {
                        itMax = it;
                        max_w = it->log_w;
                }
        }

        return *itMax->d;
}

/*---------------------------------------------------------------
                                        bayesianFusion
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::bayesianFusion( const CPose3DPDF &p1,const  CPose3DPDF &p2 )
{
        MRPT_UNUSED_PARAM(p1);MRPT_UNUSED_PARAM(p2);
        THROW_EXCEPTION("Not implemented yet!");
}


/*---------------------------------------------------------------
                                                        resetDeterministic
        Reset PDF to a single point and set the number of m_particles
 ---------------------------------------------------------------*/
void  CPose3DPDFParticles::resetDeterministic( const CPose3D &location,
                                                                                                size_t  particlesCount)
{
        CPose3DPDFParticles::CParticleList::iterator            it;

        if (particlesCount>0)
        {
                clearParticles();
                m_particles.resize(particlesCount);
                for (it = m_particles.begin(); it != m_particles.end(); ++it)
                        it->d.reset(new CPose3D());
        }

        for (it=m_particles.begin();it!=m_particles.end();++it)
        {
                *it->d  = location;
                it->log_w       = 0;
        }
}