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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019