9 #ifndef CVISUALIZER_IMPL_H 10 #define CVISUALIZER_IMPL_H 16 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
17 class EDGE_ANNOTATIONS>
18 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
24 m_is_3D_graph = constraint_t::is_3D();
26 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
27 class EDGE_ANNOTATIONS>
28 CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
29 EDGE_ANNOTATIONS>::~CVisualizer()
35 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
36 class EDGE_ANNOTATIONS>
37 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
46 const bool show_ID_labels =
48 const bool show_ground_grid =
51 const bool show_node_corners =
53 const bool show_edge_rel_poses =
55 const double nodes_point_size =
60 this->drawGroundGrid(
object, &viz_params);
63 if (nodes_point_size > 0)
65 this->drawNodePoints(
object, &viz_params);
68 if (show_node_corners || show_ID_labels)
70 this->drawNodeCorners(
object, &viz_params);
73 if (show_edge_rel_poses)
75 this->drawEdgeRelPoses(
object, &viz_params);
80 this->drawEdges(
object, &viz_params);
84 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
85 class EDGE_ANNOTATIONS>
86 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
93 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
99 m_graph.nodes.begin();
100 n_it != m_graph.nodes.end(); ++n_it)
115 const double grid_frequency = 5.0;
117 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
118 grid->setColor(0.3, 0.3, 0.3);
119 object->insert(grid);
122 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
123 class EDGE_ANNOTATIONS>
124 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
130 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
135 const double nodes_point_size =
138 "nodes_point_color", (
unsigned int)0xA0A0A0);
142 pnts->setPointSize(nodes_point_size);
146 m_graph.nodes.begin();
147 n_it != m_graph.nodes.end(); ++n_it)
151 pnts->insertPoint(
p.x(),
p.y(),
p.z());
154 pnts->enablePointSmooth();
155 object->insert(pnts);
158 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
159 class EDGE_ANNOTATIONS>
160 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
169 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
171 const bool show_node_corners =
173 const bool show_ID_labels =
175 const double nodes_corner_scale =
179 m_graph.nodes.begin();
180 n_it != m_graph.nodes.end(); ++n_it)
188 nodes_corner_scale, 1.0 )
190 nodes_corner_scale, 1.0 ))
191 : mrpt::make_aligned_shared<CSetOfObjects>();
192 gl_corner->setPose(
p);
196 format(
"%u", static_cast<unsigned int>(n_it->first)));
197 gl_corner->enableShowName();
199 object->insert(gl_corner);
203 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
204 class EDGE_ANNOTATIONS>
205 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
214 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
216 const double nodes_edges_corner_scale =
219 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
221 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
224 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
227 const TNodeID node_id_start = m_graph.edges_store_inverse_poses
228 ? edge_it->first.second
229 : edge_it->first.first;
233 m_graph.nodes.find(node_id_start);
234 if (n_it != m_graph.nodes.end())
240 mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
241 gl_rel_edge->setPose(pSource);
244 edge_it->second.getPoseMean();
251 nodes_edges_corner_scale, 1.0 )
253 nodes_edges_corner_scale, 1.0 ));
255 gl_edge_corner->setPose(edge_pose);
256 gl_rel_edge->insert(gl_edge_corner);
260 0, 0, 0, edge_pose_pt.
x(), edge_pose_pt.
y(),
263 gl_line->setColor_u8(col8bit);
264 gl_line->setLineWidth(edge_width);
265 gl_rel_edge->insert(gl_line);
267 object->insert(gl_rel_edge);
272 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
273 class EDGE_ANNOTATIONS>
274 void CVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
283 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
286 const unsigned int edge_color =
290 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
292 gl_edges->setColor_u8(col8bit);
293 gl_edges->setLineWidth(edge_width);
297 edge_it != m_graph.end(); ++edge_it)
299 const TNodeID id1 = edge_it->first.first;
300 const TNodeID id2 = edge_it->first.second;
304 m_graph.nodes.find(id1);
306 m_graph.nodes.find(id2);
307 if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
311 gl_edges->appendLine(
316 object->insert(gl_edges);
double x() const
Common members of all points & poses classes.
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).
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
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...
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
Internal functions for MRPT.
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...
typename CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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].
The namespace for 3D scene representation and rendering.
typename edges_map_t::const_iterator const_iterator
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
static Ptr Create(Args &&... args)
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)