9 #ifndef opengl_graph_tools_impl_H 10 #define opengl_graph_tools_impl_H 23 template <
class GRAPH_T>
33 using constraint_t =
typename GRAPH_T::constraint_t;
35 const bool is_3D_graph = constraint_t::is_3D();
41 const bool show_ID_labels =
43 const bool show_ground_grid =
45 const bool show_edges =
47 const bool show_node_corners =
49 const bool show_edge_rel_poses =
51 const double nodes_point_size =
53 const double nodes_corner_scale =
55 const double nodes_edges_corner_scale =
58 "nodes_point_color", (
unsigned int)0xA0A0A0);
59 const unsigned int edge_color =
62 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
72 itNod !=
g.nodes.end(); ++itNod)
74 const CPose3D
p = CPose3D(
87 const double grid_frequency = 5.0;
89 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
90 grid->setColor(0.3, 0.3, 0.3);
95 if (nodes_point_size > 0)
100 pnts->setPointSize(nodes_point_size);
105 itNod !=
g.nodes.end(); ++itNod)
107 const CPose3D
p = CPose3D(
109 pnts->insertPoint(
p.x(),
p.y(),
p.z());
112 pnts->enablePointSmooth();
118 if (show_node_corners || show_ID_labels)
122 itNod !=
g.nodes.end(); ++itNod)
124 const CPose3D
p = CPose3D(
130 nodes_corner_scale, 1.0 )
132 nodes_corner_scale, 1.0 ))
133 : mrpt::make_aligned_shared<CSetOfObjects>();
134 gl_corner->setPose(
p);
138 format(
"%u", static_cast<unsigned int>(itNod->first)));
139 gl_corner->enableShowName();
141 ret->insert(gl_corner);
145 if (show_edge_rel_poses)
148 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
150 for (
const auto& edge :
g)
153 const auto node_id_start =
g.edges_store_inverse_poses
158 auto itNod =
g.nodes.find(node_id_start);
159 if (itNod !=
g.nodes.end())
161 const CPose3D pSource = CPose3D(itNod->second);
165 gl_rel_edge->setPose(pSource);
167 const auto& edge_pose = edge.second.getPoseMean();
170 auto gl_edge_corner =
173 nodes_edges_corner_scale, 1.0 )
175 nodes_edges_corner_scale, 1.0 ));
177 gl_edge_corner->setPose(edge_pose);
178 gl_rel_edge->insert(gl_edge_corner);
181 0, 0, 0, edge_pose_pt.x(), edge_pose_pt.y(),
183 gl_line->setColor_u8(col8bit);
184 gl_line->setLineWidth(edge_width);
185 gl_rel_edge->insert(gl_line);
187 ret->insert(gl_rel_edge);
196 edge_color & 0xffffff, edge_color >> 24);
198 gl_edges->setColor_u8(col8bit);
199 gl_edges->setLineWidth(edge_width);
201 for (
const auto& edge :
g)
203 const auto id1 = edge.first.first;
204 const auto id2 = edge.first.second;
207 auto itNod1 =
g.nodes.find(id1);
208 auto itNod2 =
g.nodes.find(id2);
209 if (itNod1 !=
g.nodes.end() && itNod2 !=
g.nodes.end())
211 const CPose3D p1 = CPose3D(itNod1->second);
212 const CPose3D p2 = CPose3D(itNod2->second);
213 gl_edges->appendLine(
218 ret->insert(gl_edges);
#define MRPT_TRY_END
The end of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ex...
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).
static Ptr Create(Args &&... args)
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_START
The start of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ...
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...
double x
X,Y,Z coordinates.
A class used to store a 3D point.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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].
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
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...
static Ptr Create(Args &&... args)
const Scalar * const_iterator
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...
static Ptr Create(Args &&... args)