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 );
61 for (
typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
63 const CPose3D p = CPose3D(itNod->second);
65 keep_min( BB_min.x, p.x() );
66 keep_min( BB_min.y, p.y() );
67 keep_min( BB_min.z, p.z() );
69 keep_max( BB_max.
x, p.x() );
70 keep_max( BB_max.
y, p.y() );
71 keep_max( BB_max.
z, p.z() );
75 const double grid_frequency = 5.0;
77 grid->setColor(0.3,0.3,0.3);
82 if (nodes_point_size>0)
86 pnts->setPointSize(nodes_point_size);
89 for (
typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
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)
103 for (
typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
105 const CPose3D p = CPose3D(itNod->second);
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);
123 for (
typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
126 const TNodeID node_id_start = g.edges_store_inverse_poses ? itEd->first.second : itEd->first.first;
129 typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.find(node_id_start);
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();
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 );
164 for (
typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
166 const TNodeID id1 = itEd->first.first;
167 const TNodeID id2 = itEd->first.second;
170 typename GRAPH_T::global_poses_t::const_iterator itNod1 = g.nodes.find(id1);
171 typename GRAPH_T::global_poses_t::const_iterator itNod2 = g.nodes.find(id2);
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 );