Main MRPT website > C++ reference for MRPT 1.5.6
graph_tools_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef opengl_graph_tools_impl_H
10 #define opengl_graph_tools_impl_H
11 
12 #include <mrpt/opengl/CGridPlaneXY.h>
13 #include <mrpt/opengl/CPointCloud.h>
14 #include <mrpt/opengl/CSetOfObjects.h>
15 #include <mrpt/opengl/CSimpleLine.h>
16 #include <mrpt/opengl/CSetOfLines.h>
17 #include <mrpt/opengl/stock_objects.h>
18 
19 namespace mrpt
20 {
21  namespace opengl
22  {
23  namespace graph_tools
24  {
25  template<class GRAPH_T>
26  CSetOfObjectsPtr graph_visualize(
27  const GRAPH_T &g,
28  const mrpt::utils::TParametersDouble &extra_params)
29  {
30  MRPT_TRY_START
31 
32  using mrpt::poses::CPose3D;
33  using mrpt::math::TPose3D;
34  using namespace mrpt::utils;
35 
36  // Is a 2D or 3D graph network?
37  typedef typename GRAPH_T::constraint_t constraint_t;
38 
39  const bool is_3D_graph = constraint_t::is_3D();
40 
41  // create opengl obejct to be filled.
42  CSetOfObjectsPtr ret = CSetOfObjects::Create();
43 
44  // graph visualization parameters
45  const bool show_ID_labels = 0!=extra_params.getWithDefaultVal("show_ID_labels", 0);
46  const bool show_ground_grid = 0!=extra_params.getWithDefaultVal("show_ground_grid", 1);
47  const bool show_edges = 0!=extra_params.getWithDefaultVal("show_edges", 1);
48  const bool show_node_corners = 0!=extra_params.getWithDefaultVal("show_node_corners", 1);
49  const bool show_edge_rel_poses = 0!=extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
50  const double nodes_point_size = extra_params.getWithDefaultVal("nodes_point_size", 0.);
51  const double nodes_corner_scale = extra_params.getWithDefaultVal("nodes_corner_scale", 0.7);
52  const double nodes_edges_corner_scale = extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4);
53  const unsigned int nodes_point_color = extra_params.getWithDefaultVal("nodes_point_color", (unsigned int)0xA0A0A0);
54  const unsigned int edge_color = extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
55  const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal("edge_rel_poses_color", (unsigned int)0x40FF8000);
56  const double edge_width = extra_params.getWithDefaultVal("edge_width", 2.);
57 
58  if (show_ground_grid) {
59  // Estimate bounding box.
60  mrpt::math::TPoint3D BB_min(-10.,-10.,0.), BB_max(10.,10.,0.);
61 
62  for (typename GRAPH_T::global_poses_t::const_iterator
63  itNod = g.nodes.begin();
64  itNod!=g.nodes.end();
65  ++itNod) {
66  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
67 
68  keep_min(BB_min.x, p.x());
69  keep_min(BB_min.y, p.y());
70  keep_min(BB_min.z, p.z());
71 
72  keep_max(BB_max.x, p.x());
73  keep_max(BB_max.y, p.y());
74  keep_max(BB_max.z, p.z());
75  }
76 
77  // Create ground plane:
78  const double grid_frequency = 5.0;
79  CGridPlaneXYPtr grid = CGridPlaneXY::Create(BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
80  grid->setColor(0.3,0.3,0.3);
81  ret->insert(grid);
82  } // end show_ground_grid
83 
84  // Draw nodes as thick points:
85  if (nodes_point_size>0) {
86  CPointCloudPtr pnts = CPointCloud::Create();
87  pnts->setColor(TColorf(TColor(nodes_point_color)));
88  pnts->setPointSize(nodes_point_size);
89 
90  // Add nodes:
91  for (typename GRAPH_T::global_poses_t::const_iterator
92  itNod = g.nodes.begin();
93  itNod!=g.nodes.end();
94  ++itNod) {
95 
96  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
97  pnts->insertPoint(p.x(),p.y(), p.z());
98 
99  }
100 
101  pnts->enablePointSmooth();
102 
103  ret->insert(pnts);
104  } // end draw node points
105 
106  // Show a 2D corner at each node (or just an empty object with the ID label)
107  if (show_node_corners || show_ID_labels) {
108  for (typename GRAPH_T::global_poses_t::const_iterator
109  itNod = g.nodes.begin();
110  itNod!=g.nodes.end();
111  ++itNod) {
112  const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
113  CSetOfObjectsPtr gl_corner = show_node_corners ?
114  (is_3D_graph ?
115  stock_objects::CornerXYZSimple(nodes_corner_scale, 1.0 /*line width*/):
116  stock_objects::CornerXYSimple(nodes_corner_scale, 1.0 /*line width*/))
117  : CSetOfObjects::Create();
118  gl_corner->setPose(p);
119  if (show_ID_labels) // don't show IDs twice!
120  {
121  gl_corner->setName(format("%u",static_cast<unsigned int>(itNod->first)));
122  gl_corner->enableShowName();
123  }
124  ret->insert(gl_corner);
125  }
126  } // end draw node corners
127 
128  if (show_edge_rel_poses) {
129  const TColor col8bit(edge_rel_poses_color& 0xffffff, edge_rel_poses_color >> 24);
130 
131  for (typename GRAPH_T::const_iterator itEd = g.begin();
132  itEd!=g.end();
133  ++itEd) {
134  // Node ID of the source pose:
135  const TNodeID node_id_start =
136  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  const CPose3D pSource = CPose3D(itNod->second);
142  // Create a set of objects at that pose and do the rest in relative coords:
143  mrpt::opengl::CSetOfObjectsPtr gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
144  gl_rel_edge->setPose(pSource);
145 
146  const typename GRAPH_T::constraint_no_pdf_t & edge_pose = itEd->second.getPoseMean();
147  const mrpt::poses::CPoint3D edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
148 
149  mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
150  (is_3D_graph ?
151  stock_objects::CornerXYZSimple(nodes_edges_corner_scale, 1.0 /*line width*/):
152  stock_objects::CornerXYSimple(nodes_edges_corner_scale, 1.0 /*line width*/));
153 
154  gl_edge_corner->setPose(edge_pose);
155  gl_rel_edge->insert(gl_edge_corner);
156 
157  mrpt::opengl::CSimpleLinePtr gl_line =
158  mrpt::opengl::CSimpleLine::Create(0,0,0, edge_pose_pt.x(), edge_pose_pt.y(), edge_pose_pt.z());
159  gl_line->setColor_u8(col8bit);
160  gl_line->setLineWidth(edge_width);
161  gl_rel_edge->insert(gl_line);
162 
163  ret->insert(gl_rel_edge);
164  }
165  }
166  }
167 
168  if (show_edges) {
169  CSetOfLinesPtr gl_edges = CSetOfLines::Create();
170  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
171 
172  gl_edges->setColor_u8(col8bit);
173  gl_edges->setLineWidth(edge_width);
174 
175  for (typename GRAPH_T::const_iterator
176  itEd = g.begin();
177  itEd!=g.end();
178  ++itEd) {
179  const TNodeID id1 = itEd->first.first;
180  const TNodeID id2 = itEd->first.second;
181 
182  // Draw only if we have the global coords of both nodes:
183  typename GRAPH_T::global_poses_t::const_iterator itNod1 = g.nodes.find(id1);
184  typename GRAPH_T::global_poses_t::const_iterator itNod2 = g.nodes.find(id2);
185  if (itNod1!=g.nodes.end() && itNod2!=g.nodes.end()) {
186  const CPose3D p1 = CPose3D(itNod1->second);
187  const CPose3D p2 = CPose3D(itNod2->second);
188  gl_edges->appendLine(mrpt::math::TPoint3D(p1.x(),p1.y(),p1.z()), mrpt::math::TPoint3D(p2.x(),p2.y(),p2.z()));
189  }
190  }
191  ret->insert(gl_edges);
192 
193  } // end draw edges
194 
195  return ret;
196 
197  MRPT_TRY_END
198 
199  }
200 
201  } // end of graph_tools namespace
202  } // end of opengl namespace
203 } // end of mrpt namespace
204 
205 #endif
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::opengl::graph_tools::graph_visualize
CSetOfObjectsPtr graph_visualize(const GRAPH_T &g, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble())
Returns an opengl objects representation of an arbitrary graph, as a network of 3D pose frames...
Definition: graph_tools_impl.h:26
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CSetOfLines.h
CGridPlaneXY.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:248
mrpt::opengl::CSetOfLines::Create
static CSetOfLinesPtr Create()
MRPT_TRY_END
#define MRPT_TRY_END
Definition: maps/CColouredPointsMap.h:352
CSetOfObjects.h
mrpt::opengl::stock_objects::CornerXYSimple
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYSimple(float scale=1.0, float lineWidth=1.0)
Returns two arrows representing a X,Y 2D corner (just thick lines, fast to render).
Definition: StockObjects.cpp:378
mrpt::opengl::stock_objects::CornerXYZSimple
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
Definition: StockObjects.cpp:350
g
GLubyte g
Definition: glext.h:5575
mrpt::utils::TColor
A RGB color - 8bit.
Definition: TColor.h:26
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::utils::TNodeID
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
mrpt::opengl::CGridPlaneXY::Create
static CGridPlaneXYPtr Create()
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
CSimpleLine.h
mrpt::utils::keep_min
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
Definition: bits.h:171
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::opengl::CSimpleLine::Create
static CSimpleLinePtr Create()
mrpt::utils::TParameters::getWithDefaultVal
T getWithDefaultVal(const std::string &s, const T &defaultVal) const
A const version of the [] operator and with a default value in case the parameter is not set (for usa...
Definition: TParameters.h:88
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
MRPT_TRY_START
#define MRPT_TRY_START
Definition: maps/CColouredPointsMap.h:351
mrpt::utils::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
mrpt::opengl::CPointCloud::Create
static CPointCloudPtr Create()
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
p
GLfloat GLfloat p
Definition: glext.h:5587
CPointCloud.h
stock_objects.h
mrpt::opengl::CSetOfObjects::Create
static CSetOfObjectsPtr Create()


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019