MRPT  1.9.9
CMRVisualizer_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CMRVISUALIZER_IMPL_H
10 #define CMRVISUALIZER_IMPL_H
11 
12 #include <mrpt/img/TColorManager.h>
13 
14 namespace mrpt::graphs::detail
15 {
16 // (Dummy) standard version
17 // vvvvvvvvvvvvvvvvvvvvvvvv
18 //////////////////////////////////////////////////////////
19 
20 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
21  class EDGE_ANNOTATIONS>
22 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
23  EDGE_ANNOTATIONS>::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 <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
34  class EDGE_ANNOTATIONS>
35 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
36  EDGE_ANNOTATIONS>::~CMRVisualizer()
37 {
38 }
39 
40 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
41  class EDGE_ANNOTATIONS>
42 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
43  EDGE_ANNOTATIONS>::
44  drawNodePoints(
46  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
47 {
48 }
49 
50 template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
51  class EDGE_ANNOTATIONS>
52 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
53  EDGE_ANNOTATIONS>::
54  drawEdges(
56  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
57 {
58 }
59 
60 // ^^^^^^^^^^^^^^^^^^^^^^^^
61 // (Dummy Standard version ends here.
62 //////////////////////////////////////////////////////////
63 
64 // Specialized version for the multi-robot case
65 //////////////////////////////////////////////////////////
66 
67 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
68 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
69  EDGE_ANNOTATIONS>::CMRVisualizer(const GRAPH_T& graph_in)
70  : parent(graph_in)
71 {
72 }
73 
74 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
75 CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
76  EDGE_ANNOTATIONS>::~CMRVisualizer()
77 {
78 }
79 
80 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
81 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
82  EDGE_ANNOTATIONS>::
83  drawNodePoints(
85  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
86 {
87  using namespace mrpt::opengl;
88  using namespace mrpt::graphs;
89  using namespace mrpt::img;
90  using namespace std;
91 
92  const double nodes_point_size =
93  viz_params->getWithDefaultVal("nodes_point_size", 0.);
94 
95  // in case this is a combination of graphs like in multiple-robot
96  // graphSLAM applications, use a unique color for all the nodes that
97  // have initially been registered by each graphSLAM agent.
98 
99  TColorManager nodes_color_mngr;
100  // map of agent string identifier to its corresponding point cloud
101  map<string, CPointCloud::Ptr> strid_to_cloud;
102  // map of agent string identifier to its corresponding CPointCloud color
103  map<string, TColorf> strid_to_color;
104 
105  // traverse all nodes; Register a new pair to the aforementioned maps
106  // if a node of a (still) unknown agent is traversed
107  for (typename GRAPH_T::global_poses_t::const_iterator n_it =
108  this->m_graph.nodes.begin();
109  n_it != this->m_graph.nodes.end(); ++n_it)
110  {
111  const typename GRAPH_T::global_pose_t curr_node = n_it->second;
112 
113  // const TMRSlamNodeAnnotations* node_annots = dynamic_cast<const
114  // TMRSlamNodeAnnotations*>(&curr_node);
115  // ASSERT_(node_annots);
116  // std::string curr_strid = node_annots->agent_ID_str;
117  const string& curr_strid = curr_node.agent_ID_str;
118 
119  // have I already found this agent_ID_str?
120  if (strid_to_cloud.find(curr_strid) != strid_to_cloud.end())
121  {
122  // if the CPointCloud is registered, its color must also be
123  // registered.
124  ASSERTMSG_(
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.");
128  }
129  else
130  { // CPointCloud not yet registered.
131  // Create CPointCloud
132  strid_to_cloud.insert(
133  make_pair(
134  curr_strid, mrpt::make_aligned_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)
158  strid_to_cloud.begin();
159  it != strid_to_cloud.end(); ++it)
160  {
161  object->insert(it->second);
162  }
163 
164 } // end of drawNodePoints
165 
166 template <class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
167 void CMRVisualizer<CPOSE, MAPS_IMPLEMENTATION, TMRSlamNodeAnnotations,
168  EDGE_ANNOTATIONS>::
169  drawEdges(
171  const mrpt::system::TParametersDouble* viz_params /*=NULL*/) const
172 {
173  using namespace mrpt::opengl;
174  using namespace mrpt::img;
175 
176  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
177  using PairToSetOfLines_t = map<pair<string, bool>, CSetOfLines::Ptr>;
178 
179  // Color manager instance managing the used colors for the registered edges
180  TColorManager edges_color_mngr;
181  // map of <agent string identifier, is_interconnecting_edge>
182  // to its corresponding CSetOfLines pointer instance
183  PairToSetOfLines_t id_to_set_of_lines;
184  // map of agent string identifier to its corresponding CPointCloud color
185  map<string, TColorf> strid_to_color;
186 
187  const double edge_width = viz_params->getWithDefaultVal("edge_width", 2.);
188  // width of the edges connecting different the nodes registered by two
189  // different agents
190  // They are of outmost importance
191  const double interconnecting_edge_width =
192  viz_params->getWithDefaultVal("interconnecting_edge_width", 4.);
193 
194  // traverse all edges
195  for (typename GRAPH_T::const_iterator edge_it = this->m_graph.begin();
196  edge_it != this->m_graph.end(); ++edge_it)
197  {
198  const TNodeID& start_node = edge_it->first.first;
199  const TNodeID& end_node = edge_it->first.second;
200 
201  // iterator objects to the start and end node instances
203  this->m_graph.nodes.find(start_node);
205  this->m_graph.nodes.find(end_node);
206 
207  // Draw only if we have the global coords of both start and end nodes:
208  if (n_it1 == this->m_graph.nodes.end() ||
209  n_it2 == this->m_graph.nodes.end())
210  {
211  // skipping current edge...
212  continue;
213  }
214 
215  // the CSetOfLines of the current edge depends only on the combination
216  // of:
217  // - agent_ID_str field of the **end node**
218  // - Whether the edge is an interconnection between two nodes having
219  // different agent_ID_str values.
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;
223 
224  pair<string, bool> curr_pair =
225  make_pair(curr_end_strid, is_interconnecting_edge);
226 
227  // have I already found the current pair
228  if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end())
229  {
230  }
231  else
232  { // CSetOfLines not yet registered.
233  // Register a new CSetOfLines when a unique pair of
234  // <agent_ID_str (of end_node), is_interconnecting_edge> is found
235  id_to_set_of_lines.insert(
236  make_pair(curr_pair, mrpt::make_aligned_shared<CSetOfLines>()));
237 
238  // Create TColorf if not in map
239  // Color depends only on the agent_ID_str
240  if (strid_to_color.find(curr_end_strid) == strid_to_color.end())
241  {
242  strid_to_color.insert(
243  make_pair(
244  curr_end_strid, edges_color_mngr.getNextTColorf()));
245  }
246 
247  // both the CSetOfLines and TColorf entries should exist in their
248  // corresponding maps by now
249  CSetOfLines::Ptr& curr_set_of_lines =
250  id_to_set_of_lines.at(curr_pair);
251 
252  // color of the line
253  curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
254 
255  // width of the line
256  double curr_width = is_interconnecting_edge
257  ? interconnecting_edge_width
258  : edge_width;
259  curr_set_of_lines->setLineWidth(curr_width);
260 
261  } // end of (is CSetOfLines/Color registered)
262 
263  // CSetOfLines is initialized
264 
265  // insert current edge to its corresponding CSetOfLines instance
266  CSetOfLines::Ptr& curr_set_of_lines = id_to_set_of_lines.at(curr_pair);
267  const CPose3D p1 = CPose3D(n_it1->second);
268  const CPose3D p2 = CPose3D(n_it2->second);
269  curr_set_of_lines->appendLine(
270  mrpt::math::TPoint3D(p1.x(), p1.y(), p1.z()),
271  mrpt::math::TPoint3D(p2.x(), p2.y(), p2.z()));
272 
273  } // end for - nodes loop
274 
275  // insert all CSetOfLines(s)
276  for (PairToSetOfLines_t::const_iterator it = id_to_set_of_lines.begin();
277  it != id_to_set_of_lines.end(); ++it)
278  {
279  object->insert(it->second);
280  }
281 }
282 } // end of namespaces
283 
284 #endif /* end of include guard: CMRVISUALIZER_IMPL_H */
285 
286 
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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.
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:101
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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.
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:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Wrapper class that provides visualization of a network of poses that have been registered by many gra...
Definition: CMRVisualizer.h:37
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...
Definition: TParameters.h:103



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020