CPose3DPDFSOG.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/CPose3DPDFSOG.h>
#include <mrpt/system/os.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/poses/SO_SE_average.h>

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

IMPLEMENTS_SERIALIZABLE( CPose3DPDFSOG, CPose3DPDF, mrpt::poses )

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPose3DPDFSOG::CPose3DPDFSOG( size_t nModes ) :
        m_modes(nModes)
{
}

/*---------------------------------------------------------------
                        clear
  ---------------------------------------------------------------*/
void CPose3DPDFSOG::clear()
{
        m_modes.clear();
}

/*---------------------------------------------------------------
        Resize
  ---------------------------------------------------------------*/
void CPose3DPDFSOG::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 CPose3DPDFSOG::getMean(CPose3D &p) const
{
        if (!m_modes.empty())
        {
                // Calc average on SE(3)
                mrpt::poses::SE_average<3> se_averager;
                for (const_iterator     it=m_modes.begin();it!=m_modes.end();++it)
                {
                        const double w  = exp(it->log_w);
                        se_averager.append( (it)->val.mean,w );
                }
                se_averager.get_average(p);
        }
        else
        {
                p.setFromValues(0,0,0,0,0,0);
        }
}

/*---------------------------------------------------------------
                                                getCovarianceAndMean
 ---------------------------------------------------------------*/
