CGeneralizedEllipsoidTemplate.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/matrix_serialization.h>  // for >> ops
#include <mrpt/opengl/CRenderizableShaderTriangles.h>
#include <mrpt/opengl/CRenderizableShaderWireFrame.h>
#include <mrpt/serialization/CArchive.h>  // for >> ops

namespace mrpt::opengl
{
/** A class that generalizes the concept of an ellipsoid to arbitrary
 * parameterizations of
 *  uncertainty shapes in either 2D or 3D. See derived classes for examples.
 *
 * Please read the documentation of
 * CGeneralizedEllipsoidTemplate::setQuantiles() for learning
 *  the mathematical details about setting the desired confidence interval.
 *
 *  The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
 *
 * \tparam DIM The dimensionality of the parameter space, which must coincide
 * with that of the rendering space (2 or 3)
 *
 * \ingroup mrpt_opengl_grp
 */
template <int DIM>
class CGeneralizedEllipsoidTemplate
    : virtual public CRenderizableShaderTriangles,
      virtual public CRenderizableShaderWireFrame
{
   public:
    /** @name Renderizable shader API virtual methods
     * @{ */
    void render(const RenderContext& rc) const override
    {
        switch (rc.shader_id)
        {
            case DefaultShaderID::TRIANGLES:
                if (m_drawSolid3D) CRenderizableShaderTriangles::render(rc);
                break;
            case DefaultShaderID::WIREFRAME:
                if (!m_drawSolid3D) CRenderizableShaderWireFrame::render(rc);
                break;
        };
    }
    void freeOpenGLResources() override
    {
        CRenderizableShaderTriangles::freeOpenGLResources();
        CRenderizableShaderWireFrame::freeOpenGLResources();
    }

    void renderUpdateBuffers() const override
    {
        // 1) Update eigenvectors/values:
        if (m_needToRecomputeEigenVals)
        {
            m_needToRecomputeEigenVals = false;
            // Handle the special case of an ellipsoid of volume = 0
            const double d = m_cov.det();
            // Note: "d!=d" is a great test for invalid numbers, don't remove!
            if (std::abs(d) < 1e-20 || d != d)
            {
                // All zeros:
                m_U.setZero(DIM, DIM);
            }
            else
            {
                // A valid matrix:
                m_cov.chol(m_U);
            }
        }

        // 2) Generate "standard" ellipsoid:
        std::vector<array_parameter_t> params_pts;
        cov_matrix_t Uscaled = m_U;
        Uscaled *= static_cast<double>(m_quantiles);
        generatePoints(Uscaled, params_pts);

        // 3) Transform into 2D/3D render space:
        this->transformFromParameterSpace(params_pts, m_render_pts);

        // 3.5) Save bounding box:
        m_bb_min = mrpt::math::TPoint3D(
            std::numeric_limits<double>::max(),
            std::numeric_limits<double>::max(), 0);
        m_bb_max = mrpt::math::TPoint3D(
            -std::numeric_limits<double>::max(),
            -std::numeric_limits<double>::max(), 0);
        for (size_t i = 0; i < m_render_pts.size(); i++)
            for (int k = 0; k < DIM; k++)
            {
                mrpt::keep_min(m_bb_min[k], m_render_pts[i][k]);
                mrpt::keep_max(m_bb_max[k], m_render_pts[i][k]);
            }
        // Convert to coordinates of my parent:
        m_pose.composePoint(m_bb_min, m_bb_min);
        m_pose.composePoint(m_bb_max, m_bb_max);

        CRenderizableShaderTriangles::renderUpdateBuffers();
        CRenderizableShaderWireFrame::renderUpdateBuffers();
    }
    virtual shader_list_t requiredShaders() const override
    {
        // May use up to two shaders (triangles and lines):
        return {DefaultShaderID::WIREFRAME, DefaultShaderID::TRIANGLES};
    }
    // Render precomputed points in m_render_pts:
    void onUpdateBuffers_Wireframe() override { implUpdate_Wireframe(); }
    // Render precomputed points in m_render_pts:
    void onUpdateBuffers_Triangles() override { implUpdate_Triangles(); }
    /** @} */

    /** The type of fixed-size covariance matrices for this representation
     */
    using cov_matrix_t = mrpt::math::CMatrixFixed<double, DIM, DIM>;
    /** The type of fixed-size vector for this representation */
    using mean_vector_t = mrpt::math::CMatrixFixed<double, DIM, 1>;

    using array_parameter_t = mrpt::math::CMatrixFixed<float, DIM, 1>;
    using array_point_t = mrpt::math::CMatrixFixed<float, DIM, 1>;

    /**  Set the NxN covariance matrix that will determine the aspect of the
     * ellipsoid - Notice that the
     *  covariance determines the uncertainty in the parameter space, which
     * would be transformed by derived function
     */
    template <typename MATRIX, typename VECTOR>
    void setCovMatrixAndMean(const MATRIX& new_cov, const VECTOR& new_mean)
    {
        MRPT_START
        ASSERT_EQUAL_(new_cov.cols(), new_cov.rows());
        ASSERT_EQUAL_(new_cov.cols(), DIM);
        m_cov = new_cov;
        m_mean = new_mean;
        m_needToRecomputeEigenVals = true;
        CRenderizable::notifyChange();
        MRPT_END
    }

    /** Gets the current uncertainty covariance of parameter space */
    const cov_matrix_t& getCovMatrix() const { return m_cov; }

    /** Like setCovMatrixAndMean(), for mean=zero.
     */
    template <typename MATRIX>
    void setCovMatrix(const MATRIX& new_cov)
    {
        setCovMatrixAndMean(new_cov, mean_vector_t::Zero());
    }

    /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid
     *(default=3, ~97 or ~98% CI); the exact mathematical meaning is:
     *   This value of "quantiles" \a q should be set to the square root of
     *the chi-squared inverse cdf corresponding to the desired confidence
     *interval. <b>Note that this value depends on the dimensionality</b>.
     *   Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
     *
     *  Some common values follow here for the convenience of users:
     *      - Dimensionality=3 (3D ellipsoids):
     *          - 19.8748% CI -> q=1
     *          - 73.8536% CI -> q=2
     *          - 97.0709% CI -> q=3
     *          - 99.8866% CI -> q=4
     *      - Dimensionality=2 (2D ellipses):
     *          - 39.347% CI -> q=1
     *          - 86.466% CI -> q=2
     *          - 98.8891% CI -> q=3
     *          - 99.9664% CI -> q=4
     *      - Dimensionality=1 (Not aplicable to this class but provided for
     *reference):
     *          - 68.27% CI -> q=1
     *          - 95.45% CI -> q=2
     *          - 99.73% CI -> q=3
     *          - 99.9937% CI -> q=4
     *
     */
    void setQuantiles(float q)
    {
        m_quantiles = q;
        CRenderizable::notifyChange();
    }
    /** Refer to documentation of \a setQuantiles() */
    float getQuantiles() const { return m_quantiles; }

    /** Set the number of segments of the surface/curve (higher means with
     * greater resolution) */
    void setNumberOfSegments(const uint32_t numSegments)
    {
        m_numSegments = numSegments;
        CRenderizable::notifyChange();
    }
    uint32_t getNumberOfSegments() { return m_numSegments; }

    /** If set to "true", a whole ellipsoid surface will be drawn, or if
     * set to "false" (default) it will be drawn as a "wireframe". */
    void enableDrawSolid3D(bool v)
    {
        m_drawSolid3D = v;
        CRenderizable::notifyChange();
    }

    /** Evaluates the bounding box of this object (including possible
     * children) in the coordinate frame of the object parent. */
    void getBoundingBox(
        mrpt::math::TPoint3D& bb_min,
        mrpt::math::TPoint3D& bb_max) const override
    {
        bb_min = m_bb_min;
        bb_max = m_bb_max;
    }

    /** Ray tracing  */
    virtual bool traceRay(
        [[maybe_unused]] const mrpt::poses::CPose3D& o,
        [[maybe_unused]] double& dist) const override
    {
        THROW_EXCEPTION("Not implemented ");
    }

   protected:
    /** To be implemented by derived classes: maps, using some arbitrary
     * space transformation, a list of points defining an ellipsoid in
     * parameter space into their corresponding points in 2D/3D space.
     */
    virtual void transformFromParameterSpace(
        const std::vector<array_point_t>& params_pts,
        std::vector<array_point_t>& out_pts) const = 0;

    mutable cov_matrix_t m_cov;
    mean_vector_t m_mean;
    mutable bool m_needToRecomputeEigenVals{true};
    /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
     */
    float m_quantiles{3.f};
    /** Number of segments in 2D/3D ellipsoids (default=20) */
    uint32_t m_numSegments{20};
    mutable mrpt::math::TPoint3D m_bb_min{0, 0, 0}, m_bb_max{0, 0, 0};

    /** If set to "true", a whole ellipsoid surface will be drawn, or
     * if set to "false" (default)it will be drawn as a "wireframe". */
    bool m_drawSolid3D{false};

    /** Cholesky U triangular matrix cache. */
    mutable cov_matrix_t m_U;
    mutable std::vector<array_point_t> m_render_pts;

    void thisclass_writeToStream(mrpt::serialization::CArchive& out) const
    {
        using namespace mrpt::math;

        const uint8_t version = 0;
        out << version << m_cov << m_mean << m_quantiles << m_lineWidth
            << m_numSegments;
    }
    void thisclass_readFromStream(mrpt::serialization::CArchive& in)
    {
        uint8_t version;
        in >> version;
        switch (version)
        {
            case 0:
            {
                in >> m_cov >> m_mean >> m_quantiles >> m_lineWidth >>
                    m_numSegments;
                m_needToRecomputeEigenVals = true;
            }
            break;
            default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
        };
        CRenderizable::notifyChange();
    }

    CGeneralizedEllipsoidTemplate() = default;
    virtual ~CGeneralizedEllipsoidTemplate() override = default;

    // Uscaled, m_mean, params_pts, m_numSegments, m_numSegments
    void generatePoints(
        const cov_matrix_t& U,
        std::vector<array_parameter_t>& out_params_pts) const;

    void implUpdate_Wireframe();
    void implUpdate_Triangles();

   public:
    // Solve virtual public inheritance ambiguity:
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const override
    {
        return CRenderizableShaderWireFrame::GetRuntimeClass();
    }
};

}  // namespace mrpt::opengl