Main MRPT website > C++ reference for MRPT 1.5.7
CVisualizer_impl.h
Go to the documentation of this file.
1 #ifndef CVISUALIZER_IMPL_H
2 #define CVISUALIZER_IMPL_H
3 
4 namespace mrpt { namespace graphs { namespace detail {
5 
6 // constructor, destructor
7 ////////////////////////////////////////////////////////////
8 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
10 CVisualizer(const GRAPH_T& graph_in):
11  m_graph(graph_in)
12 {
13  // Is a 2D or 3D graph network?
14  typedef typename GRAPH_T::constraint_t constraint_t;
15  m_is_3D_graph = constraint_t::is_3D();
16 }
17 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
20 
21 // methods implementations
22 ////////////////////////////////////////////////////////////
23 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
26  mrpt::opengl::CSetOfObjectsPtr& object,
27  mrpt::utils::TParametersDouble viz_params) const {
28  using namespace mrpt::opengl;
29  using namespace mrpt::utils;
30 
31  // graph visualization parameters
32  const bool show_ID_labels = 0 != viz_params.getWithDefaultVal(
33  "show_ID_labels", 0);
34  const bool show_ground_grid = 0 != viz_params.getWithDefaultVal(
35  "show_ground_grid", 1);
36  const bool show_edges = 0 != viz_params.getWithDefaultVal(
37  "show_edges", 1);
38  const bool show_node_corners = 0 != viz_params.getWithDefaultVal(
39  "show_node_corners", 1);
40  const bool show_edge_rel_poses = 0 != viz_params.getWithDefaultVal(
41  "show_edge_rel_poses", 0);
42  const double nodes_point_size = viz_params.getWithDefaultVal(
43  "nodes_point_size", 0.);
44 
45 
46  if (show_ground_grid) {
47  this->drawGroundGrid(object, &viz_params);
48  }
49 
50  if (nodes_point_size > 0) {
51  this->drawNodePoints(object, &viz_params);
52  }
53 
54  if (show_node_corners || show_ID_labels) {
55  this->drawNodeCorners(object, &viz_params);
56  }
57 
58  if (show_edge_rel_poses) {
59  this->drawEdgeRelPoses(object, &viz_params);
60  }
61 
62  if (show_edges) {
63  this->drawEdges(object, &viz_params);
64  }
65 
66 }
67 
68 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
70 drawGroundGrid(mrpt::opengl::CSetOfObjectsPtr& object,
71  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
72  using namespace mrpt::opengl;
73  using namespace mrpt::utils;
74  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
75 
76  // Estimate bounding box.
77  mrpt::math::TPoint3D BB_min(-10.,-10.,0.), BB_max(10.,10.,0.);
78 
80  n_it = m_graph.nodes.begin();
81  n_it!=m_graph.nodes.end();
82  ++n_it) {
83  const CPose3D p = CPose3D(n_it->second); // Convert to 3D from whatever its real type.
84 
85  keep_min(BB_min.x, p.x());
86  keep_min(BB_min.y, p.y());
87  keep_min(BB_min.z, p.z());
88 
89  keep_max(BB_max.x, p.x());
90  keep_max(BB_max.y, p.y());
91  keep_max(BB_max.z, p.z());
92  }
93 
94  // Create ground plane:
95  const double grid_frequency = 5.0;
96  CGridPlaneXYPtr grid = CGridPlaneXY::Create(
97  BB_min.x, BB_max.x,
98  BB_min.y, BB_max.y,
99  BB_min.z, grid_frequency);
100  grid->setColor(0.3,0.3,0.3);
101  object->insert(grid);
102 
103 }
104 
105 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
107 drawNodePoints(mrpt::opengl::CSetOfObjectsPtr& object,
108  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
109  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
110 
111  using namespace mrpt::opengl;
112  using namespace mrpt::utils;
113 
114  const double nodes_point_size = viz_params->getWithDefaultVal(
115  "nodes_point_size", 0.);
116  const unsigned int nodes_point_color = viz_params->getWithDefaultVal(
117  "nodes_point_color", (unsigned int)0xA0A0A0);
118 
119  CPointCloudPtr pnts = CPointCloud::Create();
120  pnts->setColor(TColorf(TColor(nodes_point_color)));
121  pnts->setPointSize(nodes_point_size);
122 
123  // Add all nodesnodes:
125  n_it = m_graph.nodes.begin();
126  n_it!=m_graph.nodes.end();
127  ++n_it) {
128 
129  const CPose3D p = CPose3D(n_it->second); // Convert to 3D from whatever its real type.
130  pnts->insertPoint(p.x(),p.y(), p.z());
131 
132  }
133 
134  pnts->enablePointSmooth();
135  object->insert(pnts);
136 
137 }
138 
139 
140 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
142 drawNodeCorners(mrpt::opengl::CSetOfObjectsPtr& object,
143  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
144  using namespace mrpt::opengl;
145  using namespace mrpt::utils;
146  using mrpt::poses::CPose3D;
147 
148  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
149 
150  const bool show_node_corners = 0 != viz_params->getWithDefaultVal(
151  "show_node_corners", 1);
152  const bool show_ID_labels = 0 != viz_params->getWithDefaultVal(
153  "show_ID_labels", 0);
154  const double nodes_corner_scale = viz_params->getWithDefaultVal(
155  "nodes_corner_scale", 0.7);
156 
158  n_it = m_graph.nodes.begin();
159  n_it!=m_graph.nodes.end();
160  ++n_it) {
161 
162  // Convert to 3D from whatever its real type. CSetOfObjectsPtr gl_corner = show_node_corners ?
163  const CPose3D p = CPose3D(n_it->second);
164  CSetOfObjectsPtr gl_corner = show_node_corners ?
165  (m_is_3D_graph ?
166  stock_objects::CornerXYZSimple(nodes_corner_scale, 1.0 /*line width*/):
167  stock_objects::CornerXYSimple(nodes_corner_scale, 1.0 /*line width*/))
169  gl_corner->setPose(p);
170  if (show_ID_labels) { // don't show IDs twice!
171  gl_corner->setName(format("%u",static_cast<unsigned int>(n_it->first)));
172  gl_corner->enableShowName();
173  }
174  object->insert(gl_corner);
175 
176  }
177 }
178 
179 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
181 drawEdgeRelPoses(mrpt::opengl::CSetOfObjectsPtr& object,
182  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
183  using namespace mrpt::opengl;
184  using namespace mrpt::utils;
185  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
186 
187  const double nodes_edges_corner_scale = viz_params->getWithDefaultVal(
188  "nodes_edges_corner_scale", 0.4);
189  const unsigned int edge_rel_poses_color = viz_params->getWithDefaultVal(
190  "edge_rel_poses_color", (unsigned int)0x40FF8000);
191  const TColor col8bit(edge_rel_poses_color& 0xffffff, edge_rel_poses_color >> 24);
192  const double edge_width = viz_params->getWithDefaultVal(
193  "edge_width", 2.);
194 
195  for (typename GRAPH_T::const_iterator edge_it = m_graph.begin();
196  edge_it!=m_graph.end();
197  ++edge_it) {
198  // Node ID of the source pose:
199  const TNodeID node_id_start =
200  m_graph.edges_store_inverse_poses ? edge_it->first.second : edge_it->first.first;
201 
202  // Draw only if we have the global coords of starting nodes:
203  typename GRAPH_T::global_poses_t::const_iterator n_it = m_graph.nodes.find(node_id_start);
204  if (n_it!=m_graph.nodes.end()) {
205  const CPose3D pSource = CPose3D(n_it->second);
206  // Create a set of objects at that pose and do the rest in relative coords:
207  mrpt::opengl::CSetOfObjectsPtr gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
208  gl_rel_edge->setPose(pSource);
209 
210  const typename GRAPH_T::constraint_no_pdf_t & edge_pose = edge_it->second.getPoseMean();
211  const mrpt::poses::CPoint3D edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
212 
213  mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
214  (m_is_3D_graph ?
215  stock_objects::CornerXYZSimple(nodes_edges_corner_scale, 1.0 /*line width*/):
216  stock_objects::CornerXYSimple(nodes_edges_corner_scale, 1.0 /*line width*/));
217 
218  gl_edge_corner->setPose(edge_pose);
219  gl_rel_edge->insert(gl_edge_corner);
220 
221  mrpt::opengl::CSimpleLinePtr gl_line =
223  0,0,0,
224  edge_pose_pt.x(), edge_pose_pt.y(), edge_pose_pt.z());
225 
226  gl_line->setColor_u8(col8bit);
227  gl_line->setLineWidth(edge_width);
228  gl_rel_edge->insert(gl_line);
229 
230  object->insert(gl_rel_edge);
231  }
232  }
233 }
234 
235 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
237 drawEdges(mrpt::opengl::CSetOfObjectsPtr& object,
238  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
239  using namespace mrpt::opengl;
240  using namespace mrpt::utils;
241  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
242 
243  CSetOfLinesPtr gl_edges = CSetOfLines::Create();
244  const unsigned int edge_color = viz_params->getWithDefaultVal(
245  "edge_color", (unsigned int)0x400000FF);
246  const double edge_width = viz_params->getWithDefaultVal(
247  "edge_width", 2.);
248 
249  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
250 
251  gl_edges->setColor_u8(col8bit);
252  gl_edges->setLineWidth(edge_width);
253 
254  // for all registered edges.
255  for (typename GRAPH_T::const_iterator
256  edge_it = m_graph.begin();
257  edge_it!=m_graph.end();
258  ++edge_it) {
259  const TNodeID id1 = edge_it->first.first;
260  const TNodeID id2 = edge_it->first.second;
261 
262  // Draw only if we have the global coords of both nodes:
263  typename GRAPH_T::global_poses_t::const_iterator n_it1 = m_graph.nodes.find(id1);
264  typename GRAPH_T::global_poses_t::const_iterator n_it2 = m_graph.nodes.find(id2);
265  if (n_it1!=m_graph.nodes.end() && n_it2!=m_graph.nodes.end()) { // both nodes found?
266  const CPose3D p1 = CPose3D(n_it1->second);
267  const CPose3D p2 = CPose3D(n_it2->second);
268  gl_edges->appendLine(
269  mrpt::math::TPoint3D(p1.x(),p1.y(),p1.z()),
270  mrpt::math::TPoint3D(p2.x(),p2.y(),p2.z()));
271  }
272  }
273  object->insert(gl_edges);
274 
275 }
276 
277 } } } // end of namespaces
278 
279 #endif /* end of include guard: CVISUALIZER_IMPL_H */
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
double z
X,Y,Z coordinates.
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
static CSetOfLinesPtr Create()
CVisualizer(const GRAPH_T &graph_in)
Constructor.
uint64_t TNodeID
The type for node IDs in graphs of different types.
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).
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...
A RGB color - 8bit.
Definition: TColor.h:26
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
A class used to store a 3D point.
Definition: CPoint3D.h:32
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
static CGridPlaneXYPtr Create()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static CSimpleLinePtr Create()
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
The namespace for 3D scene representation and rendering.
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
static CPointCloudPtr Create()
Lightweight 3D point.
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
static CSetOfObjectsPtr Create()



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019