graph_tools_impl.h

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 |
   +------------------------------------------------------------------------+ */
#ifndef opengl_graph_tools_impl_H
#define opengl_graph_tools_impl_H

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

namespace mrpt
{
namespace opengl
{
namespace graph_tools
{
template <class GRAPH_T>
CSetOfObjects::Ptr graph_visualize(
        const GRAPH_T& g, const mrpt::utils::TParametersDouble& extra_params)
{
        MRPT_TRY_START

        using mrpt::poses::CPose3D;
        using mrpt::math::TPose3D;
        using namespace mrpt::utils;

        // Is a 2D or 3D graph network?
        typedef typename GRAPH_T::constraint_t constraint_t;

        const bool is_3D_graph = constraint_t::is_3D();

        // create opengl obejct to be filled.
        CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();

        // graph visualization parameters
        const bool show_ID_labels =
                0 != extra_params.getWithDefaultVal("show_ID_labels", 0);
        const bool show_ground_grid =
                0 != extra_params.getWithDefaultVal("show_ground_grid", 1);
        const bool show_edges =
                0 != extra_params.getWithDefaultVal("show_edges", 1);
        const bool show_node_corners =
                0 != extra_params.getWithDefaultVal("show_node_corners", 1);
        const bool show_edge_rel_poses =
                0 != extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
        const double nodes_point_size =
                extra_params.getWithDefaultVal("nodes_point_size", 0.);
        const double nodes_corner_scale =
                extra_params.getWithDefaultVal("nodes_corner_scale", 0.7);
        const double nodes_edges_corner_scale =
                extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4);
        const unsigned int nodes_point_color = extra_params.getWithDefaultVal(
                "nodes_point_color", (unsigned int)0xA0A0A0);
        const unsigned int edge_color =
                extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
        const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal(
                "edge_rel_poses_color", (unsigned int)0x40FF8000);
        const double edge_width = extra_params.getWithDefaultVal("edge_width", 2.);

        if (show_ground_grid)
        {
                // Estimate bounding box.
                mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.);

                for (typename GRAPH_T::global_poses_t::const_iterator itNod =
                                 g.nodes.begin();
                         itNod != g.nodes.end(); ++itNod)
                {
                        const CPose3D p = CPose3D(
                                itNod->second);  // Convert to 3D from whatever its real type.

                        keep_min(BB_min.x, p.x());
                        keep_min(BB_min.y, p.y());
                        keep_min(BB_min.z, p.z());

                        keep_max(BB_max.x, p.x());
                        keep_max(BB_max.y, p.y());
                        keep_max(BB_max.z, p.z());
                }

                // Create ground plane:
                const double grid_frequency = 5.0;
                CGridPlaneXY::Ptr grid = mrpt::make_aligned_shared<CGridPlaneXY>(
                        BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
                grid->setColor(0.3, 0.3, 0.3);
                ret->insert(grid);
        }  // end show_ground_grid

        // Draw nodes as thick points:
        if (nodes_point_size > 0)
        {
                CPointCloud::Ptr pnts = mrpt::make_aligned_shared<CPointCloud>();
                pnts->setColor(TColorf(TColor(nodes_point_color)));
                pnts->setPointSize(nodes_point_size);

                // Add nodes:
                for (typename GRAPH_T::global_poses_t::const_iterator itNod =
                                 g.nodes.begin();
                         itNod != g.nodes.end(); ++itNod)
                {
                        const CPose3D p = CPose3D(
                                itNod->second);  // Convert to 3D from whatever its real type.
                        pnts->insertPoint(p.x(), p.y(), p.z());
                }

                pnts->enablePointSmooth();

                ret->insert(pnts);
        }  // end draw node points

