16 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
17 class EDGE_ANNOTATIONS>
27 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
28 class EDGE_ANNOTATIONS>
35 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
36 class EDGE_ANNOTATIONS>
38 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
46 const bool show_ID_labels =
48 const bool show_ground_grid =
51 const bool show_node_corners =
53 const bool show_edge_rel_poses =
55 const double nodes_point_size =
60 this->drawGroundGrid(
object, &viz_params);
63 if (nodes_point_size > 0)
65 this->drawNodePoints(
object, &viz_params);
68 if (show_node_corners || show_ID_labels)
70 this->drawNodeCorners(
object, &viz_params);
73 if (show_edge_rel_poses)
75 this->drawEdgeRelPoses(
object, &viz_params);
80 this->drawEdges(
object, &viz_params);
85 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
86 class EDGE_ANNOTATIONS>
88 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
94 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
99 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
114 const double grid_frequency = 5.0;
116 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
117 grid->setColor(0.3f, 0.3f, 0.3f);
118 object->insert(grid);
122 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
123 class EDGE_ANNOTATIONS>
125 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
130 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
135 const double nodes_point_size =
138 "nodes_point_color", (
unsigned int)0xA0A0A0);
142 pnts->setPointSize(nodes_point_size);
145 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
149 pnts->insertPoint(p.
x(), p.
y(), p.z());
152 object->insert(pnts);
156 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
157 class EDGE_ANNOTATIONS>
159 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
167 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
169 const bool show_node_corners =
171 const bool show_ID_labels =
173 const double nodes_corner_scale =
176 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
184 nodes_corner_scale, 1.0 )
186 nodes_corner_scale, 1.0 ))
187 : std::make_shared<CSetOfObjects>();
188 gl_corner->setPose(p);
192 format(
"%u",
static_cast<unsigned int>(n_it->first)));
193 gl_corner->enableShowName();
195 object->insert(gl_corner);
200 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
201 class EDGE_ANNOTATIONS>
203 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
211 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
213 const double nodes_edges_corner_scale =
216 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
218 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
221 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
224 const TNodeID node_id_start = m_graph.edges_store_inverse_poses
225 ? edge_it->first.second
226 : edge_it->first.first;
229 auto n_it = m_graph.nodes.find(node_id_start);
230 if (n_it != m_graph.nodes.end())
237 gl_rel_edge->setPose(pSource);
240 edge_it->second.getPoseMean();
247 nodes_edges_corner_scale, 1.0 )
249 nodes_edges_corner_scale, 1.0 ));
251 gl_edge_corner->setPose(edge_pose);
252 gl_rel_edge->insert(gl_edge_corner);
256 0, 0, 0, edge_pose_pt.
x(), edge_pose_pt.
y(),
259 gl_line->setColor_u8(col8bit);
260 gl_line->setLineWidth(edge_width);
261 gl_rel_edge->insert(gl_line);
263 object->insert(gl_rel_edge);
269 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
270 class EDGE_ANNOTATIONS>
272 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
280 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
283 const unsigned int edge_color =
287 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
289 gl_edges->setColor_u8(col8bit);
290 gl_edges->setLineWidth(edge_width);
293 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
295 const TNodeID id1 = edge_it->first.first;
296 const TNodeID id2 = edge_it->first.second;
299 auto n_it1 = m_graph.nodes.find(id1);
300 auto n_it2 = m_graph.nodes.find(id2);
301 if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
305 gl_edges->appendLine(
310 object->insert(gl_edges);