Main MRPT website > C++ reference for MRPT 1.9.9
graph_tools_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-2018, 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 opengl_graph_tools_impl_H
10 #define opengl_graph_tools_impl_H
11 
18 #include <mrpt/poses/CPoint3D.h>
19 #include <mrpt/img/TColor.h>
20 
21 namespace mrpt
22 {
23 namespace opengl
24 {
25 namespace graph_tools
26 {
27 template <class GRAPH_T>
29  const GRAPH_T& g, const mrpt::system::TParametersDouble& extra_params)
30 {
32 
33  using mrpt::math::TPose3D;
35 
36  // Is a 2D or 3D graph network?
37  using constraint_t = typename GRAPH_T::constraint_t;
38 
39  const bool is_3D_graph = constraint_t::is_3D();
40 
41  // create opengl obejct to be filled.
42  CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
43 
44  // graph visualization parameters
45  const bool show_ID_labels =
46  0 != extra_params.getWithDefaultVal("show_ID_labels", 0);
47  const bool show_ground_grid =
48  0 != extra_params.getWithDefaultVal("show_ground_grid", 1);
49  const bool show_edges =
50  0 != extra_params.getWithDefaultVal("show_edges", 1);
51  const bool show_node_corners =
52  0 != extra_params.getWithDefaultVal("show_node_corners", 1);
53  const bool show_edge_rel_poses =
54  0 != extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
55  const double nodes_point_size =
56  extra_params.getWithDefaultVal("nodes_point_size", 0.);
57  const double nodes_corner_scale =
58  extra_params.getWithDefaultVal("nodes_corner_scale", 0.7);
59  const double nodes_edges_corner_scale =
60  extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4);
61  const unsigned int nodes_point_color = extra_params.getWithDefaultVal(
62  "nodes_point_color", (unsigned int)0xA0A0A0);
63  const unsigned int edge_color =
64  extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF);
65  const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal(
66  "edge_rel_poses_color", (unsigned int)0x40FF8000);
67  const double edge_width = extra_params.getWithDefaultVal("edge_width", 2.);
68 
69  if (show_ground_grid)
70  {
71  // Estimate bounding box.
72  mrpt::math::TPoint3D BB_min(-10., -10., 0.), BB_max(10., 10., 0.);
73 
74  for (typename GRAPH_T::global_poses_t::const_iterator itNod =
75  g.nodes.begin();
76  itNod != g.nodes.end(); ++itNod)
77  {
78  const CPose3D p = CPose3D(
79  itNod->second); // Convert to 3D from whatever its real type.
80 
81  keep_min(BB_min.x, p.x());
82  keep_min(BB_min.y, p.y());
83  keep_min(BB_min.z, p.z());
84 
85  keep_max(BB_max.x, p.x());
86  keep_max(BB_max.y, p.y());
87  keep_max(BB_max.z, p.z());
88  }
89 
90  // Create ground plane:
91  const double grid_frequency = 5.0;
93  BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
94  grid->setColor(0.3, 0.3, 0.3);
95  ret->insert(grid);
96  } // end show_ground_grid
97 
98  // Draw nodes as thick points:
99  if (nodes_point_size > 0)
100  {
101  CPointCloud::Ptr pnts = mrpt::make_aligned_shared<CPointCloud>();
102  pnts->setColor(
103  mrpt::img::TColorf(mrpt::img::TColor(nodes_point_color)));
104  pnts->setPointSize(nodes_point_size);
105 
106  // Add nodes:
107  for (typename GRAPH_T::global_poses_t::const_iterator itNod =
108  g.nodes.begin();
109  itNod != g.nodes.end(); ++itNod)
110  {
111  const CPose3D p = CPose3D(
112  itNod->second); // Convert to 3D from whatever its real type.
113  pnts->insertPoint(p.x(), p.y(), p.z());
114  }
115 
116  pnts->enablePointSmooth();
117 
118  ret->insert(pnts);
119  } // end draw node points
120 
121  // Show a 2D corner at each node (or just an empty object with the ID label)
122  if (show_node_corners || show_ID_labels)
123  {
124  for (typename GRAPH_T::global_poses_t::const_iterator itNod =
125  g.nodes.begin();
126  itNod != g.nodes.end(); ++itNod)
127  {
128  const CPose3D p = CPose3D(
129  itNod->second); // Convert to 3D from whatever its real type.
130  CSetOfObjects::Ptr gl_corner =
131  show_node_corners
132  ? (is_3D_graph
134  nodes_corner_scale, 1.0 /*line width*/)
136  nodes_corner_scale, 1.0 /*line width*/))
137  : mrpt::make_aligned_shared<CSetOfObjects>();
138  gl_corner->setPose(p);
139  if (show_ID_labels) // don't show IDs twice!
140  {
141  gl_corner->setName(
142  format("%u", static_cast<unsigned int>(itNod->first)));
143  gl_corner->enableShowName();
144  }
145  ret->insert(gl_corner);
146  }
147  } // end draw node corners
148 
149  if (show_edge_rel_poses)
150  {
151  const mrpt::img::TColor col8bit(
152  edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
153 
154  for (const auto& edge : g)
155  {
156  // Node ID of the source pose:
157  const auto node_id_start = g.edges_store_inverse_poses
158  ? edge.first.second
159  : edge.first.first;
160 
161  // Draw only if we have the global coords of starting nodes:
162  auto itNod = g.nodes.find(node_id_start);
163  if (itNod != g.nodes.end())
164  {
165  const CPose3D pSource = CPose3D(itNod->second);
166  // Create a set of objects at that pose and do the rest in
167  // relative coords:
168  auto gl_rel_edge = mrpt::opengl::CSetOfObjects::Create();
169  gl_rel_edge->setPose(pSource);
170 
171  const auto& edge_pose = edge.second.getPoseMean();
172  const auto edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
173 
174  auto gl_edge_corner =
175  (is_3D_graph
177  nodes_edges_corner_scale, 1.0 /*line width*/)
179  nodes_edges_corner_scale, 1.0 /*line width*/));
180 
181  gl_edge_corner->setPose(edge_pose);
182  gl_rel_edge->insert(gl_edge_corner);
183 
184  auto gl_line = mrpt::opengl::CSimpleLine::Create(
185  0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
186  edge_pose_pt.z());
187  gl_line->setColor_u8(col8bit);
188  gl_line->setLineWidth(edge_width);
189  gl_rel_edge->insert(gl_line);
190 
191  ret->insert(gl_rel_edge);
192  }
193  }
194  }
195 
196  if (show_edges)
197  {
198  CSetOfLines::Ptr gl_edges = mrpt::make_aligned_shared<CSetOfLines>();
199  const mrpt::img::TColor col8bit(
200  edge_color & 0xffffff, edge_color >> 24);
201 
202  gl_edges->setColor_u8(col8bit);
203  gl_edges->setLineWidth(edge_width);
204 
205  for (const auto& edge : g)
206  {
207  const auto id1 = edge.first.first;
208  const auto id2 = edge.first.second;
209 
210  // Draw only if we have the global coords of both nodes:
211  auto itNod1 = g.nodes.find(id1);
212  auto itNod2 = g.nodes.find(id2);
213  if (itNod1 != g.nodes.end() && itNod2 != g.nodes.end())
214  {
215  const CPose3D p1 = CPose3D(itNod1->second);
216  const CPose3D p2 = CPose3D(itNod2->second);
217  gl_edges->appendLine(
218  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
219  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
220  }
221  }
222  ret->insert(gl_edges);
223 
224  } // end draw edges
225 
226  return ret;
227 
229 }
230 
231 } // namespace graph_tools
232 } // namespace opengl
233 } // namespace mrpt
234 
235 #endif
CSetOfLines.h
mrpt::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: core/include/mrpt/core/bits_math.h:124
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
mrpt::opengl::CSetOfLines::Ptr
std::shared_ptr< CSetOfLines > Ptr
Definition: CSetOfLines.h:35
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:30
CSimpleLine.h
CSetOfObjects.h
mrpt::math::TPoint3D::z
double z
Definition: lightweight_geom_data.h:385
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
g
GLubyte g
Definition: glext.h:6279
stock_objects.h
CPointCloud.h
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:419
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::opengl::CPointCloud::Ptr
std::shared_ptr< CPointCloud > Ptr
Definition: CPointCloud.h:50
MRPT_TRY_END
#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:231
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:34
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::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: core/include/mrpt/core/bits_math.h:131
mrpt::system::TParameters< double >
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::opengl::graph_tools::graph_visualize
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.
Definition: graph_tools_impl.h:28
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:385
mrpt::img::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
MRPT_TRY_START
#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:224
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:22
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:603
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:30
CPoint3D.h
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::math::TPoint3D::y
double y
Definition: lightweight_geom_data.h:385
mrpt::system::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:105
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:447
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
CGridPlaneXY.h
mrpt::opengl::CSimpleLine::Create
static Ptr Create(Args &&... args)
Definition: CSimpleLine.h:24
TColor.h



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST