Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
19namespace mrpt
20{
21 namespace opengl
22 {
23 namespace graph_tools
24 {
25 template<class GRAPH_T>
27 const GRAPH_T &g,
28 const mrpt::utils::TParametersDouble &extra_params)
29 {
31
34 using namespace mrpt::utils;
35
36 // Is a 2D or 3D graph network?
37 typedef typename GRAPH_T::constraint_t constraint_t;
38
39 const bool is_3D_graph = constraint_t::is_3D();
40
42
43 const bool show_ID_labels = 0!=extra_params.getWithDefaultVal("show_ID_labels", 0);
44 const bool show_ground_grid = 0!=extra_params.getWithDefaultVal("show_ground_grid", 1);
45 const bool show_edges = 0!=extra_params.getWithDefaultVal("show_edges", 1);
46 const bool show_node_corners = 0!=extra_params.getWithDefaultVal("show_node_corners", 1);
47 const bool show_edge_rel_poses = 0!=extra_params.getWithDefaultVal("show_edge_rel_poses", 0);
48 const double nodes_point_size = extra_params.getWithDefaultVal("nodes_point_size", 0. );
49 const double nodes_corner_scale = extra_params.getWithDefaultVal("nodes_corner_scale", 0.7 );
50 const double nodes_edges_corner_scale = extra_params.getWithDefaultVal("nodes_edges_corner_scale", 0.4 );
51 const unsigned int nodes_point_color = extra_params.getWithDefaultVal("nodes_point_color", (unsigned int)0xA0A0A0 );
52 const unsigned int edge_color = extra_params.getWithDefaultVal("edge_color", (unsigned int)0x400000FF );
53 const unsigned int edge_rel_poses_color = extra_params.getWithDefaultVal("edge_rel_poses_color", (unsigned int)0x40FF8000 );
54 const double edge_width = extra_params.getWithDefaultVal("edge_width", 2. );
55
56 if (show_ground_grid)
57 {
58 // Estimate bounding box.
59 mrpt::math::TPoint3D BB_min(-10.,-10.,0.), BB_max(10.,10.,0.);
60
61 for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
62 {
63 const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
64
65 keep_min( BB_min.x, p.x() );
66 keep_min( BB_min.y, p.y() );
67 keep_min( BB_min.z, p.z() );
68
69 keep_max( BB_max.x, p.x() );
70 keep_max( BB_max.y, p.y() );
71 keep_max( BB_max.z, p.z() );
72 }
73
74 // Create ground plane:
75 const double grid_frequency = 5.0;
76 CGridPlaneXYPtr grid = CGridPlaneXY::Create(BB_min.x, BB_max.x, BB_min.y, BB_max.y, BB_min.z, grid_frequency);
77 grid->setColor(0.3,0.3,0.3);
78 ret->insert( grid );
79 } // end show_ground_grid
80
81 // Draw nodes as thick points:
82 if (nodes_point_size>0)
83 {
85 pnts->setColor( TColorf(TColor(nodes_point_color)) );
86 pnts->setPointSize(nodes_point_size);
87
88 // Add nodes:
89 for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
90 {
91 const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
92 pnts->insertPoint(p.x(),p.y(), p.z() );
93 }
94
95 pnts->enablePointSmooth();
96
97 ret->insert(pnts);
98 } // end draw node points
99
100 // Show a 2D corner at each node (or just an empty object with the ID label)
101 if (show_node_corners || show_ID_labels)
102 {
103 for (typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.begin();itNod!=g.nodes.end();++itNod)
104 {
105 const CPose3D p = CPose3D(itNod->second); // Convert to 3D from whatever its real type.
106 CSetOfObjectsPtr gl_corner = show_node_corners ?
107 (is_3D_graph ? stock_objects::CornerXYZSimple(nodes_corner_scale, 1.0 /*line width*/ ) : stock_objects::CornerXYSimple(nodes_corner_scale, 1.0 /*line width*/ ))
109 gl_corner->setPose( p );
110 if (show_ID_labels) // don't show IDs twice!
111 {
112 gl_corner->setName(format("%u",static_cast<unsigned int>(itNod->first) ));
113 gl_corner->enableShowName();
114 }
115 ret->insert( gl_corner );
116 }
117 } // end draw node corners
118
119 if (show_edge_rel_poses)
120 {
121 const TColor col8bit(edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
122
123 for (typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
124 {
125 // Node ID of the source pose:
126 const TNodeID node_id_start = g.edges_store_inverse_poses ? itEd->first.second : itEd->first.first;
127
128 // Draw only if we have the global coords of starting nodes:
129 typename GRAPH_T::global_poses_t::const_iterator itNod = g.nodes.find(node_id_start);
130 if (itNod!=g.nodes.end())
131 {
132 const CPose3D pSource = CPose3D(itNod->second);
133 // Create a set of objects at that pose and do the rest in relative coords:
135 gl_rel_edge->setPose(pSource);
136
137 const typename GRAPH_T::constraint_no_pdf_t & edge_pose = itEd->second.getPoseMean();
138 const mrpt::poses::CPoint3D edge_pose_pt = mrpt::poses::CPoint3D(edge_pose);
139
140 mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
141 (is_3D_graph ? stock_objects::CornerXYZSimple(nodes_edges_corner_scale, 1.0 /*line width*/ ) : stock_objects::CornerXYSimple(nodes_edges_corner_scale, 1.0 /*line width*/ ));
142
143 gl_edge_corner->setPose(edge_pose);
144 gl_rel_edge->insert(gl_edge_corner);
145
146 mrpt::opengl::CSimpleLinePtr gl_line = mrpt::opengl::CSimpleLine::Create(0,0,0, edge_pose_pt.x(), edge_pose_pt.y(), edge_pose_pt.z() );
147 gl_line->setColor_u8( col8bit );
148 gl_line->setLineWidth(edge_width);
149 gl_rel_edge->insert(gl_line);
150
151 ret->insert( gl_rel_edge );
152 }
153 }
154 }
155
156 if (show_edges)
157 {
159 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
160
161 gl_edges->setColor_u8( col8bit );
162 gl_edges->setLineWidth( edge_width );
163
164 for (typename GRAPH_T::const_iterator itEd = g.begin();itEd!=g.end();++itEd)
165 {
166 const TNodeID id1 = itEd->first.first;
167 const TNodeID id2 = itEd->first.second;
168
169 // Draw only if we have the global coords of both nodes:
170 typename GRAPH_T::global_poses_t::const_iterator itNod1 = g.nodes.find(id1);
171 typename GRAPH_T::global_poses_t::const_iterator itNod2 = g.nodes.find(id2);
172 if (itNod1!=g.nodes.end() && itNod2!=g.nodes.end())
173 {
174 const CPose3D p1 = CPose3D(itNod1->second);
175 const CPose3D p2 = CPose3D(itNod2->second);
176 gl_edges->appendLine( mrpt::math::TPoint3D(p1.x(),p1.y(),p1.z()), mrpt::math::TPoint3D(p2.x(),p2.y(),p2.z()) );
177 }
178 }
179 ret->insert( gl_edges );
180
181 } // end draw edges
182
183 return ret;
184
186
187 }
188
189 }
190 }
191}
192
193#endif
static CGridPlaneXYPtr Create()
static CPointCloudPtr Create()
static CSetOfLinesPtr Create()
static CSetOfObjectsPtr Create()
static CSimpleLinePtr Create()
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
CSetOfObjectsPtr graph_visualize(const GRAPH_T &g, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble())
Returns an opengl objects representation of an arbitrary graph, as a network of 3D pose frames.
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
#define MRPT_TRY_START
Definition: mrpt_macros.h:334
#define MRPT_TRY_END
Definition: mrpt_macros.h:335
CSetOfObjectsPtr OPENGL_IMPEXP 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...
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYSimple(float scale=1.0, float lineWidth=1.0)
Returns two arrows representing a X,Y 2D corner (just thick lines, fast to render).
struct OPENGL_IMPEXP CSimpleLinePtr
Definition: CSimpleLine.h:21
struct OPENGL_IMPEXP CSetOfLinesPtr
Definition: CSetOfLines.h:21
struct OPENGL_IMPEXP CGridPlaneXYPtr
Definition: CGridPlaneXY.h:22
struct OPENGL_IMPEXP CPointCloudPtr
Definition: CPointCloud.h:25
struct OPENGL_IMPEXP CSetOfObjectsPtr
Definition: CSetOfObjects.h:23
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
A RGB color - 8bit.
Definition: TColor.h:26
A RGB color - floats in the range [0,1].
Definition: TColor.h:53
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:47
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:83



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 09:54:56 UTC 2023