Main MRPT website > C++ reference for MRPT 1.5.6
CMRVisualizer_impl.h
Go to the documentation of this file.
1 #ifndef CMRVISUALIZER_IMPL_H
2 #define CMRVISUALIZER_IMPL_H
3 
5 
6 namespace mrpt { namespace graphs { namespace detail {
7 
8 
9 // (Dummy) standard version
10 // vvvvvvvvvvvvvvvvvvvvvvvv
11 //////////////////////////////////////////////////////////
12 
13 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
15 CMRVisualizer(const GRAPH_T& graph_in):
16  parent(graph_in)
17 {
19  "CMRVisualizer standard (non-specialized) edition doesn't server any role."
20  "In case you use this visualizer specify TMRSlamNodeAnnotations"
21  "as the 3rd template argument");
22 }
23 
24 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
27 
28 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
30 drawNodePoints(mrpt::opengl::CSetOfObjectsPtr& object,
31  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const { }
32 
33 template<class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
35 drawEdges(mrpt::opengl::CSetOfObjectsPtr& object,
36  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const { }
37 
38 // ^^^^^^^^^^^^^^^^^^^^^^^^
39 // (Dummy Standard version ends here.
40 //////////////////////////////////////////////////////////
41 
42 
43 // Specialized version for the multi-robot case
44 //////////////////////////////////////////////////////////
45 
46 template<class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
48 CMRVisualizer(const GRAPH_T& graph_in):
49  parent(graph_in)
50 { }
51 
52 template<class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
55 
56 
57 template<class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
59 drawNodePoints(mrpt::opengl::CSetOfObjectsPtr& object,
60  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
61 
62  using namespace mrpt::opengl;
63  using namespace mrpt::utils;
64  using namespace mrpt::graphs;
65  using namespace std;
66 
67  const double nodes_point_size = viz_params->getWithDefaultVal(
68  "nodes_point_size", 0.);
69 
70  // in case this is a combination of graphs like in multiple-robot
71  // graphSLAM applications, use a unique color for all the nodes that
72  // have initially been registered by each graphSLAM agent.
73 
74  TColorManager nodes_color_mngr;
75  // map of agent string identifier to its corresponding point cloud
76  map<string, CPointCloudPtr> strid_to_cloud;
77  // map of agent string identifier to its corresponding CPointCloud color
78  map<string, TColorf> strid_to_color;
79 
80  // traverse all nodes; Register a new pair to the aforementioned maps
81  // if a node of a (still) unknown agent is traversed
83  n_it = this->m_graph.nodes.begin();
84  n_it != this->m_graph.nodes.end();
85  ++n_it) {
86 
87  const typename GRAPH_T::global_pose_t curr_node = n_it->second;
88 
89  //const TMRSlamNodeAnnotations* node_annots = dynamic_cast<const TMRSlamNodeAnnotations*>(&curr_node);
90  //ASSERT_(node_annots);
91  //std::string curr_strid = node_annots->agent_ID_str;
92  const string& curr_strid = curr_node.agent_ID_str;
93 
94  // have I already found this agent_ID_str?
95  if (strid_to_cloud.find(curr_strid) != strid_to_cloud.end()) {
96 
97  // if the CPointCloud is registered, its color must also be registered.
98  ASSERTMSG_(strid_to_color.find(curr_strid) != strid_to_color.end(),
99  "Agent string ID not found in colors map even though its CPointCloud exists.");
100 
101  }
102  else { // CPointCloud not yet registered.
103  // Create CPointCloud
104  strid_to_cloud.insert(make_pair(
105  curr_strid, CPointCloud::Create()));
106  // Create TColorf
107  strid_to_color.insert(make_pair(
108  curr_strid, nodes_color_mngr.getNextTColorf()));
109 
110  CPointCloudPtr& curr_cloud = strid_to_cloud.at(curr_strid);
111  curr_cloud->setColor(strid_to_color.at(curr_strid));
112  curr_cloud->setPointSize(nodes_point_size);
113  curr_cloud->enablePointSmooth();
114 
115  } // end of (is CPointCloud/Color registered)
116 
117  // CPointCloud is initialized
118  const CPose3D p = CPose3D(n_it->second); // Convert to 3D from whatever its real type.
119 
120  // insert current pose to its corresponding CPointCloud instance
121  CPointCloudPtr& curr_cloud = strid_to_cloud.at(curr_strid);
122  curr_cloud->insertPoint(p.x(),p.y(), p.z());
123 
124  } // end for - nodes loop
125 
126  // insert all CPointCloud(s)
128  it = strid_to_cloud.begin();
129  it != strid_to_cloud.end();
130  ++it) {
131 
132  object->insert(it->second);
133  }
134 
135 } // end of drawNodePoints
136 
137 template<class CPOSE, class MAPS_IMPLEMENTATION, class EDGE_ANNOTATIONS>
139 drawEdges(mrpt::opengl::CSetOfObjectsPtr& object,
140  const mrpt::utils::TParametersDouble* viz_params/*=NULL*/) const {
141 
142  using namespace mrpt::opengl;
143  using namespace mrpt::utils;
144 
145  ASSERTMSG_(viz_params, "Pointer to viz_params was not provided.");
146  typedef map<pair<string, bool>, CSetOfLinesPtr> PairToSetOfLines_t;
147 
148  //Color manager instance managing the used colors for the registered edges
149  TColorManager edges_color_mngr;
150  // map of <agent string identifier, is_interconnecting_edge>
151  // to its corresponding CSetOfLines pointer instance
152  PairToSetOfLines_t id_to_set_of_lines;
153  // map of agent string identifier to its corresponding CPointCloud color
154  map<string, TColorf> strid_to_color;
155 
156  const double edge_width = viz_params->getWithDefaultVal(
157  "edge_width", 2.);
158  // width of the edges connecting different the nodes registered by two different agents
159  // They are of outmost importance
160  const double interconnecting_edge_width = viz_params->getWithDefaultVal(
161  "interconnecting_edge_width", 4.);
162 
163  // traverse all edges
164  for (typename GRAPH_T::const_iterator
165  edge_it = this->m_graph.begin();
166  edge_it != this->m_graph.end();
167  ++edge_it) {
168 
169  const TNodeID& start_node = edge_it->first.first;
170  const TNodeID& end_node = edge_it->first.second;
171 
172  // iterator objects to the start and end node instances
174  this->m_graph.nodes.find(start_node);
176  this->m_graph.nodes.find(end_node);
177 
178  // Draw only if we have the global coords of both start and end nodes:
179  if (n_it1 == this->m_graph.nodes.end() || n_it2 == this->m_graph.nodes.end()) {
180  // skipping current edge...
181  continue;
182  }
183 
184  // the CSetOfLines of the current edge depends only on the combination of:
185  // - agent_ID_str field of the **end node**
186  // - Whether the edge is an interconnection between two nodes having
187  // different agent_ID_str values.
188  std::string curr_end_strid = n_it2->second.agent_ID_str;
189  bool is_interconnecting_edge = n_it1->second.agent_ID_str != n_it2->second.agent_ID_str;
190 
191  pair<string, bool> curr_pair = make_pair(curr_end_strid, is_interconnecting_edge);
192 
193  // have I already found the current pair
194  if (id_to_set_of_lines.find(curr_pair) != id_to_set_of_lines.end()) {
195  }
196  else { // CSetOfLines not yet registered.
197  // Register a new CSetOfLines when a unique pair of
198  // <agent_ID_str (of end_node), is_interconnecting_edge> is found
199  id_to_set_of_lines.insert(make_pair(
200  curr_pair, CSetOfLines::Create()));
201 
202  // Create TColorf if not in map
203  // Color depends only on the agent_ID_str
204  if (strid_to_color.find(curr_end_strid) == strid_to_color.end()) {
205  strid_to_color.insert(make_pair(
206  curr_end_strid, edges_color_mngr.getNextTColorf()));
207  }
208 
209  // both the CSetOfLines and TColorf entries should exist in their
210  // corresponding maps by now
211  CSetOfLinesPtr& curr_set_of_lines =
212  id_to_set_of_lines.at(curr_pair);
213 
214  // color of the line
215  curr_set_of_lines->setColor(strid_to_color.at(curr_end_strid));
216 
217  // width of the line
218  double curr_width =
219  is_interconnecting_edge? interconnecting_edge_width : edge_width;
220  curr_set_of_lines->setLineWidth(curr_width);
221 
222  } // end of (is CSetOfLines/Color registered)
223 
224  // CSetOfLines is initialized
225 
226  // insert current edge to its corresponding CSetOfLines instance
227  CSetOfLinesPtr& curr_set_of_lines = id_to_set_of_lines.at(curr_pair);
228  const CPose3D p1 = CPose3D(n_it1->second);
229  const CPose3D p2 = CPose3D(n_it2->second);
230  curr_set_of_lines->appendLine(
231  mrpt::math::TPoint3D(p1.x(),p1.y(),p1.z()),
232  mrpt::math::TPoint3D(p2.x(),p2.y(),p2.z()));
233 
234  } // end for - nodes loop
235 
236  // insert all CSetOfLines(s)
238  it = id_to_set_of_lines.begin();
239  it != id_to_set_of_lines.end();
240  ++it) {
241 
242  object->insert(it->second);
243  }
244 } // end of drawEdges
245 
246 
247 } } } // end of namespaces
248 
249 #endif /* end of include guard: CMRVISUALIZER_IMPL_H */
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
void drawEdges(mrpt::opengl::CSetOfObjectsPtr &object, const mrpt::utils::TParametersDouble *viz_params=NULL) const
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
uint64_t TNodeID
The type for node IDs in graphs of different types.
Manage R, G, B color triads and ask class instance of the next unique RGB combination.
Definition: TColorManager.h:26
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...
mrpt::utils::TColorf getNextTColorf()
Get the next RGB triad in TColorf form.
GLsizei const GLchar ** string
Definition: glext.h:3919
void drawNodePoints(mrpt::opengl::CSetOfObjectsPtr &object, const mrpt::utils::TParametersDouble *viz_params=NULL) const
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:72
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:88
The namespace for 3D scene representation and rendering.
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
Lightweight 3D point.
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019