MRPT  2.0.1
graph_tools_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 #include <mrpt/img/TColor.h>
18 #include <mrpt/poses/CPoint3D.h>
19 
21 {
22 template <class GRAPH_T>
24  const GRAPH_T& g, const mrpt::system::TParametersDouble& extra_params)
25 {
27 
28  using mrpt::math::TPose3D;
30 
31  // Is a 2D or 3D graph network?
32  using constraint_t = typename GRAPH_T::constraint_t;
33 
34  const bool is_3D_graph = constraint_t::is_3D();
35 
36  // create opengl obejct to be filled.
37  CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
38 
39  // graph visualization parameters
40  const bool show_ID_labels =
41  0 != extra_params.getWithDefaultVal("show_ID_labels", 0);
42  const bool show_ground_grid =
43  0 != extra_params.getWithDefaultVal("show_ground_grid", 1);
44  const bool show_edges =
45  0 != extra_params.getWithDefaultVal("show_edges", 1);
46  const bool show_node_corners =
47  0 != extra_params.getWithDefaultVal("show_node_corners", 1);
48  const bool show_edge_rel_poses =
49  0 != extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
50  const double nodes_point_size =
51  extra_params.getWithDefaultVal("nodes_point_size", 0.);
52  const double nodes_corner_scale =
53  extra_params.getWithDefaultVal("nodes_corner_scale", 0.7);
54  const double nodes_edges_corner_scale =
55  extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4);
56  const unsigned int nodes_point_color = extra_params.getWithDefaultVal(
57  "nodes_point_color", (unsigned int)0xA0A0A0);
58  const unsigned int edge_color =
59  extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
60  const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal(
61  "edge_rel_poses_color", (unsigned int)0x40FF8000);
62  const double edge_width = extra_params.getWithDefaultVal("edge_width", 2.);
63 
64  if (show_ground_grid)
65  {
66  // Estimate bounding box.
67  mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.);
68 
69  for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
70  {
71  const CPose3D p = CPose3D(
72  itNod->second); // Convert to 3D from whatever its real type.
73 
74  keep_min(BB_min.x, p.x());
75  keep_min(BB_min.y, p.y());
76  keep_min(BB_min.z, p.z());
77 
78  keep_max(BB_max.x, p.x());
79  keep_max(BB_max.y, p.y());
80  keep_max(BB_max.z, p.z());
81  }
82 
83  // Create ground plane:
84  const double grid_frequency = 5.0;
86  BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
87  grid->setColor(0.3f, 0.3f, 0.3f);
88  ret->insert(grid);
89  } // end show_ground_grid
90 
91  // Draw nodes as thick points:
92  if (nodes_point_size > 0)
93  {
94  CPointCloud::Ptr pnts = std::make_shared<CPointCloud>();
95  pnts->setColor(
96  mrpt::img::TColorf(mrpt::img::TColor(nodes_point_color)));
97  pnts->setPointSize(nodes_point_size);
98 
99  // Add nodes:
100  for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
101  {
102  const CPose3D p = CPose3D(
103  itNod->second); // Convert to 3D from whatever its real type.
104  pnts->insertPoint(p.x(), p.y(), p.z());
105  }
106 
107  ret->insert(pnts);
108  } // end draw node points
109 
110  // Show a 2D corner at each node (or just an empty object with the ID label)
111  if (show_node_corners || show_ID_labels)
112  {
113  for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
114  {
115  const CPose3D p = CPose3D(
116  itNod->second); // Convert to 3D from whatever its real type.
117  CSetOfObjects::Ptr gl_corner =
118  show_node_corners
119  ? (is_3D_graph
121  nodes_corner_scale, 1.0 /*line width*/)
123  nodes_corner_scale, 1.0 /*line width*/))
124  : std::make_shared<CSetOfObjects>();
125  gl_corner->setPose(p);
126  if (show_ID_labels) // don't show IDs twice!
127  {
128  gl_corner->setName(
129  format("%u", static_cast<unsigned int>(itNod->first)));
130  gl_corner->enableShowName();
131  }
132  ret->insert(gl_corner);
133  }
134  } // end draw node corners
135 
136  if (show_edge_rel_poses)
137  {
138  const mrpt::img::TColor col8bit(
139  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
140 
141  for (const auto& edge : g)
142  {
143  // Node ID of the source pose:
144  const auto node_id_start = g.edges_store_inverse_poses
145  ? edge.first.second
146  : edge.first.first;
147 
148  // Draw only if we have the global coords of starting nodes:
149  auto itNod = g.nodes.find(node_id_start);
150  if (itNod != g.nodes.end())
151  {
152  const CPose3D pSource = CPose3D(itNod->second);
153  // Create a set of objects at that pose and do the rest in
154  // relative coords:
155  auto gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
156  gl_rel_edge->setPose(pSource);
157 
158  const auto& edge_pose = edge.second.getPoseMean();
159  const auto edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
160 
161  auto gl_edge_corner =
162  (is_3D_graph
164  nodes_edges_corner_scale, 1.0 /*line width*/)
166  nodes_edges_corner_scale, 1.0 /*line width*/));
167 
168  gl_edge_corner->setPose(edge_pose);
169  gl_rel_edge->insert(gl_edge_corner);
170 
171  auto gl_line = mrpt::opengl::CSimpleLine::Create(
172  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
173  edge_pose_pt.z());
174  gl_line->setColor_u8(col8bit);
175  gl_line->setLineWidth(edge_width);
176  gl_rel_edge->insert(gl_line);
177 
178  ret->insert(gl_rel_edge);
179  }
180  }
181  }
182 
183  if (show_edges)
184  {
185  CSetOfLines::Ptr gl_edges = std::make_shared<CSetOfLines>();
186  const mrpt::img::TColor col8bit(
187  edge_color & 0xffffff, edge_color >> 24);
188 
189  gl_edges->setColor_u8(col8bit);
190  gl_edges->setLineWidth(edge_width);
191 
192  for (const auto& edge : g)
193  {
194  const auto id1 = edge.first.first;
195  const auto id2 = edge.first.second;
196 
197  // Draw only if we have the global coords of both nodes:
198  auto itNod1 = g.nodes.find(id1);
199  auto itNod2 = g.nodes.find(id2);
200  if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end())
201  {
202  const CPose3D p1 = CPose3D(itNod1->second);
203  const CPose3D p2 = CPose3D(itNod2->second);
204  gl_edges->appendLine(
205  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
206  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
207  }
208  }
209  ret->insert(gl_edges);
210 
211  } // end draw edges
212 
213  return ret;
214 
216 }
217 } // namespace mrpt::opengl::graph_tools
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...
#define MRPT_TRY_END
The end of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ex...
Definition: exceptions.h:213
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
Tool functions for graphs of pose constraints.
Definition: graph_tools.h:20
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
#define MRPT_TRY_START
The start of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ...
Definition: exceptions.h:206
A class used to store a 3D point.
Definition: CPoint3D.h:31
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
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
CSetOfObjects::Ptr graph_visualize(const GRAPH_T &g, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble())
Returns an opengl objects representation of an arbitrary graph, as a network of 3D pose frames...
A RGB color - 8bit.
Definition: TColor.h:25
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:54
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020