pose_pdfs.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 "opengl-precomp.h"  // Precompiled header

#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/stock_objects.h>

#include <mrpt/poses.h>

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

const double POSE_TAIL_LENGTH = 0.1;
const double POSE_TAIL_WIDTH = 3.0;
const double POSE_POINT_SIZE = 4.0;
const double POSE_AXIS_SCALE = 0.1;

#define POSE_COLOR 0, 0, 1
#define POINT_COLOR 1, 0, 0

/** Returns a representation of a the PDF - this is just an auxiliary function,
 * it's more natural to call
        *    mrpt::poses::CPosePDF::getAs3DObject     */
CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPosePDF& o)
{
        CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();

        if (IS_CLASS(&o, CPosePDFSOG))
        {
                const CPosePDFSOG* p = static_cast<const CPosePDFSOG*>(&o);

                opengl::CSetOfLines::Ptr lins =
                        mrpt::make_aligned_shared<opengl::CSetOfLines>();
                lins->setColor(0, 0, 1, 0.6);
                lins->setLineWidth(POSE_TAIL_WIDTH);

                for (CPosePDFSOG::const_iterator it = p->begin(); it != p->end(); ++it)
                {
                        opengl::CEllipsoid::Ptr ellip =
                                mrpt::make_aligned_shared<opengl::CEllipsoid>();

                        ellip->setPose(CPose3D((it)->mean.x(), (it)->mean.y(), 0));
                        ellip->setCovMatrix((it)->cov, 2 /* x y */);
                        ellip->setColor(POSE_COLOR, 0.6);
                        ellip->setQuantiles(3);
                        ellip->enableDrawSolid3D(false);

                        outObj->insert(ellip);

                        lins->appendLine(
                                (it)->mean.x(), (it)->mean.y(), 0,
                                (it)->mean.x() + POSE_TAIL_LENGTH * cos((it)->mean.phi()),
                                (it)->mean.y() + POSE_TAIL_LENGTH * sin((it)->mean.phi()), 0);
                }
                outObj->insert(lins);
        }
        else if (IS_CLASS(&o, CPosePDFGaussian))
        {
                const CPosePDFGaussian* p = static_cast<const CPosePDFGaussian*>(&o);

                opengl::CSetOfLines::Ptr lins =
                        mrpt::make_aligned_shared<opengl::CSetOfLines>();
                lins->setColor(POSE_COLOR, 0.6);
                lins->setLineWidth(POSE_TAIL_WIDTH);

                opengl::CEllipsoid::Ptr ellip =
                        mrpt::make_aligned_shared<opengl::CEllipsoid>();

                ellip->setPose(CPose3D(p->mean.x(), p->mean.y(), 0));
                ellip->setCovMatrix(p->cov, 2 /* x y */);
                ellip->setColor(POSE_COLOR, 0.6);

                ellip->setQuantiles(3);
                ellip->enableDrawSolid3D(false);

                outObj->insert(ellip);

                lins->appendLine(
                        p->mean.x(), p->mean.y(), 0,
                        p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()),
                        p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()), 0);

                outObj->insert(lins);
        }
        else if (IS_CLASS(&o, CPosePDFParticles))
        {
                const CPosePDFParticles* p = static_cast<const CPosePDFParticles*>(&o);

                opengl::CPointCloud::Ptr pnts =
                        mrpt::make_aligned_shared<opengl::CPointCloud>();
                pnts->setColor(POSE_COLOR, 0.6);
                pnts->setPointSize(POSE_POINT_SIZE);

                opengl::CSetOfLines::Ptr lins =
                        mrpt::make_aligned_shared<opengl::CSetOfLines>();
                lins->setColor(POSE_COLOR, 0.6);
                lins->setLineWidth(POSE_TAIL_WIDTH);

                for (size_t i = 0; i < p->size(); ++i)
                {
                        const mrpt::poses::CPose2D* po = p->m_particles[i].d.get();
                        pnts->insertPoint(po->x(), po->y(), 0);
                        lins->appendLine(
                                po->x(), po->y(), 0,
                                po->x() + POSE_TAIL_LENGTH * cos(po->phi()),
                                po->y() + POSE_TAIL_LENGTH * sin(po->phi()), 0);
                }
                outObj->insert(pnts);
                outObj->insert(lins);
        }

        return outObj;
}

/** Returns a representation of a the PDF - this is just an auxiliary function,
 * it's more natural to call
        *    mrpt::poses::CPointPDF::getAs3DObject     */
CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPointPDF& o)
{
        CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();

        if (IS_CLASS(&o, CPointPDFSOG))
        {
                const CPointPDFSOG* p = static_cast<const CPointPDFSOG*>(&o);

                // For each gaussian node
                for (CPointPDFSOG::CListGaussianModes::const_iterator it = p->begin();
                         it != p->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(POINT_COLOR);

                        outObj->insert(obj);
                }  // end for each gaussian node
        }
        else if (IS_CLASS(&o, CPointPDFGaussian))
        {
                const CPointPDFGaussian* p = static_cast<const CPointPDFGaussian*>(&o);

                CEllipsoid::Ptr obj = mrpt::make_aligned_shared<CEllipsoid>();
                obj->setLocation(p->mean);
                obj->setCovMatrix(p->cov, p->cov(2, 2) == 0 ? 2 : 3);
                obj->setColor(POINT_COLOR);
                obj->setQuantiles(3);
                obj->enableDrawSolid3D(false);
                outObj->insert(obj);
        }
        else if (IS_CLASS(&o, CPointPDFParticles))
        {
                const CPointPDFParticles* p =
                        static_cast<const CPointPDFParticles*>(&o);

                mrpt::opengl::CPointCloud::Ptr obj =
                        mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
                const size_t N = p->size();

                obj->resize(N);
                obj->setColor(POINT_COLOR);
                for (size_t i = 0; i < N; i++)
                        obj->setPoint(
                                i, p->m_particles[i].d->x, p->m_particles[i].d->y,
                                p->m_particles[i].d->z);
                outObj->insert(obj);
        }

        return outObj;
}

/** Returns a representation of a the PDF - this is just an auxiliary function,
 * it's more natural to call
        *    mrpt::poses::CPose3DPDF::getAs3DObject     */
CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPose3DPDF& o)
{
        CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();

        if (IS_CLASS(&o, CPose3DPDFSOG))
        {
                const CPose3DPDFSOG* p = static_cast<const CPose3DPDFSOG*>(&o);

                // For each gaussian node
                for (CPose3DPDFSOG::const_iterator it = p->begin(); it != p->end();
                         ++it)
                {
                        opengl::CEllipsoid::Ptr obj =
                                mrpt::make_aligned_shared<opengl::CEllipsoid>();

                        obj->setPose(it->val.mean);
                        obj->setCovMatrix(
                                CMatrixDouble(it->val.cov), it->val.cov(2, 2) == 0 ? 2 : 3);

                        obj->setQuantiles(3);
                        obj->enableDrawSolid3D(false);
                        obj->setColor(POSE_COLOR);

                        outObj->insert(obj);

                        opengl::CSetOfObjects::Ptr axes =
                                opengl::stock_objects::CornerXYZ();
                        axes->setPose(it->val.mean);
                        axes->setScale(POSE_AXIS_SCALE);
                        outObj->insert(axes);
                }  // end for each gaussian node
        }
        else if (IS_CLASS(&o, CPose3DPDFGaussian))
        {
                const CPose3DPDFGaussian* p =
                        static_cast<const CPose3DPDFGaussian*>(&o);

                opengl::CEllipsoid::Ptr obj =
                        mrpt::make_aligned_shared<opengl::CEllipsoid>();

                obj->setPose(p->mean);
                obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);

                obj->setQuantiles(3);
                obj->enableDrawSolid3D(false);
                obj->setColor(POSE_COLOR);

                outObj->insert(obj);

                opengl::CSetOfObjects::Ptr axes = opengl::stock_objects::CornerXYZ();
                axes->setPose(p->mean);
                axes->setScale(POSE_AXIS_SCALE);
                outObj->insert(axes);
        }
        else if (IS_CLASS(&o, CPose3DPDFParticles))
        {
                const CPose3DPDFParticles* p =
                        static_cast<const CPose3DPDFParticles*>(&o);

                for (size_t i = 0; i < p->size(); i++)
                {
                        opengl::CSetOfObjects::Ptr axes =
                                opengl::stock_objects::CornerXYZSimple(POSE_AXIS_SCALE);
                        axes->setPose(*p->m_particles[i].d);
                        outObj->insert(axes);
                }
        }

        return outObj;
}

/** Returns a representation of a the PDF - this is just an auxiliary function,
 * it's more natural to call
        *    mrpt::poses::CPose3DQuatPDF::getAs3DObject     */
CSetOfObjects::Ptr CSetOfObjects::posePDF2opengl(const CPose3DQuatPDF& o)
{
        CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();

        if (IS_CLASS(&o, CPose3DQuatPDFGaussian))
        {
                const CPose3DQuatPDFGaussian* p =
                        static_cast<const CPose3DQuatPDFGaussian*>(&o);

                opengl::CEllipsoid::Ptr obj =
                        mrpt::make_aligned_shared<opengl::CEllipsoid>();

                obj->setPose(CPose3D(p->mean));
                obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);

                obj->setQuantiles(3);
                obj->enableDrawSolid3D(false);
                obj->setColor(POSE_COLOR);

                outObj->insert(obj);

                opengl::CSetOfObjects::Ptr axes = opengl::stock_objects::CornerXYZ();
                axes->setPose(CPose3D(p->mean));
                axes->setScale(POSE_AXIS_SCALE);
                outObj->insert(axes);
        }

        return outObj;
}