CPoseRandomSampler.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 "poses-precomp.h"  // Precompiled headers

#include <mrpt/poses/CPoseRandomSampler.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/random.h>

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

using namespace mrpt::random;

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
CPoseRandomSampler::CPoseRandomSampler() {}
CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
{
    if (o.m_pdf2D)
        m_pdf2D.reset(dynamic_cast<const CPosePDF*>(o.m_pdf2D->clone()));
    if (o.m_pdf3D)
        m_pdf3D.reset(dynamic_cast<const CPose3DPDF*>(o.m_pdf3D->clone()));
    m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
    m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
    m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
    m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
}

CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
{
    if (&o == this) return *this;
    m_pdf2D.reset();
    m_pdf3D.reset();
    if (o.m_pdf2D)
        m_pdf2D.reset(dynamic_cast<const CPosePDF*>(o.m_pdf2D->clone()));
    if (o.m_pdf3D)
        m_pdf3D.reset(dynamic_cast<const CPose3DPDF*>(o.m_pdf3D->clone()));
    m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
    m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
    m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
    m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
    return *this;
}

CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
    : m_pdf2D(nullptr), m_pdf3D(nullptr)
{
    if (o.m_pdf2D)
    {
        m_pdf2D = std::move(o.m_pdf2D);
        o.m_pdf2D = nullptr;
    }
    if (o.m_pdf3D)
    {
        m_pdf3D = std::move(o.m_pdf3D);
        o.m_pdf3D = nullptr;
    }
    m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
    m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
    m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
    m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
}
CPoseRandomSampler& CPoseRandomSampler::operator=(CPoseRandomSampler&& o)
{
    if (this == &o) return *this;
    this->clear();
    if (o.m_pdf2D)
    {
        m_pdf2D = std::move(o.m_pdf2D);
        o.m_pdf2D = nullptr;
    }
    if (o.m_pdf3D)
    {
        m_pdf3D = std::move(o.m_pdf3D);
        o.m_pdf3D = nullptr;
    }
    m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
    m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
    m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
    m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
    return *this;
}

/*---------------------------------------------------------------
            clear
  ---------------------------------------------------------------*/
void CPoseRandomSampler::clear()
{
    m_pdf2D.reset();
    m_pdf3D.reset();
}

/*---------------------------------------------------------------
                        setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPosePDF* pdf)
{
    MRPT_START

    clear();
    m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf->clone()));

    // According to the PDF type:
    if (IS_CLASS(m_pdf2D.get(), CPosePDFGaussian))
    {
        const CPosePDFGaussian* gPdf =
            static_cast<const CPosePDFGaussian*>(pdf);
        const CMatrixDouble33& cov = gPdf->cov;

        m_fastdraw_gauss_M_2D = gPdf->mean;

        /** Computes the eigenvalues/eigenvector decomposition of this matrix,
        *    so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
        *     eigenvectors and the diagonal matrix D contains the eigenvalues
        *    as diagonal elements, sorted in <i>ascending</i> order.
        */
        CMatrixDouble33 D;
        cov.eigenVectors(m_fastdraw_gauss_Z3, D);

        // Scale eigenvectors with eigenvalues:
        D = D.array().sqrt().matrix();
        m_fastdraw_gauss_Z3.multiply(m_fastdraw_gauss_Z3, D);
    }
    else if (IS_CLASS(m_pdf2D.get(), CPosePDFParticles))
    {
        return;  // Nothing to prepare.
    }
    else
    {
        THROW_EXCEPTION_FMT(
            "Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
    }

    MRPT_END
}

/*---------------------------------------------------------------
                        setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPose3DPDF* pdf)
{
    MRPT_START

    clear();
    m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf->clone()));

    // According to the PDF type:
    if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
    {
        const CPose3DPDFGaussian* gPdf =
            static_cast<const CPose3DPDFGaussian*>(pdf);
        const CMatrixDouble66& cov = gPdf->cov;

        m_fastdraw_gauss_M_3D = gPdf->mean;

        /** Computes the eigenvalues/eigenvector decomposition of this matrix,
        *    so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
        *     eigenvectors and the diagonal matrix D contains the eigenvalues
        *    as diagonal elements, sorted in <i>ascending</i> order.
        */
        CMatrixDouble66 D;
        cov.eigenVectors(m_fastdraw_gauss_Z6, D);

        // Scale eigenvectors with eigenvalues:
        D = D.array().sqrt().matrix();
        m_fastdraw_gauss_Z6.multiply(m_fastdraw_gauss_Z6, D);
    }
    else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
    {
        return;  // Nothing to prepare.
    }
    else
    {
        THROW_EXCEPTION_FMT(
            "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
    }

    MRPT_END
}

void CPoseRandomSampler::setPosePDF(const CPose3DPDF::Ptr& pdf)
{
    setPosePDF(pdf.get());
}

void CPoseRandomSampler::setPosePDF(const CPosePDF::Ptr& pdf)
{
    setPosePDF(pdf.get());
}

