MRPT  1.9.9
CVisualizer_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-2018, 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 CVISUALIZER_IMPL_H
10 #define CVISUALIZER_IMPL_H
11 
12 namespace mrpt::graphs::detail
13 {
14 // constructor, destructor
15 ////////////////////////////////////////////////////////////
16 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
17  class EDGE_ANNOTATIONS>
18 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
19  EDGE_ANNOTATIONS>::CVisualizer(const GRAPH_T& graph_in)
20  : m_graph(graph_in)
21 {
22  // Is a 2D or 3D graph network?
23  using constraint_t = typename GRAPH_T::constraint_t;
24  m_is_3D_graph = constraint_t::is_3D();
25 }
26 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
27  class EDGE_ANNOTATIONS>
28 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
29  EDGE_ANNOTATIONS>::~CVisualizer()
30 {
31 }
32 
33 // methods implementations
34 ////////////////////////////////////////////////////////////
35 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
36  class EDGE_ANNOTATIONS>
37 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
38  EDGE_ANNOTATIONS>::
39  getAs3DObject(
41  mrpt::system::TParametersDouble viz_params) const
42 {
43  using namespace mrpt::opengl;
44 
45  // graph visualization parameters
46  const bool show_ID_labels =
47  0 != viz_params.getWithDefaultVal("show_ID_labels", 0);
48  const bool show_ground_grid =
49  0 != viz_params.getWithDefaultVal("show_ground_grid", 1);
50  const bool show_edges = 0 != viz_params.getWithDefaultVal("show_edges", 1);
51  const bool show_node_corners =
52  0 != viz_params.getWithDefaultVal("show_node_corners", 1);
53  const bool show_edge_rel_poses =
54  0 != viz_params.getWithDefaultVal("show_edge_rel_poses", 0);
55  const double nodes_point_size =
56  viz_params.getWithDefaultVal("nodes_point_size", 0.);
57 
58  if (show_ground_grid)
59  {
60  this->drawGroundGrid(object, &viz_params);
61  }
62 
63  if (nodes_point_size > 0)
64  {
65  this->drawNodePoints(object, &viz_params);
66  }
67 
68  if (show_node_corners || show_ID_labels)
69  {
70  this->drawNodeCorners(object, &viz_params);
71  }
72 
73  if (show_edge_rel_poses)
74  {
75  this->drawEdgeRelPoses(object, &viz_params);
76  }
77 
78  if (show_edges)
79  {
80  this->drawEdges(object, &viz_params);
81  }
82 }
83 
84 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
85  class EDGE_ANNOTATIONS>
86 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
87  EDGE_ANNOTATIONS>::
88  drawGroundGrid(
90  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
91 {
92  using namespace mrpt::opengl;
93  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
94 
95  // Estimate bounding box.
96  mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.);
97 
98  for (typename GRAPH_T::global_poses_t::const_iterator n_it =
99  m_graph.nodes.begin();
100  n_it != m_graph.nodes.end(); ++n_it)
101  {
102  const CPose3D p = CPose3D(
103  n_it->second); // Convert to 3D from whatever its real type.
104 
105  keep_min(BB_min.x, p.x());
106  keep_min(BB_min.y, p.y());
107  keep_min(BB_min.z, p.z());
108 
109  keep_max(BB_max.x, p.x());
110  keep_max(BB_max.y, p.y());
111  keep_max(BB_max.z, p.z());
112  }
113 
114  // Create ground plane:
115  const double grid_frequency = 5.0;
117  BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
118  grid->setColor(0.3, 0.3, 0.3);
119  object->insert(grid);
120 }
121 
122 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
123  class EDGE_ANNOTATIONS>
124 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
125  EDGE_ANNOTATIONS>::
126  drawNodePoints(
128  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
129 {
130  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
131 
132  using namespace mrpt::opengl;
133  using namespace mrpt::img;
134 
135  const double nodes_point_size =
136  viz_params->getWithDefaultVal("nodes_point_size", 0.);
137  const unsigned int nodes_point_color = viz_params->getWithDefaultVal(
138  "nodes_point_color", (unsigned int)0xA0A0A0);
139 
140  CPointCloud::Ptr pnts = mrpt::make_aligned_shared<CPointCloud>();
141  pnts->setColor(TColorf(TColor(nodes_point_color)));
142  pnts->setPointSize(nodes_point_size);
143 
144  // Add all nodesnodes:
145  for (typename GRAPH_T::global_poses_t::const_iterator n_it =
146  m_graph.nodes.begin();
147  n_it != m_graph.nodes.end(); ++n_it)
148  {
149  const CPose3D p = CPose3D(
150  n_it->second); // Convert to 3D from whatever its real type.
151  pnts->insertPoint(p.x(), p.y(), p.z());
152  }
153 
154  pnts->enablePointSmooth();
155  object->insert(pnts);
156 }
157 
158 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
159  class EDGE_ANNOTATIONS>
160 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
161  EDGE_ANNOTATIONS>::
162  drawNodeCorners(
164  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
165 {
166  using namespace mrpt::opengl;
167  using mrpt::poses::CPose3D;
168 
169  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
170 
171  const bool show_node_corners =
172  0 != viz_params->getWithDefaultVal("show_node_corners", 1);
173  const bool show_ID_labels =
174  0 != viz_params->getWithDefaultVal("show_ID_labels", 0);
175  const double nodes_corner_scale =
176  viz_params->getWithDefaultVal("nodes_corner_scale", 0.7);
177 
178  for (typename GRAPH_T::global_poses_t::const_iterator n_it =
179  m_graph.nodes.begin();
180  n_it != m_graph.nodes.end(); ++n_it)
181  {
182  // Convert to 3D from whatever its real type. CSetOfObjects::Ptr
183  // gl_corner = show_node_corners ?
184  const CPose3D p = CPose3D(n_it->second);
185  CSetOfObjects::Ptr gl_corner =
186  show_node_corners
187  ? (m_is_3D_graph ? stock_objects::CornerXYZSimple(
188  nodes_corner_scale, 1.0 /*line width*/)
190  nodes_corner_scale, 1.0 /*line width*/))
191  : mrpt::make_aligned_shared<CSetOfObjects>();
192  gl_corner->setPose(p);
193  if (show_ID_labels)
194  { // don't show IDs twice!
195  gl_corner->setName(
196  format("%u", static_cast<unsigned int>(n_it->first)));
197  gl_corner->enableShowName();
198  }
199  object->insert(gl_corner);
200  }
201 }
202 
203 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
204  class EDGE_ANNOTATIONS>
205 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
206  EDGE_ANNOTATIONS>::
207  drawEdgeRelPoses(
209  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
210 {
211  using namespace mrpt::opengl;
212  using namespace mrpt::img;
213  using mrpt::graphs::TNodeID;
214  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
215 
216  const double nodes_edges_corner_scale =
217  viz_params->getWithDefaultVal("nodes_edges_corner_scale", 0.4);
218  const unsigned int edge_rel_poses_color = viz_params->getWithDefaultVal(
219  "edge_rel_poses_color", (unsigned int)0x40FF8000);
220  const TColor col8bit(
221  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
222  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
223 
224  for (auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
225  {
226  // Node ID of the source pose:
227  const TNodeID node_id_start = m_graph.edges_store_inverse_poses
228  ? edge_it->first.second
229  : edge_it->first.first;
230 
231  // Draw only if we have the global coords of starting nodes:
233  m_graph.nodes.find(node_id_start);
234  if (n_it != m_graph.nodes.end())
235  {
236  const CPose3D pSource = CPose3D(n_it->second);
237  // Create a set of objects at that pose and do the rest in relative
238  // coords:
240  mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
241  gl_rel_edge->setPose(pSource);
242 
243  const typename GRAPH_T::constraint_no_pdf_t& edge_pose =
244  edge_it->second.getPoseMean();
245  const mrpt::poses::CPoint3D edge_pose_pt =
246  mrpt::poses::CPoint3D(edge_pose);
247 
248  mrpt::opengl::CSetOfObjects::Ptr gl_edge_corner =
249  (m_is_3D_graph
251  nodes_edges_corner_scale, 1.0 /*line width*/)
253  nodes_edges_corner_scale, 1.0 /*line width*/));
254 
255  gl_edge_corner->setPose(edge_pose);
256  gl_rel_edge->insert(gl_edge_corner);
257 
260  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
261  edge_pose_pt.z());
262 
263  gl_line->setColor_u8(col8bit);
264  gl_line->setLineWidth(edge_width);
265  gl_rel_edge->insert(gl_line);
266 
267  object->insert(gl_rel_edge);
268  }
269  }
270 }
271 
272 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
273  class EDGE_ANNOTATIONS>
274 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
275  EDGE_ANNOTATIONS>::
276  drawEdges(
278  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
279 {
280  using namespace mrpt::opengl;
281  using namespace mrpt::img;
282  using namespace mrpt::poses;
283  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
284 
285  CSetOfLines::Ptr gl_edges = mrpt::make_aligned_shared<CSetOfLines>();
286  const unsigned int edge_color =
287  viz_params->getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
288  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
289 
290  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
291 
292  gl_edges->setColor_u8(col8bit);
293  gl_edges->setLineWidth(edge_width);
294 
295  // for all registered edges.
296  for (typename GRAPH_T::const_iterator edge_it = m_graph.begin();
297  edge_it != m_graph.end(); ++edge_it)
298  {
299  const TNodeID id1 = edge_it->first.first;
300  const TNodeID id2 = edge_it->first.second;
301 
302  // Draw only if we have the global coords of both nodes:
304  m_graph.nodes.find(id1);
306  m_graph.nodes.find(id2);
307  if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
308  { // both nodes found?
309  const CPose3D p1 = CPose3D(n_it1->second);
310  const CPose3D p2 = CPose3D(n_it2->second);
311  gl_edges->appendLine(
312  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
313  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
314  }
315  }
316  object->insert(gl_edges);
317 }
318 } // end of namespaces
319 
320 #endif /* end of include guard: CVISUALIZER_IMPL_H */
321 
322 
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
CSetOfObjects::Ptr CornerXYSimple(float scale=1.0, float lineWidth=1.0)
Returns two arrows representing a X,Y 2D corner (just thick lines, fast to render).
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...
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
Internal functions for MRPT.
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...
typename CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
CSetOfObjects::Ptr 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 - floats in the range [0,1].
Definition: TColor.h:77
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
typename edges_map_t::const_iterator const_iterator
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
GLfloat GLfloat p
Definition: glext.h:6305
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:32
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:103
static Ptr Create(Args &&... args)
Definition: CSimpleLine.h:22



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020