        // Show a 2D corner at each node (or just an empty object with the ID label)
        if (show_node_corners || show_ID_labels)
        {
                for (typename GRAPH_T::global_poses_t::const_iterator itNod =
                                 g.nodes.begin();
                         itNod != g.nodes.end(); ++itNod)
                {
                        const CPose3D p = CPose3D(
                                itNod->second);  // Convert to 3D from whatever its real type.
                        CSetOfObjects::Ptr gl_corner =
                                show_node_corners
                                        ? (is_3D_graph
                                                   ? stock_objects::CornerXYZSimple(
                                                                 nodes_corner_scale, 1.0 /*line width*/)
                                                   : stock_objects::CornerXYSimple(
                                                                 nodes_corner_scale, 1.0 /*line width*/))
                                        : mrpt::make_aligned_shared<CSetOfObjects>();
                        gl_corner->setPose(p);
                        if (show_ID_labels)  // don't show IDs twice!
                        {
                                gl_corner->setName(
                                        format("%u", static_cast<unsigned int>(itNod->first)));
                                gl_corner->enableShowName();
                        }
                        ret->insert(gl_corner);
                }
        }  // end draw node corners

        if (show_edge_rel_poses)
        {
                const TColor col8bit(
                        edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);

                for (typename GRAPH_T::const_iterator itEd = g.begin(); itEd != g.end();
                         ++itEd)
                {
                        // Node ID of the source pose:
                        const TNodeID node_id_start = g.edges_store_inverse_poses
                                                                                          ? itEd->first.second
                                                                                          : itEd->first.first;

                        // Draw only if we have the global coords of starting nodes:
                        typename GRAPH_T::global_poses_t::const_iterator itNod =
                                g.nodes.find(node_id_start);
                        if (itNod != g.nodes.end())
                        {
                                const CPose3D pSource = CPose3D(itNod->second);
                                // Create a set of objects at that pose and do the rest in
                                // relative coords:
                                mrpt::opengl::CSetOfObjects::Ptr gl_rel_edge =
                                        mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
                                gl_rel_edge->setPose(pSource);

                                const typename GRAPH_T::constraint_no_pdf_t& edge_pose =
                                        itEd->second.getPoseMean();
                                const mrpt::poses::CPoint3D edge_pose_pt =
                                        mrpt::poses::CPoint3D(edge_pose);

                                mrpt::opengl::CSetOfObjects::Ptr gl_edge_corner =
                                        (is_3D_graph
                                                 ? stock_objects::CornerXYZSimple(
                                                           nodes_edges_corner_scale, 1.0 /*line width*/)
                                                 : stock_objects::CornerXYSimple(
                                                           nodes_edges_corner_scale, 1.0 /*line width*/));

                                gl_edge_corner->setPose(edge_pose);
                                gl_rel_edge->insert(gl_edge_corner);

                                mrpt::opengl::CSimpleLine::Ptr gl_line =
                                        mrpt::make_aligned_shared<mrpt::opengl::CSimpleLine>(
                                                0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
                                                edge_pose_pt.z());
                                gl_line->setColor_u8(col8bit);
                                gl_line->setLineWidth(edge_width);
                                gl_rel_edge->insert(gl_line);

                                ret->insert(gl_rel_edge);
                        }
                }
        }

        if (show_edges)
        {
                CSetOfLines::Ptr gl_edges = mrpt::make_aligned_shared<CSetOfLines>();
                const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);

                gl_edges->setColor_u8(col8bit);
                gl_edges->setLineWidth(edge_width);

                for (typename GRAPH_T::const_iterator itEd = g.begin(); itEd != g.end();
                         ++itEd)
                {
                        const TNodeID id1 = itEd->first.first;
                        const TNodeID id2 = itEd->first.second;

                        // Draw only if we have the global coords of both nodes:
                        typename GRAPH_T::global_poses_t::const_iterator itNod1 =
                                g.nodes.find(id1);
                        typename GRAPH_T::global_poses_t::const_iterator itNod2 =
                                g.nodes.find(id2);
                        if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end())
                        {
                                const CPose3D p1 = CPose3D(itNod1->second);
                                const CPose3D p2 = CPose3D(itNod2->second);
                                gl_edges->appendLine(
                                        mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
                                        mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
                        }
                }
                ret->insert(gl_edges);

        }  // end draw edges

        return ret;

        MRPT_TRY_END
}

}  // end of graph_tools namespace
}  // end of opengl namespace
}  // end of mrpt namespace

#endif