/*---------------------------------------------------------------
                    drawSample
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::drawSample(CPose2D& p) const
{
    MRPT_START

    if (m_pdf2D)
    {
        do_sample_2D(p);
    }
    else if (m_pdf3D)
    {
        CPose3D q;
        do_sample_3D(q);
        p.x(q.x());
        p.y(q.y());
        p.phi(q.yaw());
    }
    else
        THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

    return p;
    MRPT_END
}

/*---------------------------------------------------------------
                    drawSample
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::drawSample(CPose3D& p) const
{
    MRPT_START

    if (m_pdf2D)
    {
        CPose2D q;
        do_sample_2D(q);
        p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
    }
    else if (m_pdf3D)
    {
        do_sample_3D(p);
    }
    else
        THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

    return p;
    MRPT_END
}

/*---------------------------------------------------------------
                  do_sample_2D: Sample from a 2D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_2D(CPose2D& p) const
{
    MRPT_START
    ASSERT_(m_pdf2D);

    // According to the PDF type:
    if (IS_CLASS(m_pdf2D.get(), CPosePDFGaussian))
    {
        // ------------------------------
        //      A single gaussian:
        // ------------------------------
        CVectorDouble rndVector(3);
        rndVector.setZero();
        for (size_t i = 0; i < 3; i++)
        {
            double rnd = getRandomGenerator().drawGaussian1D_normalized();
            for (size_t d = 0; d < 3; d++)
                rndVector[d] += (m_fastdraw_gauss_Z3.get_unsafe(d, i) * rnd);
        }

        p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
        p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
        p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
        p.normalizePhi();
    }
    else if (IS_CLASS(m_pdf2D.get(), CPosePDFSOG))
    {
        // -------------------------------------
        //                  SOG
        // -------------------------------------
        THROW_EXCEPTION("TODO");
    }
    else if (IS_CLASS(m_pdf2D.get(), CPosePDFParticles))
    {
        // -------------------------------------
        //      Particles: just sample as usual
        // -------------------------------------
        const CPosePDFParticles* pdf =
            static_cast<const CPosePDFParticles*>(m_pdf2D.get());
        pdf->drawSingleSample(p);
    }
    else
        THROW_EXCEPTION_FMT(
            "Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);

    MRPT_END
}

/*---------------------------------------------------------------
                  do_sample_3D: Sample from a 3D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_3D(CPose3D& p) const
{
    MRPT_START
    ASSERT_(m_pdf3D);

    // According to the PDF type:
    if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
    {
        // ------------------------------
        //      A single gaussian:
        // ------------------------------
        CVectorDouble rndVector(6);
        rndVector.setZero();
        for (size_t i = 0; i < 6; i++)
        {
            double rnd = getRandomGenerator().drawGaussian1D_normalized();
            for (size_t d = 0; d < 6; d++)
                rndVector[d] += (m_fastdraw_gauss_Z6.get_unsafe(d, i) * rnd);
        }

        p.setFromValues(
            m_fastdraw_gauss_M_3D.x() + rndVector[0],
            m_fastdraw_gauss_M_3D.y() + rndVector[1],
            m_fastdraw_gauss_M_3D.z() + rndVector[2],
            m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
            m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
            m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
    }
    else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFSOG))
    {
        // -------------------------------------
        //                  SOG
        // -------------------------------------
        THROW_EXCEPTION("TODO");
    }
    else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
    {
        // -------------------------------------
        //      Particles: just sample as usual
        // -------------------------------------
        const CPose3DPDFParticles* pdf =
            static_cast<const CPose3DPDFParticles*>(m_pdf3D.get());
        pdf->drawSingleSample(p);
    }
    else
        THROW_EXCEPTION_FMT(
            "Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);

    MRPT_END
}

/*---------------------------------------------------------------
                  isPrepared
  ---------------------------------------------------------------*/
bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
/*---------------------------------------------------------------
                  getOriginalPDFCov2D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov2D(CMatrixDouble33& cov3x3) const
{
    MRPT_START
    ASSERT_(this->isPrepared());

    if (m_pdf2D)
    {
        m_pdf2D->getCovariance(cov3x3);
    }
    else
    {
        ASSERT_(m_pdf3D);

        CPosePDFGaussian P;
        P.copyFrom(*m_pdf3D);
        cov3x3 = P.cov;
    }

    MRPT_END
}

/*---------------------------------------------------------------
                  getOriginalPDFCov3D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov3D(CMatrixDouble66& cov6x6) const
{
    MRPT_START
    ASSERT_(this->isPrepared());

    if (m_pdf2D)
    {
        CPose3DPDFGaussian P;
        P.copyFrom(*m_pdf2D);
        cov6x6 = P.cov;
    }
    else
    {
        ASSERT_(m_pdf3D);
        m_pdf3D->getCovariance(cov6x6);
    }

    MRPT_END
}

/*---------------------------------------------------------------
                  getSamplingMean2D
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::getSamplingMean2D(CPose2D& out_mean) const
{
    MRPT_START
    ASSERT_(this->isPrepared());

    if (m_pdf2D)
        out_mean = m_fastdraw_gauss_M_2D;
    else
        out_mean = CPose2D(m_fastdraw_gauss_M_3D);

    return out_mean;
    MRPT_END
}

/*---------------------------------------------------------------
                  getSamplingMean3D
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::getSamplingMean3D(CPose3D& out_mean) const
{
    MRPT_START
    ASSERT_(this->isPrepared());

    if (m_pdf3D)
        out_mean = m_fastdraw_gauss_M_3D;
    else
        out_mean = CPose3D(m_fastdraw_gauss_M_2D);

    return out_mean;
    MRPT_END
}