9 #ifndef opengl_graph_tools_impl_H
10 #define opengl_graph_tools_impl_H
27 template <
class GRAPH_T>
37 using constraint_t =
typename GRAPH_T::constraint_t;
39 const bool is_3D_graph = constraint_t::is_3D();
45 const bool show_ID_labels =
47 const bool show_ground_grid =
49 const bool show_edges =
51 const bool show_node_corners =
53 const bool show_edge_rel_poses =
55 const double nodes_point_size =
57 const double nodes_corner_scale =
59 const double nodes_edges_corner_scale =
62 "nodes_point_color", (
unsigned int)0xA0A0A0);
63 const unsigned int edge_color =
66 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
76 itNod !=
g.nodes.end(); ++itNod)
78 const CPose3D
p = CPose3D(
91 const double grid_frequency = 5.0;
93 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
94 grid->setColor(0.3, 0.3, 0.3);
99 if (nodes_point_size > 0)
104 pnts->setPointSize(nodes_point_size);
109 itNod !=
g.nodes.end(); ++itNod)
111 const CPose3D
p = CPose3D(
113 pnts->insertPoint(
p.x(),
p.y(),
p.z());
116 pnts->enablePointSmooth();
122 if (show_node_corners || show_ID_labels)
126 itNod !=
g.nodes.end(); ++itNod)
128 const CPose3D
p = CPose3D(
134 nodes_corner_scale, 1.0 )
136 nodes_corner_scale, 1.0 ))
137 : mrpt::make_aligned_shared<CSetOfObjects>();
138 gl_corner->setPose(
p);
142 format(
"%u",
static_cast<unsigned int>(itNod->first)));
143 gl_corner->enableShowName();
145 ret->insert(gl_corner);
149 if (show_edge_rel_poses)
152 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
154 for (
const auto& edge :
g)
157 const auto node_id_start =
g.edges_store_inverse_poses
162 auto itNod =
g.nodes.find(node_id_start);
163 if (itNod !=
g.nodes.end())
165 const CPose3D pSource = CPose3D(itNod->second);
169 gl_rel_edge->setPose(pSource);
171 const auto& edge_pose = edge.second.getPoseMean();
174 auto gl_edge_corner =
177 nodes_edges_corner_scale, 1.0 )
179 nodes_edges_corner_scale, 1.0 ));
181 gl_edge_corner->setPose(edge_pose);
182 gl_rel_edge->insert(gl_edge_corner);
185 0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
187 gl_line->setColor_u8(col8bit);
188 gl_line->setLineWidth(edge_width);
189 gl_rel_edge->insert(gl_line);
191 ret->insert(gl_rel_edge);
200 edge_color & 0xffffff, edge_color >> 24);
202 gl_edges->setColor_u8(col8bit);
203 gl_edges->setLineWidth(edge_width);
205 for (
const auto& edge :
g)
207 const auto id1 = edge.first.first;
208 const auto id2 = edge.first.second;
211 auto itNod1 =
g.nodes.find(id1);
212 auto itNod2 =
g.nodes.find(id2);
213 if (itNod1 !=
g.nodes.end() && itNod2 !=
g.nodes.end())
215 const CPose3D p1 = CPose3D(itNod1->second);
216 const CPose3D p2 = CPose3D(itNod2->second);
217 gl_edges->appendLine(
222 ret->insert(gl_edges);