CEllipsoid.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/math/CMatrix.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/utils/CStream.h>
 
#include "opengl_internals.h"
 
using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace std;
 
IMPLEMENTS_SERIALIZABLE(CEllipsoid, CRenderizableDisplayList, mrpt::opengl)
 
/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
        MRPT_START
 
        const size_t dim = m_cov.getColCount();
 
        if (m_eigVal(0, 0) != 0.0 && m_eigVal(1, 1) != 0.0 &&
                (dim == 2 || m_eigVal(2, 2) != 0.0) && m_quantiles != 0.0)
        {
                glEnable(GL_BLEND);
                checkOpenGLError();
                glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
                checkOpenGLError();
                glLineWidth(m_lineWidth);
                checkOpenGLError();
 
                if (dim == 2)
                {
                        glDisable(GL_LIGHTING);  // Disable lights when drawing lines
 
                        // ---------------------
                        //     2D ellipse
                        // ---------------------
 
                        /* Equivalent MATLAB code:
                         *
                         * q=1;
                         * [vec val]=eig(C);
                         * M=(q*val*vec)';
                         * R=M*[x;y];
                         * xx=R(1,:);yy=R(2,:);
                         * plot(xx,yy), axis equal;
                         */
 
                        double ang;
                        unsigned int i;
 
                        // Compute the new vectors for the ellipsoid:
                        CMatrixDouble M;
                        M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
 
                        glBegin(GL_LINE_LOOP);
 
                        // Compute the points of the 2D ellipse:
                        for (i = 0, ang = 0; i < m_2D_segments;
                                 i++, ang += (M_2PI / m_2D_segments))
                        {
                                double ccos = cos(ang);
                                double ssin = sin(ang);
 
                                const float x =
                                        ccos * M.get_unsafe(0, 0) + ssin * M.get_unsafe(1, 0);
                                const float y =
                                        ccos * M.get_unsafe(0, 1) + ssin * M.get_unsafe(1, 1);
 
                                glVertex2f(x, y);
                        }  // end for points on ellipse
 
                        glEnd();
 
                        // 2D: Save bounding box:
                        const double max_radius =
                                m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
                        m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
                        m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
                        // Convert to coordinates of my parent:
                        m_pose.composePoint(m_bb_min, m_bb_min);
                        m_pose.composePoint(m_bb_max, m_bb_max);
 
                        glEnable(GL_LIGHTING);
                }
                else
                {
                        // ---------------------
                        //    3D ellipsoid
                        // ---------------------
                        GLfloat mat[16];
 
                        //  A homogeneous transformation matrix, in this order:
                        //
                        //     0  4  8  12
                        //     1  5  9  13
                        //     2  6  10 14
                        //     3  7  11 15
                        //
                        mat[3] = mat[7] = mat[11] = 0;
                        mat[15] = 1;
                        mat[12] = mat[13] = mat[14] = 0;
 
                        mat[0] = m_eigVec(0, 0);
                        mat[1] = m_eigVec(1, 0);
                        mat[2] = m_eigVec(2, 0);  // New X-axis
                        mat[4] = m_eigVec(0, 1);
                        mat[5] = m_eigVec(1, 1);
                        mat[6] = m_eigVec(2, 1);  // New X-axis
                        mat[8] = m_eigVec(0, 2);
                        mat[9] = m_eigVec(1, 2);
                        mat[10] = m_eigVec(2, 2);  // New X-axis
 
                        GLUquadricObj* obj = gluNewQuadric();
                        checkOpenGLError();
 
                        if (!m_drawSolid3D)
                                glDisable(GL_LIGHTING);  // Disable lights when drawing lines
 
                        gluQuadricDrawStyle(obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
 
                        glPushMatrix();
                        glMultMatrixf(mat);
                        glScalef(
                                m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
                                m_eigVal(2, 2) * m_quantiles);
 
                        gluSphere(obj, 1, m_3D_segments, m_3D_segments);
                        checkOpenGLError();
 
                        glPopMatrix();
 
                        gluDeleteQuadric(obj);
                        checkOpenGLError();
 
                        // 3D: Save bounding box:
                        const double max_radius =
                                m_quantiles *
                                std::max(
                                        m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
                        m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
                        m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
                        // Convert to coordinates of my parent:
                        m_pose.composePoint(m_bb_min, m_bb_min);
                        m_pose.composePoint(m_bb_max, m_bb_max);
                }
 
                glDisable(GL_BLEND);
 
                glEnable(GL_LIGHTING);
        }
        MRPT_END_WITH_CLEAN_UP(
                cout << "Covariance matrix leading to error is:" << endl
                         << m_cov << endl;);
#endif
}
 
/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CEllipsoid::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 1;
        else
        {
                writeToStreamRender(out);
                out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments
                        << (uint32_t)m_3D_segments << m_lineWidth;
        }
}
 
