Main MRPT website > C++ reference
MRPT logo
graph_tools_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef opengl_graph_tools_impl_H
29 #define opengl_graph_tools_impl_H
30 
31 namespace mrpt
32 {
33  namespace opengl
34  {
35  namespace graph_tools
36  {
37  template<class GRAPH_T>
38  CSetOfObjectsPtr graph_visualize(
39  const GRAPH_T &g,
40  const mrpt::utils::TParametersDouble &extra_params)
41  {
42  MRPT_TRY_START
43 
44  // Is a 2D or 3D graph network?
45  typedef typename GRAPH_T::constraint_t constraint_t;
46 
47  const bool is_3D_graph = constraint_t::is_3D();
48 
49  CSetOfObjectsPtr ret = CSetOfObjects::Create();
50 
51  const bool show_ID_labels = 0!=extra_params.getWithDefaultVal("show_ID_labels", 0);
52  const bool show_ground_grid = 0!=extra_params.getWithDefaultVal("show_ground_grid", 1);
53  const bool show_edges = 0!=extra_params.getWithDefaultVal("show_edges", 1);
54  const bool show_node_corners = 0!=extra_params.getWithDefaultVal("show_node_corners", 1);
55  const bool show_edge_rel_poses = 0!=extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
56  const double nodes_point_size = extra_params.getWithDefaultVal("nodes_point_size", 0. );
57  const double nodes_corner_scale = extra_params.getWithDefaultVal("nodes_corner_scale", 0.7 );
58  const double nodes_edges_corner_scale = extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4 );
59  const unsigned int nodes_point_color = extra_params.getWithDefaultVal("nodes_point_color", (unsigned int)0xA0A0A0 );
60  const unsigned int edge_color = extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF );
61  const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal("edge_rel_poses_color", (unsigned int)0x40FF8000 );
62  const double edge_width = extra_params.getWithDefaultVal("edge_width", 2. );
63 
64  if (show_ground_grid)
65  {
66  // Estimate bounding box.
67  TPoint3D BB_min(-10.,-10.,0.), BB_max(10.,10.,0.);
68 
69  for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
70  {
71  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
72 
73  keep_min( BB_min.x, p.x() );
74  keep_min( BB_min.y, p.y() );
75  keep_min( BB_min.z, p.z() );
76 
77  keep_max( BB_max.x, p.x() );
78  keep_max( BB_max.y, p.y() );
79  keep_max( BB_max.z, p.z() );
80  }
81 
82  // Create ground plane:
83  const double grid_frequency = 5.0;
84  CGridPlaneXYPtr grid = CGridPlaneXY::Create(BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
85  grid->setColor(0.3,0.3,0.3);
86  ret->insert( grid );
87  } // end show_ground_grid
88 
89  // Draw nodes as thick points:
90  if (nodes_point_size!=0)
91  {
92  ASSERT_(nodes_point_size>=0);
93 
94  CPointCloudPtr pnts = CPointCloud::Create();
95  pnts->setColor( TColorf(TColor(nodes_point_color)) );
96  pnts->setPointSize(nodes_point_size);
97 
98  // Add nodes:
99  for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
100  {
101  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
102  pnts->insertPoint(p.x(),p.y(), p.z() );
103  }
104 
105  pnts->enablePointSmooth();
106 
107  ret->insert(pnts);
108  } // end draw node points
109 
110  // Show a 2D corner at each node (or just an empty object with the ID label)
111  if (show_node_corners || show_ID_labels)
112  {
113  for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
114  {
115  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
116  CSetOfObjectsPtr gl_corner = show_node_corners ?
117  (is_3D_graph ? stock_objects::CornerXYZSimple(nodes_corner_scale, 1.0 /*line width*/ ) : stock_objects::CornerXYSimple(nodes_corner_scale, 1.0 /*line width*/ ))
118  : CSetOfObjects::Create();
119  gl_corner->setPose( p );
120  if (show_ID_labels) // don't show IDs twice!
121  {
122  gl_corner->setName(format("%u",static_cast<unsigned int>(itNod->first) ));
123  gl_corner->enableShowName();
124  }
125  ret->insert( gl_corner );
126  }
127  } // end draw node corners
128 
129  if (show_edge_rel_poses)
130  {
131  const TColor col8bit(edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
132 
133  for (typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
134  {
135  // Node ID of the source pose:
136  const TNodeID node_id_start = g.edges_store_inverse_poses ? itEd->first.second : itEd->first.first;
137 
138  // Draw only if we have the global coords of starting nodes:
139  typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.find(node_id_start);
140  if (itNod!=g.nodes.end())
141  {
142  const CPose3D pSource = CPose3D(itNod->second);
143  // Create a set of objects at that pose and do the rest in relative coords:
144  mrpt::opengl::CSetOfObjectsPtr gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
145  gl_rel_edge->setPose(pSource);
146 
147  const typename GRAPH_T::constraint_no_pdf_t & edge_pose = itEd->second.getPoseMean();
148  const mrpt::poses::CPoint3D edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
149 
150  mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
151  (is_3D_graph ? stock_objects::CornerXYZSimple(nodes_edges_corner_scale, 1.0 /*line width*/ ) : stock_objects::CornerXYSimple(nodes_edges_corner_scale, 1.0 /*line width*/ ));
152 
153  gl_edge_corner->setPose(edge_pose);
154  gl_rel_edge->insert(gl_edge_corner);
155 
156  mrpt::opengl::CSimpleLinePtr gl_line = mrpt::opengl::CSimpleLine::Create(0,0,0, edge_pose_pt.x(), edge_pose_pt.y(), edge_pose_pt.z() );
157  gl_line->setColor_u8( col8bit );
158  gl_line->setLineWidth(edge_width);
159  gl_rel_edge->insert(gl_line);
160 
161  ret->insert( gl_rel_edge );
162  }
163  }
164  }
165 
166  if (show_edges)
167  {
168  CSetOfLinesPtr gl_edges = CSetOfLines::Create();
169  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
170 
171  gl_edges->setColor_u8( col8bit );
172  gl_edges->setLineWidth( edge_width );
173 
174  for (typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
175  {
176  const TNodeID id1 = itEd->first.first;
177  const TNodeID id2 = itEd->first.second;
178 
179  // Draw only if we have the global coords of both nodes:
180  typename GRAPH_T::global_poses_t::const_iterator itNod1 = g.nodes.find(id1);
181  typename GRAPH_T::global_poses_t::const_iterator itNod2 = g.nodes.find(id2);
182  if (itNod1!=g.nodes.end() && itNod2!=g.nodes.end())
183  {
184  const CPose3D p1 = CPose3D(itNod1->second);
185  const CPose3D p2 = CPose3D(itNod2->second);
186  gl_edges->appendLine( TPoint3D(p1.x(),p1.y(),p1.z()), TPoint3D(p2.x(),p2.y(),p2.z()) );
187  }
188  }
189  ret->insert( gl_edges );
190 
191  } // end draw edges
192 
193  return ret;
194 
195  MRPT_TRY_END
196 
197  }
198 
199  }
200  }
201 }
202 
203 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013