22 template <
class GRAPH_T>
32 using constraint_t =
typename GRAPH_T::constraint_t;
34 const bool is_3D_graph = constraint_t::is_3D();
40 const bool show_ID_labels =
42 const bool show_ground_grid =
44 const bool show_edges =
46 const bool show_node_corners =
48 const bool show_edge_rel_poses =
50 const double nodes_point_size =
52 const double nodes_corner_scale =
54 const double nodes_edges_corner_scale =
57 "nodes_point_color", (
unsigned int)0xA0A0A0);
58 const unsigned int edge_color =
61 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
69 for (
auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
71 const CPose3D p = CPose3D(
84 const double grid_frequency = 5.0;
86 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
87 grid->setColor(0.3f, 0.3f, 0.3f);
92 if (nodes_point_size > 0)
97 pnts->setPointSize(nodes_point_size);
100 for (
auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
102 const CPose3D p = CPose3D(
104 pnts->insertPoint(p.x(), p.y(), p.z());
111 if (show_node_corners || show_ID_labels)
113 for (
auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
115 const CPose3D p = CPose3D(
121 nodes_corner_scale, 1.0 )
123 nodes_corner_scale, 1.0 ))
124 : std::make_shared<CSetOfObjects>();
125 gl_corner->setPose(p);
129 format(
"%u",
static_cast<unsigned int>(itNod->first)));
130 gl_corner->enableShowName();
132 ret->insert(gl_corner);
136 if (show_edge_rel_poses)
139 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
141 for (
const auto& edge : g)
144 const auto node_id_start = g.edges_store_inverse_poses
149 auto itNod = g.nodes.find(node_id_start);
150 if (itNod != g.nodes.end())
152 const CPose3D pSource = CPose3D(itNod->second);
156 gl_rel_edge->setPose(pSource);
158 const auto& edge_pose = edge.second.getPoseMean();
161 auto gl_edge_corner =
164 nodes_edges_corner_scale, 1.0 )
166 nodes_edges_corner_scale, 1.0 ));
168 gl_edge_corner->setPose(edge_pose);
169 gl_rel_edge->insert(gl_edge_corner);
172 0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
174 gl_line->setColor_u8(col8bit);
175 gl_line->setLineWidth(edge_width);
176 gl_rel_edge->insert(gl_line);
178 ret->insert(gl_rel_edge);
187 edge_color & 0xffffff, edge_color >> 24);
189 gl_edges->setColor_u8(col8bit);
190 gl_edges->setLineWidth(edge_width);
192 for (
const auto& edge : g)
194 const auto id1 = edge.first.first;
195 const auto id2 = edge.first.second;
198 auto itNod1 = g.nodes.find(id1);
199 auto itNod2 = g.nodes.find(id2);
200 if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end())
202 const CPose3D p1 = CPose3D(itNod1->second);
203 const CPose3D p2 = CPose3D(itNod2->second);
204 gl_edges->appendLine(
209 ret->insert(gl_edges);