MRPT  1.9.9
CVisualizer_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 namespace mrpt::graphs::detail
12 {
13 // constructor, destructor
14 ////////////////////////////////////////////////////////////
15 template <
16  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
17  class EDGE_ANNOTATIONS>
19  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 <
27  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
28  class EDGE_ANNOTATIONS>
30  ~CVisualizer() = default;
31 
32 // methods implementations
33 ////////////////////////////////////////////////////////////
34 template <
35  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
36  class EDGE_ANNOTATIONS>
37 void CVisualizer<
38  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, 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 <
85  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
86  class EDGE_ANNOTATIONS>
87 void CVisualizer<
88  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
89  drawGroundGrid(
91  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
92 {
93  using namespace mrpt::opengl;
94  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
95 
96  // Estimate bounding box.
97  mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.);
98 
99  for (auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
100  {
101  const CPose3D p = CPose3D(
102  n_it->second); // Convert to 3D from whatever its real type.
103 
104  keep_min(BB_min.x, p.x());
105  keep_min(BB_min.y, p.y());
106  keep_min(BB_min.z, p.z());
107 
108  keep_max(BB_max.x, p.x());
109  keep_max(BB_max.y, p.y());
110  keep_max(BB_max.z, p.z());
111  }
112 
113  // Create ground plane:
114  const double grid_frequency = 5.0;
116  BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
117  grid->setColor(0.3, 0.3, 0.3);
118  object->insert(grid);
119 }
120 
121 template <
122  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
123  class EDGE_ANNOTATIONS>
124 void CVisualizer<
125  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, 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 = std::make_shared<CPointCloud>();
141  pnts->setColor(TColorf(TColor(nodes_point_color)));
142  pnts->setPointSize(nodes_point_size);
143 
144  // Add all nodesnodes:
145  for (auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
146  {
147  const CPose3D p = CPose3D(
148  n_it->second); // Convert to 3D from whatever its real type.
149  pnts->insertPoint(p.x(), p.y(), p.z());
150  }
151 
152  pnts->enablePointSmooth();
153  object->insert(pnts);
154 }
155 
156 template <
157  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
158  class EDGE_ANNOTATIONS>
159 void CVisualizer<
160  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
161  drawNodeCorners(
163  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
164 {
165  using namespace mrpt::opengl;
166  using mrpt::poses::CPose3D;
167 
168  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
169 
170  const bool show_node_corners =
171  0 != viz_params->getWithDefaultVal("show_node_corners", 1);
172  const bool show_ID_labels =
173  0 != viz_params->getWithDefaultVal("show_ID_labels", 0);
174  const double nodes_corner_scale =
175  viz_params->getWithDefaultVal("nodes_corner_scale", 0.7);
176 
177  for (auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
178  {
179  // Convert to 3D from whatever its real type. CSetOfObjects::Ptr
180  // gl_corner = show_node_corners ?
181  const CPose3D p = CPose3D(n_it->second);
182  CSetOfObjects::Ptr gl_corner =
183  show_node_corners
184  ? (m_is_3D_graph ? stock_objects::CornerXYZSimple(
185  nodes_corner_scale, 1.0 /*line width*/)
187  nodes_corner_scale, 1.0 /*line width*/))
188  : std::make_shared<CSetOfObjects>();
189  gl_corner->setPose(p);
190  if (show_ID_labels)
191  { // don't show IDs twice!
192  gl_corner->setName(
193  format("%u", static_cast<unsigned int>(n_it->first)));
194  gl_corner->enableShowName();
195  }
196  object->insert(gl_corner);
197  }
198 }
199 
200 template <
201  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
202  class EDGE_ANNOTATIONS>
203 void CVisualizer<
204  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
205  drawEdgeRelPoses(
207  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
208 {
209  using namespace mrpt::opengl;
210  using namespace mrpt::img;
211  using mrpt::graphs::TNodeID;
212  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
213 
214  const double nodes_edges_corner_scale =
215  viz_params->getWithDefaultVal("nodes_edges_corner_scale", 0.4);
216  const unsigned int edge_rel_poses_color = viz_params->getWithDefaultVal(
217  "edge_rel_poses_color", (unsigned int)0x40FF8000);
218  const TColor col8bit(
219  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
220  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
221 
222  for (auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
223  {
224  // Node ID of the source pose:
225  const TNodeID node_id_start = m_graph.edges_store_inverse_poses
226  ? edge_it->first.second
227  : edge_it->first.first;
228 
229  // Draw only if we have the global coords of starting nodes:
230  auto n_it = m_graph.nodes.find(node_id_start);
231  if (n_it != m_graph.nodes.end())
232  {
233  const CPose3D pSource = CPose3D(n_it->second);
234  // Create a set of objects at that pose and do the rest in relative
235  // coords:
238  gl_rel_edge->setPose(pSource);
239 
240  const typename GRAPH_T::constraint_no_pdf_t& edge_pose =
241  edge_it->second.getPoseMean();
242  const mrpt::poses::CPoint3D edge_pose_pt =
243  mrpt::poses::CPoint3D(edge_pose);
244 
245  mrpt::opengl::CSetOfObjects::Ptr gl_edge_corner =
246  (m_is_3D_graph
248  nodes_edges_corner_scale, 1.0 /*line width*/)
250  nodes_edges_corner_scale, 1.0 /*line width*/));
251 
252  gl_edge_corner->setPose(edge_pose);
253  gl_rel_edge->insert(gl_edge_corner);
254 
257  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
258  edge_pose_pt.z());
259 
260  gl_line->setColor_u8(col8bit);
261  gl_line->setLineWidth(edge_width);
262  gl_rel_edge->insert(gl_line);
263 
264  object->insert(gl_rel_edge);
265  }
266  }
267 }
268 
269 template <
270  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
271  class EDGE_ANNOTATIONS>
272 void CVisualizer<
273  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
274  drawEdges(
276  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
277 {
278  using namespace mrpt::opengl;
279  using namespace mrpt::img;
280  using namespace mrpt::poses;
281  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
282 
283  CSetOfLines::Ptr gl_edges = std::make_shared<CSetOfLines>();
284  const unsigned int edge_color =
285  viz_params->getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
286  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
287 
288  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
289 
290  gl_edges->setColor_u8(col8bit);
291  gl_edges->setLineWidth(edge_width);
292 
293  // for all registered edges.
294  for (auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
295  {
296  const TNodeID id1 = edge_it->first.first;
297  const TNodeID id2 = edge_it->first.second;
298 
299  // Draw only if we have the global coords of both nodes:
300  auto n_it1 = m_graph.nodes.find(id1);
301  auto n_it2 = m_graph.nodes.find(id2);
302  if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
303  { // both nodes found?
304  const CPose3D p1 = CPose3D(n_it1->second);
305  const CPose3D p2 = CPose3D(n_it2->second);
306  gl_edges->appendLine(
307  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
308  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
309  }
310  }
311  object->insert(gl_edges);
312 }
313 } // namespace mrpt::graphs::detail
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...
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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).
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
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.
CVisualizer(const GRAPH_T &graph_in)
Constructor.
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) ...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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...
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...
RET getWithDefaultVal(const std::string &s, const RET &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:90
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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:78
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
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.
Definition: TPoint3D.h:90
GLfloat GLfloat p
Definition: glext.h:6398
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
static Ptr Create(Args &&... args)
Definition: CSimpleLine.h:21



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019