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-2020, 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.3f, 0.3f, 0.3f);
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  object->insert(pnts);
153 }
154 
155 template <
156  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
157  class EDGE_ANNOTATIONS>
158 void CVisualizer<
159  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
160  drawNodeCorners(
162  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
163 {
164  using namespace mrpt::opengl;
165  using mrpt::poses::CPose3D;
166 
167  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
168 
169  const bool show_node_corners =
170  0 != viz_params->getWithDefaultVal("show_node_corners", 1);
171  const bool show_ID_labels =
172  0 != viz_params->getWithDefaultVal("show_ID_labels", 0);
173  const double nodes_corner_scale =
174  viz_params->getWithDefaultVal("nodes_corner_scale", 0.7);
175 
176  for (auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
177  {
178  // Convert to 3D from whatever its real type. CSetOfObjects::Ptr
179  // gl_corner = show_node_corners ?
180  const CPose3D p = CPose3D(n_it->second);
181  CSetOfObjects::Ptr gl_corner =
182  show_node_corners
183  ? (m_is_3D_graph ? stock_objects::CornerXYZSimple(
184  nodes_corner_scale, 1.0 /*line width*/)
186  nodes_corner_scale, 1.0 /*line width*/))
187  : std::make_shared<CSetOfObjects>();
188  gl_corner->setPose(p);
189  if (show_ID_labels)
190  { // don't show IDs twice!
191  gl_corner->setName(
192  format("%u", static_cast<unsigned int>(n_it->first)));
193  gl_corner->enableShowName();
194  }
195  object->insert(gl_corner);
196  }
197 }
198 
199 template <
200  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
201  class EDGE_ANNOTATIONS>
202 void CVisualizer<
203  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
204  drawEdgeRelPoses(
206  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
207 {
208  using namespace mrpt::opengl;
209  using namespace mrpt::img;
210  using mrpt::graphs::TNodeID;
211  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
212 
213  const double nodes_edges_corner_scale =
214  viz_params->getWithDefaultVal("nodes_edges_corner_scale", 0.4);
215  const unsigned int edge_rel_poses_color = viz_params->getWithDefaultVal(
216  "edge_rel_poses_color", (unsigned int)0x40FF8000);
217  const TColor col8bit(
218  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
219  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
220 
221  for (auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
222  {
223  // Node ID of the source pose:
224  const TNodeID node_id_start = m_graph.edges_store_inverse_poses
225  ? edge_it->first.second
226  : edge_it->first.first;
227 
228  // Draw only if we have the global coords of starting nodes:
229  auto n_it = m_graph.nodes.find(node_id_start);
230  if (n_it != m_graph.nodes.end())
231  {
232  const CPose3D pSource = CPose3D(n_it->second);
233  // Create a set of objects at that pose and do the rest in relative
234  // coords:
237  gl_rel_edge->setPose(pSource);
238 
239  const typename GRAPH_T::constraint_no_pdf_t& edge_pose =
240  edge_it->second.getPoseMean();
241  const mrpt::poses::CPoint3D edge_pose_pt =
242  mrpt::poses::CPoint3D(edge_pose);
243 
244  mrpt::opengl::CSetOfObjects::Ptr gl_edge_corner =
245  (m_is_3D_graph
247  nodes_edges_corner_scale, 1.0 /*line width*/)
249  nodes_edges_corner_scale, 1.0 /*line width*/));
250 
251  gl_edge_corner->setPose(edge_pose);
252  gl_rel_edge->insert(gl_edge_corner);
253 
256  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
257  edge_pose_pt.z());
258 
259  gl_line->setColor_u8(col8bit);
260  gl_line->setLineWidth(edge_width);
261  gl_rel_edge->insert(gl_line);
262 
263  object->insert(gl_rel_edge);
264  }
265  }
266 }
267 
268 template <
269  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
270  class EDGE_ANNOTATIONS>
271 void CVisualizer<
272  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
273  drawEdges(
275  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
276 {
277  using namespace mrpt::opengl;
278  using namespace mrpt::img;
279  using namespace mrpt::poses;
280  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
281 
282  CSetOfLines::Ptr gl_edges = std::make_shared<CSetOfLines>();
283  const unsigned int edge_color =
284  viz_params->getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
285  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
286 
287  const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
288 
289  gl_edges->setColor_u8(col8bit);
290  gl_edges->setLineWidth(edge_width);
291 
292  // for all registered edges.
293  for (auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
294  {
295  const TNodeID id1 = edge_it->first.first;
296  const TNodeID id2 = edge_it->first.second;
297 
298  // Draw only if we have the global coords of both nodes:
299  auto n_it1 = m_graph.nodes.find(id1);
300  auto n_it2 = m_graph.nodes.find(id2);
301  if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
302  { // both nodes found?
303  const CPose3D p1 = CPose3D(n_it1->second);
304  const CPose3D p2 = CPose3D(n_it2->second);
305  gl_edges->appendLine(
306  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
307  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
308  }
309  }
310  object->insert(gl_edges);
311 }
312 } // 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...
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...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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...
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
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:25
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020