srba/impl/export_opengl_landmark_renderers.h

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, 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    |
   +---------------------------------------------------------------------------+ */

#pragma once
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/opengl/CText3D.h>
#include <mrpt/math/lightweight_geom_data.h>
#include <srba/landmark_render_models.h>

namespace srba {

// Specializations for rendering each kind of landmark:
template <class landmark_t>
struct LandmarkRendererBase { };

template <> struct LandmarkRendererBase<landmark_rendering_as_point>
{
    template <class RBA>
    static void render(
        const RBA &rba,
        const srba::TKeyFrameID root_keyframe,
        const typename RBA::frameid2pose_map_t  &spantree,
        const typename RBA::TOpenGLRepresentationOptions &options,
        mrpt::opengl::CSetOfObjects& scene)
    {
        using namespace mrpt::math;
        using mrpt::utils::DEG2RAD;
        using mrpt::poses::CPose3D;

        // For each fixed known LM, add a point to a point cloud
        //  and a text label with the landmark ID:
        mrpt::opengl::CPointCloudPtr  gl_lms = mrpt::opengl::CPointCloud::Create();
        gl_lms->setPointSize(5);
        gl_lms->setColor(0,0,1);

        scene.insert(gl_lms);

        vector<typename RBA::TRelativeLandmarkPosMap::const_iterator> lms_to_draw;
        vector<const typename RBA::hessian_traits_t::TSparseBlocksHessian_f::matrix_t *> lms_to_draw_inf_covs;

        for (typename RBA::TRelativeLandmarkPosMap::const_iterator itLM = rba.get_rba_state().known_lms.begin();itLM != rba.get_rba_state().known_lms.end();++itLM)
        {
            lms_to_draw.push_back(itLM);
            lms_to_draw_inf_covs.push_back( NULL );
        }

        const size_t nKnown = lms_to_draw.size();

        if (options.draw_unknown_feats)
        {
            for (typename RBA::TRelativeLandmarkPosMap::const_iterator itLM = rba.get_rba_state().unknown_lms.begin();itLM != rba.get_rba_state().unknown_lms.end();++itLM)
            {
                lms_to_draw.push_back(itLM);

                typename RBA::hessian_traits_t::landmarks2infmatrix_t::const_iterator it_inf = rba.get_rba_state().unknown_lms_inf_matrices.find(itLM->first);

                if (it_inf != rba.get_rba_state().unknown_lms_inf_matrices.end())
                        lms_to_draw_inf_covs.push_back( &it_inf->second );
                else    lms_to_draw_inf_covs.push_back(NULL);
            }
        }

        const mrpt::utils::TColorf col_known_lms(1.f,1.f,0.f);
        const mrpt::utils::TColorf col_unknown_lms(1.f,0.f,0.f);

        for (size_t i=0;i<lms_to_draw.size();i++)
        {
            const typename RBA::TRelativeLandmarkPosMap::const_iterator &itLM = lms_to_draw[i];
            const bool is_known = (i<nKnown);

            // Don't draw those unknown LMs which hasn't been estimated not even once:
            //if (!is_known && lms_to_draw_inf_covs[i]==NULL) continue;

            mrpt::poses::CPose3D base_pose;
            if (itLM->second.id_frame_base!=root_keyframe)
            {
                typename RBA::frameid2pose_map_t::const_iterator itBaseNode = spantree.find(itLM->second.id_frame_base);
                if(itBaseNode==spantree.end())
                    continue;
                base_pose = itBaseNode->second.pose; // Inverse!
            }
            else
            {
                // It's the origin.
            }

            // If landmark_t defines this kind of renderer, we
         mrpt::math::TPoint3D p_wrt_base;
            RBA::landmark_t::relativeEuclideanLocation(itLM->second.pos, p_wrt_base);

         mrpt::math::TPoint3D p_global;
            base_pose.composePoint(p_wrt_base,p_global);

            gl_lms->insertPoint(p_global.x,p_global.y,p_global.z);

            if( options.show_unknown_feats_ids )
            {
                // Add text label:
                mrpt::opengl::CText3DPtr  gl_txt = mrpt::opengl::CText3D::Create(
                    mrpt::format("%u",static_cast<unsigned int>( itLM->first)),
                    "mono", 0.15,
                    mrpt::opengl::NICE );
                gl_txt->setPose(CPose3D(p_global.x,p_global.y,p_global.z,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(90)));
                gl_txt->setColor( is_known ? col_known_lms : col_unknown_lms );

                scene.insert(gl_txt);
            }

            // Uncertainty ellipse?
            if (options.draw_unknown_feats_ellipses && lms_to_draw_inf_covs[i] )
            {
                double min_inf_diag=std::numeric_limits<double>::max();
                for (size_t k=0;k<RBA::LM_DIMS;k++)
                    mrpt::utils::keep_min(min_inf_diag, (*lms_to_draw_inf_covs[i])(k,k));
                if (min_inf_diag<1e-5) continue; // Too large covariance!

                mrpt::math::CMatrixFixedNumeric<double,RBA::LM_DIMS,RBA::LM_DIMS> cov(mrpt::math::UNINITIALIZED_MATRIX);
                lms_to_draw_inf_covs[i]->inv(cov);

                mrpt::opengl::CEllipsoidPtr gl_ellip = mrpt::opengl::CEllipsoid::Create();
                gl_ellip->setQuantiles( options.draw_unknown_feats_ellipses_quantiles );
                gl_ellip->enableDrawSolid3D(false);

                gl_ellip->setCovMatrix(cov);
                CPose3D ellip_pose = base_pose;
                ellip_pose.x(p_global.x);
                ellip_pose.y(p_global.y);
                ellip_pose.z(p_global.z);

                gl_ellip->setPose(ellip_pose);
                scene.insert(gl_ellip);
            }
        }
    }
};

template <> struct LandmarkRendererBase<landmark_rendering_as_pose_constraints>
{
    template <class RBA>
    static void render(
        const RBA &rba,
        const srba::TKeyFrameID root_keyframe,
        const typename RBA::frameid2pose_map_t  &spantree,
        const typename RBA::TOpenGLRepresentationOptions &options,
        mrpt::opengl::CSetOfObjects& scene)
    {
        MRPT_UNUSED_PARAM(options);
        MRPT_UNUSED_PARAM(root_keyframe);
        using namespace mrpt::math;

        mrpt::opengl::CSetOfLinesPtr gl_edges = mrpt::opengl::CSetOfLines::Create();
        gl_edges->setLineWidth(1);
        gl_edges->setColor(0,0,1);

        scene.insert(gl_edges);

        // For each KF: check all its "observations"
        for (typename RBA::frameid2pose_map_t::const_iterator it=spantree.begin();it!=spantree.end();++it)
        {
            const TKeyFrameID kf_id = it->first;
            const typename RBA::pose_flag_t & pf = it->second;

            const typename RBA::keyframe_info &kfi = rba.get_rba_state().keyframes[kf_id];

            for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
            {
                const typename RBA::k2f_edge_t * k2f = kfi.adjacent_k2f_edges[i];
                const TKeyFrameID other_kf_id = k2f->feat_rel_pos->id_frame_base;
                if (kf_id==other_kf_id)
                    continue; // It's not an constraint with ANOTHER keyframe

                // Is the other KF in the spanning tree?
                typename RBA::frameid2pose_map_t::const_iterator other_it=spantree.find(other_kf_id);
                if (other_it==spantree.end()) continue;

                const typename RBA::pose_flag_t & other_pf = other_it->second;

                // Add edge between the two KFs to represent the pose constraint:
                mrpt::poses::CPose3D p1 = mrpt::poses::CPose3D(pf.pose);  // Convert to 3D
                mrpt::poses::CPose3D p2 = mrpt::poses::CPose3D(other_pf.pose);

                gl_edges->appendLine( p1.x(),p1.y(),p1.z()+0.10, p2.x(),p2.y(),p2.z()+0.10 );
            }

        } // end for each KF
    }
};

template <> struct LandmarkRendererBase<landmark_rendering_none>
{
    template <class RBA>
    static void render(
        const RBA &rba,
        const srba::TKeyFrameID root_keyframe,
        const typename RBA::frameid2pose_map_t  &spantree,
        const typename RBA::TOpenGLRepresentationOptions &options,
        mrpt::opengl::CSetOfObjects& scene)
    {
        MRPT_UNUSED_PARAM(rba);
        MRPT_UNUSED_PARAM(root_keyframe);
        MRPT_UNUSED_PARAM(spantree);
        MRPT_UNUSED_PARAM(options);
        MRPT_UNUSED_PARAM(scene);
        // Nothing to render
    }
};

} // End of namespaces