9 #ifndef opengl_graph_tools_impl_H
10 #define opengl_graph_tools_impl_H
25 template<
class GRAPH_T>
37 typedef typename GRAPH_T::constraint_t constraint_t;
39 const bool is_3D_graph = constraint_t::is_3D();
43 const bool show_ID_labels = 0!=extra_params.
getWithDefaultVal(
"show_ID_labels", 0);
44 const bool show_ground_grid = 0!=extra_params.
getWithDefaultVal(
"show_ground_grid", 1);
46 const bool show_node_corners = 0!=extra_params.
getWithDefaultVal(
"show_node_corners", 1);
47 const bool show_edge_rel_poses = 0!=extra_params.
getWithDefaultVal(
"show_edge_rel_poses", 0);
48 const double nodes_point_size = extra_params.
getWithDefaultVal(
"nodes_point_size", 0. );
49 const double nodes_corner_scale = extra_params.
getWithDefaultVal(
"nodes_corner_scale", 0.7 );
50 const double nodes_edges_corner_scale = extra_params.
getWithDefaultVal(
"nodes_edges_corner_scale", 0.4 );
51 const unsigned int nodes_point_color = extra_params.
getWithDefaultVal(
"nodes_point_color", (
unsigned int)0xA0A0A0 );
52 const unsigned int edge_color = extra_params.
getWithDefaultVal(
"edge_color", (
unsigned int)0x400000FF );
53 const unsigned int edge_rel_poses_color = extra_params.
getWithDefaultVal(
"edge_rel_poses_color", (
unsigned int)0x40FF8000 );
63 const CPose3D p = CPose3D(itNod->second);
75 const double grid_frequency = 5.0;
76 CGridPlaneXYPtr grid =
CGridPlaneXY::Create(BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
77 grid->setColor(0.3,0.3,0.3);
82 if (nodes_point_size>0)
86 pnts->setPointSize(nodes_point_size);
91 const CPose3D p = CPose3D(itNod->second);
92 pnts->insertPoint(p.x(),p.y(), p.z() );
95 pnts->enablePointSmooth();
101 if (show_node_corners || show_ID_labels)
105 const CPose3D p = CPose3D(itNod->second);
106 CSetOfObjectsPtr gl_corner = show_node_corners ?
109 gl_corner->setPose( p );
112 gl_corner->setName(
format(
"%u",static_cast<unsigned int>(itNod->first) ));
113 gl_corner->enableShowName();
115 ret->insert( gl_corner );
119 if (show_edge_rel_poses)
121 const TColor col8bit(edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
126 const TNodeID node_id_start = g.edges_store_inverse_poses ? itEd->first.second : itEd->first.first;
130 if (itNod!=g.nodes.end())
132 const CPose3D pSource = CPose3D(itNod->second);
135 gl_rel_edge->setPose(pSource);
137 const typename GRAPH_T::constraint_no_pdf_t & edge_pose = itEd->second.getPoseMean();
140 mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
143 gl_edge_corner->setPose(edge_pose);
144 gl_rel_edge->insert(gl_edge_corner);
147 gl_line->setColor_u8( col8bit );
148 gl_line->setLineWidth(edge_width);
149 gl_rel_edge->insert(gl_line);
151 ret->insert( gl_rel_edge );
159 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
161 gl_edges->setColor_u8( col8bit );
162 gl_edges->setLineWidth( edge_width );
166 const TNodeID id1 = itEd->first.first;
167 const TNodeID id2 = itEd->first.second;
172 if (itNod1!=g.nodes.end() && itNod2!=g.nodes.end())
174 const CPose3D p1 = CPose3D(itNod1->second);
175 const CPose3D p2 = CPose3D(itNod2->second);
179 ret->insert( gl_edges );