9 #ifndef CVISUALIZER_IMPL_H
10 #define CVISUALIZER_IMPL_H
20 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
21 class EDGE_ANNOTATIONS>
22 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
28 m_is_3D_graph = constraint_t::is_3D();
30 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
31 class EDGE_ANNOTATIONS>
32 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
33 EDGE_ANNOTATIONS>::~CVisualizer()
39 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
40 class EDGE_ANNOTATIONS>
41 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
50 const bool show_ID_labels =
52 const bool show_ground_grid =
55 const bool show_node_corners =
57 const bool show_edge_rel_poses =
59 const double nodes_point_size =
64 this->drawGroundGrid(
object, &viz_params);
67 if (nodes_point_size > 0)
69 this->drawNodePoints(
object, &viz_params);
72 if (show_node_corners || show_ID_labels)
74 this->drawNodeCorners(
object, &viz_params);
77 if (show_edge_rel_poses)
79 this->drawEdgeRelPoses(
object, &viz_params);
84 this->drawEdges(
object, &viz_params);
88 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
89 class EDGE_ANNOTATIONS>
90 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
97 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
103 m_graph.nodes.begin();
104 n_it != m_graph.nodes.end(); ++n_it)
119 const double grid_frequency = 5.0;
121 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
122 grid->setColor(0.3, 0.3, 0.3);
123 object->insert(grid);
126 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
127 class EDGE_ANNOTATIONS>
128 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
134 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
139 const double nodes_point_size =
142 "nodes_point_color", (
unsigned int)0xA0A0A0);
146 pnts->setPointSize(nodes_point_size);
150 m_graph.nodes.begin();
151 n_it != m_graph.nodes.end(); ++n_it)
155 pnts->insertPoint(
p.x(),
p.y(),
p.z());
158 pnts->enablePointSmooth();
159 object->insert(pnts);
162 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
163 class EDGE_ANNOTATIONS>
164 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
173 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
175 const bool show_node_corners =
177 const bool show_ID_labels =
179 const double nodes_corner_scale =
183 m_graph.nodes.begin();
184 n_it != m_graph.nodes.end(); ++n_it)
192 nodes_corner_scale, 1.0 )
194 nodes_corner_scale, 1.0 ))
195 : mrpt::make_aligned_shared<CSetOfObjects>();
196 gl_corner->setPose(
p);
200 format(
"%u",
static_cast<unsigned int>(n_it->first)));
201 gl_corner->enableShowName();
203 object->insert(gl_corner);
207 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
208 class EDGE_ANNOTATIONS>
209 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
218 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
220 const double nodes_edges_corner_scale =
223 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
225 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
228 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
231 const TNodeID node_id_start = m_graph.edges_store_inverse_poses
232 ? edge_it->first.second
233 : edge_it->first.first;
237 m_graph.nodes.find(node_id_start);
238 if (n_it != m_graph.nodes.end())
244 mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
245 gl_rel_edge->setPose(pSource);
248 edge_it->second.getPoseMean();
255 nodes_edges_corner_scale, 1.0 )
257 nodes_edges_corner_scale, 1.0 ));
259 gl_edge_corner->setPose(edge_pose);
260 gl_rel_edge->insert(gl_edge_corner);
264 0, 0, 0, edge_pose_pt.
x(), edge_pose_pt.
y(),
267 gl_line->setColor_u8(col8bit);
268 gl_line->setLineWidth(edge_width);
269 gl_rel_edge->insert(gl_line);
271 object->insert(gl_rel_edge);
276 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
277 class EDGE_ANNOTATIONS>
278 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
287 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
290 const unsigned int edge_color =
294 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
296 gl_edges->setColor_u8(col8bit);
297 gl_edges->setLineWidth(edge_width);
301 edge_it != m_graph.end(); ++edge_it)
303 const TNodeID id1 = edge_it->first.first;
304 const TNodeID id2 = edge_it->first.second;
308 m_graph.nodes.find(id1);
310 m_graph.nodes.find(id2);
311 if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
315 gl_edges->appendLine(
320 object->insert(gl_edges);