MRPT  1.9.9
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-2019, 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  curr_cloud->enablePointSmooth();
143 
144  } // end of (is CPointCloud/Color registered)
145 
146  // CPointCloud is initialized
147  const CPose3D p = CPose3D(
148  n_it->second); // Convert to 3D from whatever its real type.
149 
150  // insert current pose to its corresponding CPointCloud instance
151  CPointCloud::Ptr& curr_cloud = strid_to_cloud.at(curr_strid);
152  curr_cloud->insertPoint(p.x(), p.y(), p.z());
153 
154  } // end for - nodes loop
155 
156  // insert all CPointCloud(s)
157  for (auto it = strid_to_cloud.begin(); it != strid_to_cloud.end(); ++it)
158  {
159  object->insert(it->second);
160  }
161 
162 } // end of drawNodePoints
163 
164 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
165 void CMRVisualizer<
166  CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations, EDGE_ANNOTATIONS>::
167  drawEdges(
169  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
170 {
171  using namespace mrpt::opengl;
172  using namespace mrpt::img;
173 
174  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
175  using PairToSetOfLines_t = map<pair<string, bool>, CSetOfLines::Ptr>;
176 
177  // Color manager instance managing the used colors for the registered edges
178  TColorManager edges_color_mngr;
179  // map of <agent string identifier, is_interconnecting_edge>
180  // to its corresponding CSetOfLines pointer instance
181  PairToSetOfLines_t id_to_set_of_lines;
182  // map of agent string identifier to its corresponding CPointCloud color
183  map<string, TColorf> strid_to_color;
184 
185  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
186  // width of the edges connecting different the nodes registered by two
187  // different agents
188  // They are of outmost importance
189  const double interconnecting_edge_width =
190  viz_params->getWithDefaultVal("interconnecting_edge_width", 4.);
191 
192  // traverse all edges
193  for (typename GRAPH_T::const_iterator edge_it = this->m_graph.begin();
194  edge_it != this->m_graph.end(); ++edge_it)
195  {
196  const TNodeID& start_node = edge_it->first.first;
197  const TNodeID& end_node = edge_it->first.second;
198 
199  // iterator objects to the start and end node instances
201  this->m_graph.nodes.find(start_node);
203  this->m_graph.nodes.find(end_node);
204 
205  // Draw only if we have the global coords of both start and end nodes:
206  if (n_it1 == this->m_graph.nodes.end() ||
207  n_it2 == this->m_graph.nodes.end())
208  {
209  // skipping current edge...
210  continue;
211  }
212 
213  // the CSetOfLines of the current edge depends only on the combination
214  // of:
215  // - agent_ID_str field of the **end node**
216  // - Whether the edge is an interconnection between two nodes having
217  // different agent_ID_str values.
218  std::string curr_end_strid = n_it2->second.agent_ID_str;
219  bool is_interconnecting_edge =
220  n_it1->second.agent_ID_str != n_it2->second.agent_ID_str;
221 
222  pair<string, bool> curr_pair =
223  make_pair(curr_end_strid, is_interconnecting_edge);
224 
225  // have I already found the current pair
226  if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end())
227  {
228  }
229  else
230  { // CSetOfLines not yet registered.
231  // Register a new CSetOfLines when a unique pair of
232  // <agent_ID_str (of end_node), is_interconnecting_edge> is found
233  id_to_set_of_lines.insert(
234  make_pair(curr_pair, std::make_shared<CSetOfLines>()));
235 
236  // Create TColorf if not in map
237  // Color depends only on the agent_ID_str
238  if (strid_to_color.find(curr_end_strid) == strid_to_color.end())
239  {
240  strid_to_color.insert(make_pair(
241  curr_end_strid, edges_color_mngr.getNextTColorf()));
242  }
243 
244  // both the CSetOfLines and TColorf entries should exist in their
245  // corresponding maps by now
246  CSetOfLines::Ptr& curr_set_of_lines =
247  id_to_set_of_lines.at(curr_pair);
248 
249  // color of the line
250  curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
251 
252  // width of the line
253  double curr_width = is_interconnecting_edge
254  ? interconnecting_edge_width
255  : edge_width;
256  curr_set_of_lines->setLineWidth(curr_width);
257 
258  } // end of (is CSetOfLines/Color registered)
259 
260  // CSetOfLines is initialized
261 
262  // insert current edge to its corresponding CSetOfLines instance
263  CSetOfLines::Ptr& curr_set_of_lines = id_to_set_of_lines.at(curr_pair);
264  const CPose3D p1 = CPose3D(n_it1->second);
265  const CPose3D p2 = CPose3D(n_it2->second);
266  curr_set_of_lines->appendLine(
267  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
268  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
269 
270  } // end for - nodes loop
271 
272  // insert all CSetOfLines(s)
273  for (auto it = id_to_set_of_lines.begin(); it != id_to_set_of_lines.end();
274  ++it)
275  {
276  object->insert(it->second);
277  }
278 }
279 } // 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
GLsizei const GLchar ** string
Definition: glext.h:4116
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:15
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
Lightweight 3D point.
Definition: TPoint3D.h:90
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...
GLfloat GLfloat p
Definition: glext.h:6398
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 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019