MRPT  1.9.9
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-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 #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.3, 0.3, 0.3);
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  pnts->enablePointSmooth();
108 
109  ret->insert(pnts);
110  } // end draw node points
111 
112  // Show a 2D corner at each node (or just an empty object with the ID label)
113  if (show_node_corners || show_ID_labels)
114  {
115  for (auto itNod = g.nodes.begin(); itNod != g.nodes.end(); ++itNod)
116  {
117  const CPose3D p = CPose3D(
118  itNod->second); // Convert to 3D from whatever its real type.
119  CSetOfObjects::Ptr gl_corner =
120  show_node_corners
121  ? (is_3D_graph
123  nodes_corner_scale, 1.0 /*line width*/)
125  nodes_corner_scale, 1.0 /*line width*/))
126  : std::make_shared<CSetOfObjects>();
127  gl_corner->setPose(p);
128  if (show_ID_labels) // don't show IDs twice!
129  {
130  gl_corner->setName(
131  format("%u", static_cast<unsigned int>(itNod->first)));
132  gl_corner->enableShowName();
133  }
134  ret->insert(gl_corner);
135  }
136  } // end draw node corners
137 
138  if (show_edge_rel_poses)
139  {
140  const mrpt::img::TColor col8bit(
141  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
142 
143  for (const auto& edge : g)
144  {
145  // Node ID of the source pose:
146  const auto node_id_start = g.edges_store_inverse_poses
147  ? edge.first.second
148  : edge.first.first;
149 
150  // Draw only if we have the global coords of starting nodes:
151  auto itNod = g.nodes.find(node_id_start);
152  if (itNod != g.nodes.end())
153  {
154  const CPose3D pSource = CPose3D(itNod->second);
155  // Create a set of objects at that pose and do the rest in
156  // relative coords:
157  auto gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
158  gl_rel_edge->setPose(pSource);
159 
160  const auto& edge_pose = edge.second.getPoseMean();
161  const auto edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
162 
163  auto gl_edge_corner =
164  (is_3D_graph
166  nodes_edges_corner_scale, 1.0 /*line width*/)
168  nodes_edges_corner_scale, 1.0 /*line width*/));
169 
170  gl_edge_corner->setPose(edge_pose);
171  gl_rel_edge->insert(gl_edge_corner);
172 
173  auto gl_line = mrpt::opengl::CSimpleLine::Create(
174  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
175  edge_pose_pt.z());
176  gl_line->setColor_u8(col8bit);
177  gl_line->setLineWidth(edge_width);
178  gl_rel_edge->insert(gl_line);
179 
180  ret->insert(gl_rel_edge);
181  }
182  }
183  }
184 
185  if (show_edges)
186  {
187  CSetOfLines::Ptr gl_edges = std::make_shared<CSetOfLines>();
188  const mrpt::img::TColor col8bit(
189  edge_color & 0xffffff, edge_color >> 24);
190 
191  gl_edges->setColor_u8(col8bit);
192  gl_edges->setLineWidth(edge_width);
193 
194  for (const auto& edge : g)
195  {
196  const auto id1 = edge.first.first;
197  const auto id2 = edge.first.second;
198 
199  // Draw only if we have the global coords of both nodes:
200  auto itNod1 = g.nodes.find(id1);
201  auto itNod2 = g.nodes.find(id2);
202  if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end())
203  {
204  const CPose3D p1 = CPose3D(itNod1->second);
205  const CPose3D p2 = CPose3D(itNod2->second);
206  gl_edges->appendLine(
207  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
208  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
209  }
210  }
211  ret->insert(gl_edges);
212 
213  } // end draw edges
214 
215  return ret;
216 
218 }
219 } // 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...
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
#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
GLubyte g
Definition: glext.h:6372
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...
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
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: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: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019