MRPT  2.0.1
CMRVisualizer_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/img/TColorManager.h>
12 
13 namespace mrpt::graphs::detail
14 {
15 // (Dummy) standard version
16 // vvvvvvvvvvvvvvvvvvvvvvvv
17 //////////////////////////////////////////////////////////
18 
19 template <
20  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
21  class EDGE_ANNOTATIONS>
23  CMRVisualizer(const GRAPH_T& graph_in)
24  : parent(graph_in)
25 {
27  "CMRVisualizer standard (non-specialized) edition doesn't server any "
28  "role."
29  "In case you use this visualizer specify TMRSlamNodeAnnotations"
30  "as the 3rd template argument");
31 }
32 
33 template <
34  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
35  class EDGE_ANNOTATIONS>
37  ~CMRVisualizer() = default;
38 
39 template <
40  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
41  class EDGE_ANNOTATIONS>
42 void CMRVisualizer<
43  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
44  drawNodePoints(
46  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
47 {
48 }
49 
50 template <
51  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
52  class EDGE_ANNOTATIONS>
53 void CMRVisualizer<
54  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
55  drawEdges(
57  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
58 {
59 }
60 
61 // ^^^^^^^^^^^^^^^^^^^^^^^^
62 // (Dummy Standard version ends here.
63 //////////////////////////////////////////////////////////
64 
65 // Specialized version for the multi-robot case
66 //////////////////////////////////////////////////////////
67 
68 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
70  CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
71  EDGE_ANNOTATIONS>::CMRVisualizer(const GRAPH_T& graph_in)
72  : parent(graph_in)
73 {
74 }
75 
76 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
78  CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
79  EDGE_ANNOTATIONS>::~CMRVisualizer() = default;
80 
81 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
82 void CMRVisualizer<
83  CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations, EDGE_ANNOTATIONS>::
84  drawNodePoints(
86  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
87 {
88  using namespace mrpt::opengl;
89  using namespace mrpt::graphs;
90  using namespace mrpt::img;
91  using namespace std;
92 
93  const double nodes_point_size =
94  viz_params->getWithDefaultVal("nodes_point_size", 0.);
95 
96  // in case this is a combination of graphs like in multiple-robot
97  // graphSLAM applications, use a unique color for all the nodes that
98  // have initially been registered by each graphSLAM agent.
99 
100  TColorManager nodes_color_mngr;
101  // map of agent string identifier to its corresponding point cloud
102  map<string, CPointCloud::Ptr> strid_to_cloud;
103  // map of agent string identifier to its corresponding CPointCloud color
104  map<string, TColorf> strid_to_color;
105 
106  // traverse all nodes; Register a new pair to the aforementioned maps
107  // if a node of a (still) unknown agent is traversed
108  for (typename GRAPH_T::global_poses_t::const_iterator n_it =
109  this->m_graph.nodes.begin();
110  n_it != this->m_graph.nodes.end(); ++n_it)
111  {
112  const typename GRAPH_T::global_pose_t curr_node = n_it->second;
113 
114  // const TMRSlamNodeAnnotations* node_annots = dynamic_cast<const
115  // TMRSlamNodeAnnotations*>(&curr_node);
116  // ASSERT_(node_annots);
117  // std::string curr_strid = node_annots->agent_ID_str;
118  const string& curr_strid = curr_node.agent_ID_str;
119 
120  // have I already found this agent_ID_str?
121  if (strid_to_cloud.find(curr_strid) != strid_to_cloud.end())
122  {
123  // if the CPointCloud is registered, its color must also be
124  // registered.
125  ASSERTMSG_(
126  strid_to_color.find(curr_strid) != strid_to_color.end(),
127  "Agent string ID not found in colors map even though its "
128  "CPointCloud exists.");
129  }
130  else
131  { // CPointCloud not yet registered.
132  // Create CPointCloud
133  strid_to_cloud.insert(
134  make_pair(curr_strid, std::make_shared<CPointCloud>()));
135  // Create TColorf
136  strid_to_color.insert(
137  make_pair(curr_strid, nodes_color_mngr.getNextTColorf()));
138 
139  CPointCloud::Ptr& curr_cloud = strid_to_cloud.at(curr_strid);
140  curr_cloud->setColor(strid_to_color.at(curr_strid));
141  curr_cloud->setPointSize(nodes_point_size);
142 
143  } // end of (is CPointCloud/Color registered)
144 
145  // CPointCloud is initialized
146  const CPose3D p = CPose3D(
147  n_it->second); // Convert to 3D from whatever its real type.
148 
149  // insert current pose to its corresponding CPointCloud instance
150  CPointCloud::Ptr& curr_cloud = strid_to_cloud.at(curr_strid);
151  curr_cloud->insertPoint(p.x(), p.y(), p.z());
152 
153  } // end for - nodes loop
154 
155  // insert all CPointCloud(s)
156  for (auto it = strid_to_cloud.begin(); it != strid_to_cloud.end(); ++it)
157  {
158  object->insert(it->second);
159  }
160 
161 } // end of drawNodePoints
162 
163 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
164 void CMRVisualizer<
165  CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations, EDGE_ANNOTATIONS>::
166  drawEdges(
168  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
169 {
170  using namespace mrpt::opengl;
171  using namespace mrpt::img;
172 
173  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
174  using PairToSetOfLines_t = map<pair<string, bool>, CSetOfLines::Ptr>;
175 
176  // Color manager instance managing the used colors for the registered edges
177  TColorManager edges_color_mngr;
178  // map of <agent string identifier, is_interconnecting_edge>
179  // to its corresponding CSetOfLines pointer instance
180  PairToSetOfLines_t id_to_set_of_lines;
181  // map of agent string identifier to its corresponding CPointCloud color
182  map<string, TColorf> strid_to_color;
183 
184  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
185  // width of the edges connecting different the nodes registered by two
186  // different agents
187  // They are of outmost importance
188  const double interconnecting_edge_width =
189  viz_params->getWithDefaultVal("interconnecting_edge_width", 4.);
190 
191  // traverse all edges
192  for (typename GRAPH_T::const_iterator edge_it = this->m_graph.begin();
193  edge_it != this->m_graph.end(); ++edge_it)
194  {
195  const TNodeID& start_node = edge_it->first.first;
196  const TNodeID& end_node = edge_it->first.second;
197 
198  // iterator objects to the start and end node instances
200  this->m_graph.nodes.find(start_node);
202  this->m_graph.nodes.find(end_node);
203 
204  // Draw only if we have the global coords of both start and end nodes:
205  if (n_it1 == this->m_graph.nodes.end() ||
206  n_it2 == this->m_graph.nodes.end())
207  {
208  // skipping current edge...
209  continue;
210  }
211 
212  // the CSetOfLines of the current edge depends only on the combination
213  // of:
214  // - agent_ID_str field of the **end node**
215  // - Whether the edge is an interconnection between two nodes having
216  // different agent_ID_str values.
217  std::string curr_end_strid = n_it2->second.agent_ID_str;
218  bool is_interconnecting_edge =
219  n_it1->second.agent_ID_str != n_it2->second.agent_ID_str;
220 
221  pair<string, bool> curr_pair =
222  make_pair(curr_end_strid, is_interconnecting_edge);
223 
224  // have I already found the current pair
225  if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end())
226  {
227  }
228  else
229  { // CSetOfLines not yet registered.
230  // Register a new CSetOfLines when a unique pair of
231  // <agent_ID_str (of end_node), is_interconnecting_edge> is found
232  id_to_set_of_lines.insert(
233  make_pair(curr_pair, std::make_shared<CSetOfLines>()));
234 
235  // Create TColorf if not in map
236  // Color depends only on the agent_ID_str
237  if (strid_to_color.find(curr_end_strid) == strid_to_color.end())
238  {
239  strid_to_color.insert(make_pair(
240  curr_end_strid, edges_color_mngr.getNextTColorf()));
241  }
242 
243  // both the CSetOfLines and TColorf entries should exist in their
244  // corresponding maps by now
245  CSetOfLines::Ptr& curr_set_of_lines =
246  id_to_set_of_lines.at(curr_pair);
247 
248  // color of the line
249  curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
250 
251  // width of the line
252  double curr_width = is_interconnecting_edge
253  ? interconnecting_edge_width
254  : edge_width;
255  curr_set_of_lines->setLineWidth(curr_width);
256 
257  } // end of (is CSetOfLines/Color registered)
258 
259  // CSetOfLines is initialized
260 
261  // insert current edge to its corresponding CSetOfLines instance
262  CSetOfLines::Ptr& curr_set_of_lines = id_to_set_of_lines.at(curr_pair);
263  const CPose3D p1 = CPose3D(n_it1->second);
264  const CPose3D p2 = CPose3D(n_it2->second);
265  curr_set_of_lines->appendLine(
266  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
267  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
268 
269  } // end for - nodes loop
270 
271  // insert all CSetOfLines(s)
272  for (auto it = id_to_set_of_lines.begin(); it != id_to_set_of_lines.end();
273  ++it)
274  {
275  object->insert(it->second);
276  }
277 }
278 } // namespace mrpt::graphs::detail
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Abstract graph and tree data structures, plus generic graph algorithms.
STL namespace.
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.
CMRVisualizer(const GRAPH_T &graph_in)
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.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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...
Definition: TParameters.h:90
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
typename edges_map_t::const_iterator const_iterator
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
Manage R, G, B color triads and ask class instance of the next unique RGB combination.
Definition: TColorManager.h:23
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
Wrapper class that provides visualization of a network of poses that have been registered by many gra...
Definition: CMRVisualizer.h:36



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020