16 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
17 class EDGE_ANNOTATIONS>
24 m_is_3D_graph = constraint_t::is_3D();
27 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
28 class EDGE_ANNOTATIONS>
35 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
36 class EDGE_ANNOTATIONS>
38 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_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);
85 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
86 class EDGE_ANNOTATIONS>
88 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
94 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
99 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
114 const double grid_frequency = 5.0;
116 BB_min.x, BB_max.
x, BB_min.y, BB_max.
y, BB_min.z, grid_frequency);
117 grid->setColor(0.3, 0.3, 0.3);
118 object->insert(grid);
122 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
123 class EDGE_ANNOTATIONS>
125 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_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);
145 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
149 pnts->insertPoint(
p.x(),
p.y(),
p.z());
152 pnts->enablePointSmooth();
153 object->insert(pnts);
157 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
158 class EDGE_ANNOTATIONS>
160 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
168 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
170 const bool show_node_corners =
172 const bool show_ID_labels =
174 const double nodes_corner_scale =
177 for (
auto n_it = m_graph.nodes.begin(); n_it != m_graph.nodes.end(); ++n_it)
185 nodes_corner_scale, 1.0 )
187 nodes_corner_scale, 1.0 ))
188 : std::make_shared<CSetOfObjects>();
189 gl_corner->setPose(
p);
193 format(
"%u", static_cast<unsigned int>(n_it->first)));
194 gl_corner->enableShowName();
196 object->insert(gl_corner);
201 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
202 class EDGE_ANNOTATIONS>
204 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
212 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
214 const double nodes_edges_corner_scale =
217 "edge_rel_poses_color", (
unsigned int)0x40FF8000);
219 edge_rel_poses_color & 0xffffff, edge_rel_poses_color >> 24);
222 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
225 const TNodeID node_id_start = m_graph.edges_store_inverse_poses
226 ? edge_it->first.second
227 : edge_it->first.first;
230 auto n_it = m_graph.nodes.find(node_id_start);
231 if (n_it != m_graph.nodes.end())
238 gl_rel_edge->setPose(pSource);
241 edge_it->second.getPoseMean();
248 nodes_edges_corner_scale, 1.0 )
250 nodes_edges_corner_scale, 1.0 ));
252 gl_edge_corner->setPose(edge_pose);
253 gl_rel_edge->insert(gl_edge_corner);
257 0, 0, 0, edge_pose_pt.
x(), edge_pose_pt.
y(),
260 gl_line->setColor_u8(col8bit);
261 gl_line->setLineWidth(edge_width);
262 gl_rel_edge->insert(gl_line);
264 object->insert(gl_rel_edge);
270 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
271 class EDGE_ANNOTATIONS>
273 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
281 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
284 const unsigned int edge_color =
288 const TColor col8bit(edge_color & 0xffffff, edge_color >> 24);
290 gl_edges->setColor_u8(col8bit);
291 gl_edges->setLineWidth(edge_width);
294 for (
auto edge_it = m_graph.begin(); edge_it != m_graph.end(); ++edge_it)
296 const TNodeID id1 = edge_it->first.first;
297 const TNodeID id2 = edge_it->first.second;
300 auto n_it1 = m_graph.nodes.find(id1);
301 auto n_it2 = m_graph.nodes.find(id2);
302 if (n_it1 != m_graph.nodes.end() && n_it2 != m_graph.nodes.end())
306 gl_edges->appendLine(
311 object->insert(gl_edges);
double x
X,Y,Z coordinates.
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)
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...
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...
CVisualizer(const GRAPH_T &graph_in)
Constructor.
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) ...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
double x() const
Common members of all points & poses classes.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
RET getWithDefaultVal(const std::string &s, const RET &defaultVal) const
A const version of the [] operator and with a default value in case the parameter is not set (for usa...
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.
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
static Ptr Create(Args &&... args)
static Ptr Create(Args &&... args)