9 #ifndef CMRVISUALIZER_IMPL_H
10 #define CMRVISUALIZER_IMPL_H
24 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
25 class EDGE_ANNOTATIONS>
26 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
31 "CMRVisualizer standard (non-specialized) edition doesn't server any "
33 "In case you use this visualizer specify TMRSlamNodeAnnotations"
34 "as the 3rd template argument");
37 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
38 class EDGE_ANNOTATIONS>
40 EDGE_ANNOTATIONS>::~CMRVisualizer()
44 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
45 class EDGE_ANNOTATIONS>
46 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
54 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
55 class EDGE_ANNOTATIONS>
56 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
71 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
78 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
80 EDGE_ANNOTATIONS>::~CMRVisualizer()
84 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
96 const double nodes_point_size =
105 map<string, CPointCloud::Ptr> strid_to_cloud;
107 map<string, TColorf> strid_to_color;
112 this->m_graph.nodes.begin();
113 n_it != this->m_graph.nodes.end(); ++n_it)
121 const string& curr_strid = curr_node.agent_ID_str;
124 if (strid_to_cloud.find(curr_strid) != strid_to_cloud.end())
129 strid_to_color.find(curr_strid) != strid_to_color.end(),
130 "Agent string ID not found in colors map even though its "
131 "CPointCloud exists.");
136 strid_to_cloud.insert(
138 curr_strid, mrpt::make_aligned_shared<CPointCloud>()));
140 strid_to_color.insert(
144 curr_cloud->setColor(strid_to_color.at(curr_strid));
145 curr_cloud->setPointSize(nodes_point_size);
146 curr_cloud->enablePointSmooth();
156 curr_cloud->insertPoint(
p.x(),
p.y(),
p.z());
162 strid_to_cloud.begin();
163 it != strid_to_cloud.end(); ++it)
165 object->insert(it->second);
170 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class EDGE_ANNOTATIONS>
180 ASSERTMSG_(viz_params,
"Pointer to viz_params was not provided.");
187 PairToSetOfLines_t id_to_set_of_lines;
189 map<string, TColorf> strid_to_color;
195 const double interconnecting_edge_width =
200 edge_it != this->m_graph.end(); ++edge_it)
202 const TNodeID& start_node = edge_it->first.first;
203 const TNodeID& end_node = edge_it->first.second;
207 this->m_graph.nodes.find(start_node);
209 this->m_graph.nodes.find(end_node);
212 if (n_it1 == this->m_graph.nodes.end() ||
213 n_it2 == this->m_graph.nodes.end())
224 std::string curr_end_strid = n_it2->second.agent_ID_str;
225 bool is_interconnecting_edge =
226 n_it1->second.agent_ID_str != n_it2->second.agent_ID_str;
228 pair<string, bool> curr_pair =
229 make_pair(curr_end_strid, is_interconnecting_edge);
232 if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end())
239 id_to_set_of_lines.insert(
240 make_pair(curr_pair, mrpt::make_aligned_shared<CSetOfLines>()));
244 if (strid_to_color.find(curr_end_strid) == strid_to_color.end())
246 strid_to_color.insert(
254 id_to_set_of_lines.at(curr_pair);
257 curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
260 double curr_width = is_interconnecting_edge
261 ? interconnecting_edge_width
263 curr_set_of_lines->setLineWidth(curr_width);
273 curr_set_of_lines->appendLine(
281 it != id_to_set_of_lines.end(); ++it)
283 object->insert(it->second);