9 #ifndef CMRVISUALIZER_IMPL_H 10 #define CMRVISUALIZER_IMPL_H 20 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
21 class EDGE_ANNOTATIONS>
22 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
27 "CMRVisualizer standard (non-specialized) edition doesn't server any " 29 "In case you use this visualizer specify TMRSlamNodeAnnotations" 30 "as the 3rd template argument");
33 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
34 class EDGE_ANNOTATIONS>
36 EDGE_ANNOTATIONS>::~CMRVisualizer()
40 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
41 class EDGE_ANNOTATIONS>
42 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
50 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
51 class EDGE_ANNOTATIONS>
52 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
67 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
74 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
76 EDGE_ANNOTATIONS>::~CMRVisualizer()
80 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
92 const double nodes_point_size =
101 map<string, CPointCloud::Ptr> strid_to_cloud;
103 map<string, TColorf> strid_to_color;
108 this->m_graph.nodes.begin();
109 n_it != this->m_graph.nodes.end(); ++n_it)
117 const string& curr_strid = curr_node.agent_ID_str;
120 if (strid_to_cloud.find(curr_strid) != strid_to_cloud.end())
125 strid_to_color.find(curr_strid) != strid_to_color.end(),
126 "Agent string ID not found in colors map even though its " 127 "CPointCloud exists.");
132 strid_to_cloud.insert(
134 curr_strid, mrpt::make_aligned_shared<CPointCloud>()));
136 strid_to_color.insert(
140 curr_cloud->setColor(strid_to_color.at(curr_strid));
141 curr_cloud->setPointSize(nodes_point_size);
142 curr_cloud->enablePointSmooth();
152 curr_cloud->insertPoint(
p.x(),
p.y(),
p.z());
158 strid_to_cloud.begin();
159 it != strid_to_cloud.end(); ++it)
161 object->insert(it->second);
166 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
176 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
183 PairToSetOfLines_t id_to_set_of_lines;
185 map<string, TColorf> strid_to_color;
191 const double interconnecting_edge_width =
196 edge_it != this->m_graph.end(); ++edge_it)
198 const TNodeID& start_node = edge_it->first.first;
199 const TNodeID& end_node = edge_it->first.second;
203 this->m_graph.nodes.find(start_node);
205 this->m_graph.nodes.find(end_node);
208 if (n_it1 == this->m_graph.nodes.end() ||
209 n_it2 == this->m_graph.nodes.end())
220 std::string curr_end_strid = n_it2->second.agent_ID_str;
221 bool is_interconnecting_edge =
222 n_it1->second.agent_ID_str != n_it2->second.agent_ID_str;
224 pair<string, bool> curr_pair =
225 make_pair(curr_end_strid, is_interconnecting_edge);
228 if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end())
235 id_to_set_of_lines.insert(
236 make_pair(curr_pair, mrpt::make_aligned_shared<CSetOfLines>()));
240 if (strid_to_color.find(curr_end_strid) == strid_to_color.end())
242 strid_to_color.insert(
250 id_to_set_of_lines.at(curr_pair);
253 curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
256 double curr_width = is_interconnecting_edge
257 ? interconnecting_edge_width
259 curr_set_of_lines->setLineWidth(curr_width);
269 curr_set_of_lines->appendLine(
277 it != id_to_set_of_lines.end(); ++it)
279 object->insert(it->second);
double x() const
Common members of all points & poses classes.
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
Base class for C*Visualizer classes.
mrpt::img::TColorf getNextTColorf()
Get the next RGB triad in TColorf form.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
Internal functions for MRPT.
The type of each global pose in nodes: an extension of the constraint_no_pdf_t pose with any optional...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
GLsizei const GLchar ** string
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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.
Manage R, G, B color triads and ask class instance of the next unique RGB combination.
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
const Scalar * const_iterator
Wrapper class that provides visualization of a network of poses that have been registered by many gra...
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...