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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019