9 #ifndef opengl_graph_tools_impl_H 10 #define opengl_graph_tools_impl_H 25 template<
class GRAPH_T>
37 typedef typename GRAPH_T::constraint_t constraint_t;
39 const bool is_3D_graph = constraint_t::is_3D();
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);
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 );
63 const CPose3D p = CPose3D(itNod->second);
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);
82 if (nodes_point_size>0)
86 pnts->setPointSize(nodes_point_size);
91 const CPose3D p = CPose3D(itNod->second);
92 pnts->insertPoint(p.x(),p.y(), p.z() );
95 pnts->enablePointSmooth();
101 if (show_node_corners || show_ID_labels)
105 const CPose3D p = CPose3D(itNod->second);
106 CSetOfObjectsPtr gl_corner = show_node_corners ?
109 gl_corner->setPose( p );
112 gl_corner->setName(
format(
"%u",static_cast<unsigned int>(itNod->first) ));
113 gl_corner->enableShowName();
115 ret->insert( gl_corner );
119 if (show_edge_rel_poses)
121 const TColor col8bit(edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
126 const TNodeID node_id_start = g.edges_store_inverse_poses ? itEd->first.second : itEd->first.first;
130 if (itNod!=g.nodes.end())
132 const CPose3D pSource = CPose3D(itNod->second);
135 gl_rel_edge->setPose(pSource);
137 const typename GRAPH_T::constraint_no_pdf_t & edge_pose = itEd->second.getPoseMean();
140 mrpt::opengl::CSetOfObjectsPtr gl_edge_corner =
143 gl_edge_corner->setPose(edge_pose);
144 gl_rel_edge->insert(gl_edge_corner);
147 gl_line->setColor_u8( col8bit );
148 gl_line->setLineWidth(edge_width);
149 gl_rel_edge->insert(gl_line);
151 ret->insert( gl_rel_edge );
159 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
161 gl_edges->setColor_u8( col8bit );
162 gl_edges->setLineWidth( edge_width );
166 const TNodeID id1 = itEd->first.first;
167 const TNodeID id2 = itEd->first.second;
172 if (itNod1!=g.nodes.end() && itNod2!=g.nodes.end())
174 const CPose3D p1 = CPose3D(itNod1->second);
175 const CPose3D p2 = CPose3D(itNod2->second);
179 ret->insert( gl_edges );
double x() const
Common members of all points & poses classes.
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
const Scalar * const_iterator
double z
X,Y,Z coordinates.
static CSetOfLinesPtr Create()
uint64_t TNodeID
The type for node IDs in graphs of different types.
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...
A class used to store a 3D point.
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.
static CGridPlaneXYPtr Create()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
static CSimpleLinePtr Create()
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...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
A RGB color - floats in the range [0,1].
static CPointCloudPtr Create()
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).
static CSetOfObjectsPtr Create()