/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CEllipsoid::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                {
                        uint32_t i;
                        readFromStreamRender(in);
                        if (version == 0)
                        {
                                CMatrix c;
                                in >> c;
                                m_cov = c.cast<double>();
                        }
                        else
                        {
                                in >> m_cov;
                        }
 
                        in >> m_drawSolid3D >> m_quantiles;
                        in >> i;
                        m_2D_segments = i;
                        in >> i;
                        m_3D_segments = i;
                        in >> m_lineWidth;
 
                        // Update cov. matrix cache:
                        m_prevComputedCov = m_cov;
                        setCovMatrix(m_cov);
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        CRenderizableDisplayList::notifyChange();
}
 
bool quickSolveEqn(double a, double b_2, double c, double& t)
{
        double delta = square(b_2) - a * c;
        if (delta == 0)
                return (t = -b_2 / a) >= 0;
        else if (delta > 0)
        {
                delta = sqrt(delta);
                if ((t = (-b_2 - delta) / a) >= 0)
                        return true;
                else
                        return (t = (-b_2 + delta) / a) >= 0;
        }
        else
                return false;
}
 
bool CEllipsoid::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
        if (m_cov.getRowCount() != 3) return false;
        TLine3D lin, lin2;
        createFromPoseX(o - this->m_pose, lin);
        lin.unitarize();  // By adding this line, distance from any point of the
        // line to its base is exactly equal to the "t".
        for (size_t i = 0; i < 3; i++)
        {
                lin2.pBase[i] = 0;
                lin2.director[i] = 0;
                for (size_t j = 0; j < 3; j++)
                {
                        double vji = m_eigVec(j, i);
                        lin2.pBase[i] += vji * lin.pBase[j];
                        lin2.director[i] += vji * lin.director[j];
                }
        }
        double a = 0, b_2 = 0, c = -square(m_quantiles);
        for (size_t i = 0; i < 3; i++)
        {
                double ev = m_eigVal(i, i);
                a += square(lin2.director[i] / ev);
                b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
                c += square(lin2.pBase[i] / ev);
        }
        return quickSolveEqn(a, b_2, c, dist);
}
 
void CEllipsoid::setCovMatrix(
        const mrpt::math::CMatrixDouble& m, int resizeToSize)
{
        MRPT_START
 
        ASSERT_(m.getColCount() == m.getRowCount());
        ASSERT_(
                size(m, 1) == 2 || size(m, 1) == 3 ||
                (resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));
 
        m_cov = m;
        if (resizeToSize > 0 && resizeToSize < (int)size(m, 1))
                m_cov.setSize(resizeToSize, resizeToSize);
 
        if (m_cov == m_prevComputedCov) return;  // Done.
 
        CRenderizableDisplayList::notifyChange();
 
        // Handle the special case of an ellipsoid of volume = 0
        const double d = m_cov.det();
        if (d == 0 || d != d)  // Note: "d!=d" is a great test for invalid numbers,
        // don't remove!
        {
                // All zeros:
                m_prevComputedCov = m_cov;
                m_eigVec.zeros(3, 3);
                m_eigVal.zeros(3, 3);
        }
        else
        {
                // Not null matrix: compute the eigen-vectors & values:
                m_prevComputedCov = m_cov;
                if (m_cov.eigenVectors(m_eigVec, m_eigVal))
                {
                        m_eigVal = m_eigVal.array().sqrt().matrix();
                        // Do the scale at render to avoid recomputing the m_eigVal for
                        // different m_quantiles
                }
                else
                {
                        m_eigVec.zeros(3, 3);
                        m_eigVal.zeros(3, 3);
                }
        }
 
        MRPT_END
}
 
void CEllipsoid::setCovMatrix(
        const mrpt::math::CMatrixFloat& m, int resizeToSize)
{
        CRenderizableDisplayList::notifyChange();
        setCovMatrix(CMatrixDouble(m), resizeToSize);
}
 
/** Evaluates the bounding box of this object (including possible children) in
 * the coordinate frame of the object parent. */
void CEllipsoid::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        bb_min = m_bb_min;
        bb_max = m_bb_max;
}