void CPose3DPDFSOG::getCovarianceAndMean(mrpt::math::CMatrixDouble66 &estCovOut,CPose3D &mean) const
{
        size_t          N = m_modes.size();

        getMean(mean);
        mrpt::math::CMatrixDouble66 estCov;

        if (N)
        {
                // 1) Get the mean:
                double          sumW = 0;
                mrpt::math::CMatrixDouble estMean( mean );

                mrpt::math::CMatrixDouble66             MMt;
                mrpt::math::CMatrixDouble61 estMean_i;
                for (const_iterator     it=m_modes.begin();it!=m_modes.end();++it)
                {
                    double w;
                        sumW += w = exp((it)->log_w);
                        estMean_i = mrpt::math::CMatrixDouble61((it)->val.mean);
                        MMt.multiply_AAt(estMean_i);
                        MMt+=(it)->val.cov;
                        MMt*=w;
                        estCov += MMt; //w * ( (it)->val.cov + ((estMean_i-estMean)*(~(estMean_i-estMean))) );
                }

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

        estCovOut = estCov;
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void  CPose3DPDFSOG::writeToStream(mrpt::utils::CStream &out,int *version) const
{
        if (version)
                *version = 2;
        else
        {
                uint32_t        N = m_modes.size();
                out << N;
                for (const_iterator it=m_modes.begin();it!=m_modes.end();++it)
                {
                        out << (it)->log_w;
                        out << (it)->val.mean;
                        out << (it)->val.cov;
                }
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void  CPose3DPDFSOG::readFromStream(mrpt::utils::CStream &in,int version)
{
        switch(version)
        {
        case 0:
        case 1:
        case 2:
                {
                        uint32_t                N;
                        in >> N;
                        this->resize(N);

                        for (iterator 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;


                                if (version==1) // were floats
                                {
                                        THROW_EXCEPTION("Unsupported serialized version: too old")
                                }
                                else
                                {
                                        in >> (it)->val.cov;
                                }
                        }
                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };
}

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

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

        if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFSOG))
        {
                *this = *static_cast<const CPose3DPDFSOG*>(&o);
        }
        else
        {
                this->resize(1);
                m_modes[0].log_w = 0;
                mrpt::math::CMatrixDouble66 C;
                o.getCovarianceAndMean( C, m_modes[0].val.mean );
                m_modes[0].val.cov = C;
        }

        MRPT_END
}

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


        for (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  CPose3DPDFSOG::changeCoordinatesReference(const CPose3D &newReferenceBase )
{
        for (iterator it=m_modes.begin();it!=m_modes.end();++it)
                (it)->val.changeCoordinatesReference( newReferenceBase );
}

/*---------------------------------------------------------------
                                        bayesianFusion
 ---------------------------------------------------------------*/
void  CPose3DPDFSOG::bayesianFusion(const  CPose3DPDF &p1_,const  CPose3DPDF &p2_ )
{
        MRPT_START

        // p1: CPose3DPDFSOG, p2: CPosePDFGaussian:

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

        THROW_EXCEPTION("TODO!!!");
#if 0
/*
        CPose3DPDFSOG           *p1 = (CPose3DPDFSOG*)&p1_;
        CPose3DPDFSOG           *p2 = (CPose3DPDFSOG*)&p2_;

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



        CMatrixD                                covInv( p2->cov.inv() );
        CMatrixD                                eta(3,1);
        eta(0,0) = p2->mean.x;
        eta(1,0) = p2->mean.y;
        eta(2,0) = p2->mean.phi;
        eta = covInv * eta;

        // 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 * p2->cov * eta)(0,0) );

        this->m_modes.clear();
        for (std::deque<TGaussianMode>::iterator it =p1->m_modes.begin();it!=p1->m_modes.end();++it)
        {
                auxSOG_Kernel_i.mean = it->mean;
                auxSOG_Kernel_i.cov  = it->cov;
                auxGaussianProduct.bayesianFusion( auxSOG_Kernel_i, *p2 );

                // ----------------------------------------------------------------------
                // 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) )
                //
                // ----------------------------------------------------------------------
                newKernel.mean = auxGaussianProduct.mean;
                newKernel.cov  = auxGaussianProduct.cov;

                CMatrixD                covInv_i( auxSOG_Kernel_i.cov.inv() );
                CMatrixD                eta_i(3,1);
                eta_i(0,0) = auxSOG_Kernel_i.mean.x;
                eta_i(1,0) = auxSOG_Kernel_i.mean.y;
                eta_i(2,0) = auxSOG_Kernel_i.mean.phi;
                eta_i = covInv_i * eta_i;

                CMatrixD                new_covInv_i( newKernel.cov.inv() );
                CMatrixD                new_eta_i(3,1);
                new_eta_i(0,0) = newKernel.mean.x;
                new_eta_i(1,0) = newKernel.mean.y;
                new_eta_i(2,0) = newKernel.mean.phi;
                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 * 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 * newKernel.cov * new_eta_i)(0,0) );

                newKernel.w        = it->w * exp( a + a_i - new_a_i );

                // Add to the results (in "this") the new kernel:
                this->m_modes.push_back( newKernel );
        }
*/
        normalizeWeights();
#endif
        MRPT_END
}

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

        MRPT_END
}

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

        if (m_modes.empty()) return;

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

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

        MRPT_END
}

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

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

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

        // Prepare the output SOG:
        out->resize(m_modes.size());

        const_iterator  it;
        iterator        outIt;

        for (it=m_modes.begin(),outIt=out->m_modes.begin();it!=m_modes.end();it++,outIt++)
        {
                (it)->val.inverse( (outIt)->val );
                (outIt)->log_w = (it)->log_w;
        }


        MRPT_END
}

/*---------------------------------------------------------------
                                                appendFrom
 ---------------------------------------------------------------*/
void  CPose3DPDFSOG::appendFrom( const CPose3DPDFSOG &o )
{
        MRPT_START

        ASSERT_( &o != this );  // Don't be bad...
        if (o.m_modes.empty()) return;

        // Make copies:
        for (const_iterator it = o.m_modes.begin();it!=o.m_modes.end();++it)
                m_modes.push_back( *it );

        normalizeWeights();
        MRPT_END
}


/*---------------------------------------------------------------
                                                getMostLikelyMode
 ---------------------------------------------------------------*/
void CPose3DPDFSOG::getMostLikelyMode( CPose3DPDFGaussian& outVal ) const
{
        if (this->empty())
        {
                outVal = CPose3DPDFGaussian();
        }
        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;
        }
}