Main MRPT website > C++ reference
MRPT logo
CNetworkOfPoses_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-2014, 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 CONSTRAINED_POSE_NETWORK_IMPL_H
10 #define CONSTRAINED_POSE_NETWORK_IMPL_H
11 
12 #include <mrpt/graphs/dijkstra.h>
14 
15 
16 namespace mrpt
17 {
18  namespace graphs
19  {
20  namespace detail
21  {
22  using namespace std;
23  using namespace mrpt;
24  using namespace mrpt::utils;
25  using namespace mrpt::poses;
26  using namespace mrpt::graphs;
27 
28  template <class POSE_PDF> struct TPosePDFHelper
29  {
30  static inline void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
31  static inline void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
32  };
33  template <> struct TPosePDFHelper<CPose2D>
34  {
35  static inline void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
36  static inline void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf ) { p = CPose2D(pdf.mean); }
37  };
38  template <> struct TPosePDFHelper<CPose3D>
39  {
40  static inline void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
41  static inline void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf ) { p = pdf.mean; }
42  };
43 
44  /// a helper struct with static template functions \sa CNetworkOfPoses
45  template <class graph_t>
46  struct graph_ops
47  {
48  static void write_VERTEX_line(const TNodeID id, const CPose2D &p, std::ofstream &f)
49  {
50  // VERTEX2 id x y phi
51  f << "VERTEX2 " << id << " " << p.x() << " " << p.y() << " " << p.phi() << endl;
52  }
53  static void write_VERTEX_line(const TNodeID id, const CPose3D &p, std::ofstream &f)
54  {
55  // VERTEX3 id x y z roll pitch yaw
56  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
57  f << "VERTEX3 " << id << " " << p.x() << " " << p.y() << " " << p.z()<< " " << p.roll()<< " " << p.pitch()<< " " << p.yaw() << endl;
58  }
59 
60 
61  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussianInf & edge, std::ofstream &f)
62  {
63  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
64  // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
65  f << "EDGE2 " << edgeIDs.first << " " << edgeIDs.second << " " <<
66  edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.phi()<<" "<<
67  edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(1,1)<<" "<<
68  edge.cov_inv(2,2)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(1,2) << endl;
69  }
70  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussianInf & edge, std::ofstream &f)
71  {
72  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
73  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
74  // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
75  f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " " <<
76  edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.z()<<" "<<
77  edge.mean.roll()<<" "<<edge.mean.pitch()<<" "<<edge.mean.yaw()<<" "<<
78  edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(0,5)<<" "<<edge.cov_inv(0,4)<<" "<<edge.cov_inv(0,3)<<" "<<
79  edge.cov_inv(1,1)<<" "<<edge.cov_inv(1,2)<<" "<<edge.cov_inv(1,5)<<" "<<edge.cov_inv(1,4)<<" "<<edge.cov_inv(1,3)<<" "<<
80  edge.cov_inv(2,2)<<" "<<edge.cov_inv(2,5)<<" "<<edge.cov_inv(2,4)<<" "<<edge.cov_inv(2,3)<<" "<<
81  edge.cov_inv(5,5)<<" "<<edge.cov_inv(5,4)<<" "<<edge.cov_inv(5,3)<<" "<<
82  edge.cov_inv(4,4)<<" "<<edge.cov_inv(4,3)<<" "<<
83  edge.cov_inv(3,3) << endl;
84  }
85  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussian & edge, std::ofstream &f)
86  {
88  p.copyFrom(edge);
89  write_EDGE_line(edgeIDs,p,f);
90  }
91  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussian & edge, std::ofstream &f)
92  {
94  p.copyFrom(edge);
95  write_EDGE_line(edgeIDs,p,f);
96  }
97  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose2D & edge, std::ofstream &f)
98  {
100  p.mean = edge;
101  p.cov_inv.unit(3,1.0);
102  write_EDGE_line(edgeIDs,p,f);
103  }
104  static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3D & edge, std::ofstream &f)
105  {
107  p.mean = edge;
108  p.cov_inv.unit(6,1.0);
109  write_EDGE_line(edgeIDs,p,f);
110  }
111 
112 
113  // =================================================================
114  // save_graph_of_poses_to_text_file
115  // =================================================================
116  static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
117  {
118  typedef typename graph_t::constraint_t CPOSE;
119 
120  std::ofstream f;
121  f.open(fil.c_str());
122  if (!f.is_open())
123  THROW_EXCEPTION_CUSTOM_MSG1("Error opening file '%s' for writing",fil.c_str());
124 
125  // 1st: Nodes
126  for (typename graph_t::global_poses_t::const_iterator itNod = g->nodes.begin();itNod!=g->nodes.end();++itNod)
127  write_VERTEX_line(itNod->first, itNod->second, f);
128 
129  // 2nd: Edges:
130  for (typename graph_t::const_iterator it=g->begin();it!=g->end();++it)
131  if (it->first.first!=it->first.second) // Ignore self-edges, typically from importing files with EQUIV's
132  write_EDGE_line( it->first, it->second, f);
133 
134  } // end save_graph
135 
136  // =================================================================
137  // save_graph_of_poses_to_binary_file
138  // =================================================================
139  static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
140  {
141  // Store class name:
142  const std::string sClassName = TTypeName<graph_t>::get();
143  out << sClassName;
144 
145  // Store serialization version & object data:
146  const uint32_t version = 0;
147  out << version;
148  out << g->nodes << g->edges << g->root;
149  }
150 
151  // =================================================================
152  // read_graph_of_poses_from_binary_file
153  // =================================================================
155  {
156  // Compare class name:
157  const std::string sClassName = TTypeName<graph_t>::get();
158  std::string sStoredClassName;
159  in >> sStoredClassName;
160  ASSERT_EQUAL_(sStoredClassName,sClassName)
161 
162  // Check serialization version:
163  uint32_t stored_version;
164  in >> stored_version;
165 
166  g->clear();
167  switch (stored_version)
168  {
169  case 0:
170  in >> g->nodes >> g->edges >> g->root;
171  break;
172  default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(stored_version)
173  }
174 
175  }
176 
177  // =================================================================
178  // load_graph_of_poses_from_text_file
179  // =================================================================
180  static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
181  {
182  typedef typename graph_t::constraint_t CPOSE;
183 
184  set<string> alreadyWarnedUnknowns; // for unknown line types, show a warning to cerr just once.
185 
186  // First, empty the graph:
187  g->clear();
188 
189  // Determine if it's a 2D or 3D graph, just to raise an error if loading a 3D graph in a 2D one, since
190  // it would be an unintentional loss of information:
191  const bool graph_is_3D = CPOSE::is_3D();
192 
193  CTextFileLinesParser filParser(fil); // raises an exception on error
194 
195  // -------------------------------------------
196  // 1st PASS: Read EQUIV entries only
197  // since processing them AFTER loading the data
198  // is much much slower.
199  // -------------------------------------------
200  map<TNodeID,TNodeID> lstEquivs; // List of EQUIV entries: NODEID -> NEWNODEID. NEWNODEID will be always the lowest ID number.
201 
202  // Read & process lines each at once until EOF:
203  istringstream s;
204  while (filParser.getNextLine(s))
205  {
206  const unsigned int lineNum = filParser.getCurrentLineNumber();
207  const string lin = s.str();
208 
209  string key;
210  if ( !(s >> key) || key.empty() )
211  THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
212 
213  if ( strCmpI(key,"EQUIV") )
214  {
215  // Process these ones at the end, for now store in a list:
216  TNodeID id1,id2;
217  if (!(s>> id1 >> id2))
218  THROW_EXCEPTION(format("Line %u: Can't read id1 & id2 in EQUIV line: '%s'", lineNum, lin.c_str() ) );
219  lstEquivs[std::max(id1,id2)] = std::min(id1,id2);
220  }
221  } // end 1st pass
222 
223  // -------------------------------------------
224  // 2nd PASS: Read all other entries
225  // -------------------------------------------
226  filParser.rewind();
227 
228  // Read & process lines each at once until EOF:
229  while (filParser.getNextLine(s))
230  {
231  const unsigned int lineNum = filParser.getCurrentLineNumber();
232  const string lin = s.str();
233 
234  // Recognized strings:
235  // VERTEX2 id x y phi
236  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
237  // VERTEX3 id x y z roll pitch yaw
238  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
239  // EQUIV id1 id2
240  string key;
241  if ( !(s >> key) || key.empty() )
242  THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
243 
244  if ( strCmpI(key,"VERTEX2") || strCmpI(key,"VERTEX") )
245  {
246  TNodeID id;
247  TPose2D p2D;
248  if (!(s>> id >> p2D.x >> p2D.y >> p2D.phi))
249  THROW_EXCEPTION(format("Line %u: Error parsing VERTEX2 line: '%s'", lineNum, lin.c_str() ) );
250 
251  // Make sure the node is new:
252  if (g->nodes.find(id)!=g->nodes.end())
253  THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );
254 
255  // EQUIV? Replace ID by new one.
256  {
257  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
258  if (itEq!=lstEquivs.end()) id = itEq->second;
259  }
260 
261  // Add to map: ID -> absolute pose:
262  if (g->nodes.find(id)==g->nodes.end())
263  {
264  typename CNetworkOfPoses<CPOSE>::constraint_t::type_value & newNode = g->nodes[id];
265  newNode = CPose2D(p2D); // Auto converted to CPose3D if needed
266  }
267  }
268  else if ( strCmpI(key,"VERTEX3") )
269  {
270  if (!graph_is_3D)
271  THROW_EXCEPTION(format("Line %u: Try to load VERTEX3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
272 
273 
274  // VERTEX3 id x y z roll pitch yaw
275  TNodeID id;
276  TPose3D p3D;
277  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
278  if (!(s>> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >> p3D.pitch >> p3D.yaw ))
279  THROW_EXCEPTION(format("Line %u: Error parsing VERTEX3 line: '%s'", lineNum, lin.c_str() ) );
280 
281  // Make sure the node is new:
282  if (g->nodes.find(id)!=g->nodes.end())
283  THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );
284 
285  // EQUIV? Replace ID by new one.
286  {
287  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
288  if (itEq!=lstEquivs.end()) id = itEq->second;
289  }
290 
291  // Add to map: ID -> absolute pose:
292  if (g->nodes.find(id)==g->nodes.end())
293  {
294  g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value( CPose3D(p3D) ); // Auto converted to CPose2D if needed
295  }
296  }
297  else if ( strCmpI(key,"EDGE2") || strCmpI(key,"EDGE") )
298  {
299  // EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
300  // s00 s01 s11 s22 s02 s12
301  // Read values are:
302  // [ s00 s01 s02 ]
303  // [ - s11 s12 ]
304  // [ - - s22 ]
305  //
306  TNodeID to_id, from_id;
307  if (!(s>> from_id >> to_id ))
308  THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );
309 
310  // EQUIV? Replace ID by new one.
311  {
312  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
313  if (itEq!=lstEquivs.end()) to_id = itEq->second;
314  }
315  {
316  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
317  if (itEq!=lstEquivs.end()) from_id = itEq->second;
318  }
319 
320  if (from_id!=to_id) // Don't load self-edges! (probably come from an EQUIV)
321  {
322  TPose2D Ap_mean;
323  CMatrixDouble33 Ap_cov_inv;
324  if (!(s>>
325  Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >>
326  Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(1,1) >>
327  Ap_cov_inv(2,2) >> Ap_cov_inv(0,2) >> Ap_cov_inv(1,2) ))
328  THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );
329 
330  // Complete low triangular part of inf matrix:
331  Ap_cov_inv(1,0) = Ap_cov_inv(0,1);
332  Ap_cov_inv(2,0) = Ap_cov_inv(0,2);
333  Ap_cov_inv(2,1) = Ap_cov_inv(1,2);
334 
335  // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
336  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
337  TPosePDFHelper<CPOSE>::copyFrom2D(newEdge, CPosePDFGaussianInf( CPose2D(Ap_mean), Ap_cov_inv ) );
338  g->insertEdge(from_id, to_id, newEdge);
339  }
340  }
341  else if ( strCmpI(key,"EDGE3") )
342  {
343  if (!graph_is_3D)
344  THROW_EXCEPTION(format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
345 
346  // EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
347  TNodeID to_id, from_id;
348  if (!(s>> from_id >> to_id ))
349  THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );
350 
351  // EQUIV? Replace ID by new one.
352  {
353  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
354  if (itEq!=lstEquivs.end()) to_id = itEq->second;
355  }
356  {
357  const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
358  if (itEq!=lstEquivs.end()) from_id = itEq->second;
359  }
360 
361  if (from_id!=to_id) // Don't load self-edges! (probably come from an EQUIV)
362  {
363  TPose3D Ap_mean;
364  CMatrixDouble66 Ap_cov_inv;
365  // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
366  if (!(s>> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.roll >> Ap_mean.pitch >> Ap_mean.yaw ))
367  THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );
368 
369  // **CAUTION** Indices are shuffled to the change YAW(3) <-> ROLL(5) in the order of the data.
370  if (!(s>>
371  Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
372  Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
373  Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
374  Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
375  Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
376  Ap_cov_inv(3,3) ))
377  {
378  // Cov may be omitted in the file:
379  Ap_cov_inv.unit(6,1.0);
380 
381  if (alreadyWarnedUnknowns.find("MISSING_3D")==alreadyWarnedUnknowns.end())
382  {
383  alreadyWarnedUnknowns.insert("MISSING_3D");
384  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: Information matrix missing, assuming unity.\n";
385  }
386  }
387  else
388  {
389  // Complete low triangular part of inf matrix:
390  for (size_t r=1;r<6;r++)
391  for (size_t c=0;c<r;c++)
392  Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
393  }
394 
395  // Convert as needed:
396  typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
397  TPosePDFHelper<CPOSE>::copyFrom3D(newEdge, CPose3DPDFGaussianInf( CPose3D(Ap_mean), Ap_cov_inv ) );
398  g->insertEdge(from_id, to_id, newEdge);
399  }
400  }
401  else if ( strCmpI(key,"EQUIV") )
402  {
403  // Already read in the 1st pass.
404  }
405  else
406  { // Unknown entry: Warn the user just once:
407  if (alreadyWarnedUnknowns.find(key)==alreadyWarnedUnknowns.end())
408  {
409  alreadyWarnedUnknowns.insert(key);
410  cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: unknown entry type: " << key << endl;
411  }
412  }
413  } // end while
414 
415  } // end load_graph
416 
417 
418  // --------------------------------------------------------------------------------
419  // Implements: collapseDuplicatedEdges
420  //
421  // Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
422  // Upon return, only one edge remains between each pair of nodes with the mean
423  // & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.
424  // --------------------------------------------------------------------------------
425  static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
426  {
427  MRPT_START
428  typedef typename graph_t::edges_map_t::iterator TEdgeIterator;
429 
430  // Data structure: (id1,id2) -> all edges between them
431  // (with id1 < id2)
432  typedef map<pair<TNodeID,TNodeID>, vector<TEdgeIterator> > TListAllEdges; // For god's sake... when will ALL compilers support auto!! :'-(
433  TListAllEdges lstAllEdges;
434 
435  // Clasify all edges to identify duplicated ones:
436  for (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
437  {
438  // Build a pair <id1,id2> with id1 < id2:
439  const pair<TNodeID,TNodeID> arc_id = make_pair( std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second) );
440  // get (or create the first time) the list of edges between them:
441  vector<TEdgeIterator> &lstEdges = lstAllEdges[arc_id];
442  // And add this one:
443  lstEdges.push_back(itEd);
444  }
445 
446  // Now, remove all but the first edge:
447  size_t nRemoved = 0;
448  for (typename TListAllEdges::const_iterator it=lstAllEdges.begin();it!=lstAllEdges.end();++it)
449  {
450  const size_t N = it->second.size();
451  for (size_t i=1;i<N;i++) // i=0 is NOT removed
452  g->edges.erase( it->second[i] );
453 
454  if (N>=2) nRemoved+=N-1;
455  }
456 
457  return nRemoved;
458  MRPT_END
459  } // end of graph_of_poses_collapse_dup_edges
460 
461  // --------------------------------------------------------------------------------
462  // Implements: dijkstra_nodes_estimate
463  //
464  // Compute a simple estimation of the global coordinates of each node just from the information in all edges, sorted in a Dijkstra tree based on the current "root" node.
465  // Note that "global" coordinates are with respect to the node with the ID specified in \a root.
466  // --------------------------------------------------------------------------------
467  static void graph_of_poses_dijkstra_init(graph_t *g)
468  {
469  MRPT_START
470 
471  // Do Dijkstra shortest path from "root" to all other nodes:
473  typedef typename graph_t::constraint_t constraint_t;
474 
475  dijkstra_t dijkstra(*g, g->root);
476 
477  // Get the tree representation of the graph and traverse it
478  // from its root toward the leafs:
479  typename dijkstra_t::tree_graph_t treeView;
480  dijkstra.getTreeGraph(treeView);
481 
482  // This visitor class performs the real job of
483  struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
484  {
485  graph_t * m_g; // The original graph
486 
487  VisitorComputePoses(graph_t *g) : m_g(g) { }
488  virtual void OnVisitNode( const TNodeID parent_id, const typename dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo &edge_to_child, const size_t depth_level )
489  {
490  const TNodeID child_id = edge_to_child.id;
491 
492  // Compute the pose of "child_id" as parent_pose (+) edge_delta_pose,
493  // taking into account that that edge may be in reverse order and then have to invert the delta_pose:
494  if ( (!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
495  ( edge_to_child.reverse && m_g->edges_store_inverse_poses)
496  )
497  { // pose_child = p_parent (+) p_delta
498  m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], edge_to_child.data->getPoseMean() );
499  }
500  else
501  { // pose_child = p_parent (+) [(-)p_delta]
502  m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], - edge_to_child.data->getPoseMean() );
503  }
504  }
505  };
506 
507  // Remove all global poses but for the root node, which is the origin:
508  g->nodes.clear();
509  g->nodes[g->root] = typename constraint_t::type_value(); // Typ: CPose2D() or CPose3D()
510 
511  // Run the visit thru all nodes in the tree:
512  VisitorComputePoses myVisitor(g);
513  treeView.visitBreadthFirst(treeView.root, myVisitor);
514 
515  MRPT_END
516  }
517 
518 
519  // Auxiliary funcs:
520  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussianInf &p) {
521  math::wrapToPiInPlace(err[2]);
522  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
523  }
524  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussianInf &p) {
525  math::wrapToPiInPlace(err[3]);
526  math::wrapToPiInPlace(err[4]);
527  math::wrapToPiInPlace(err[5]);
528  return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
529  }
530  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussian &p) {
531  math::wrapToPiInPlace(err[2]);
533  p.cov.inv(COV_INV);
534  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
535  }
536  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussian &p) {
537  math::wrapToPiInPlace(err[3]);
538  math::wrapToPiInPlace(err[4]);
539  math::wrapToPiInPlace(err[5]);
541  p.cov.inv(COV_INV);
542  return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
543  }
544  // These two are for simulating maha2 distances for non-PDF types: fallback to squared-norm:
545  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose2D &p) {
546  math::wrapToPiInPlace(err[2]);
547  return square(err[0])+square(err[1])+square(err[2]);
548  }
549  template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3D &p) {
550  math::wrapToPiInPlace(err[3]);
551  math::wrapToPiInPlace(err[4]);
552  math::wrapToPiInPlace(err[5]);
553  return square(err[0])+square(err[1])+square(err[2])+square(err[3])+square(err[4])+square(err[5]);
554  }
555 
556 
557  static inline double auxEuclid2Dist(const CPose2D &p1,const CPose2D &p2) {
558  return
559  square(p1.x()-p2.x())+
560  square(p1.y()-p2.y())+
561  square( mrpt::math::wrapToPi(p1.phi()-p2.phi() ) );
562  }
563  static inline double auxEuclid2Dist(const CPose3D &p1,const CPose3D &p2) {
564  return
565  square(p1.x()-p2.x())+
566  square(p1.y()-p2.y())+
567  square(p1.z()-p2.z())+
568  square( mrpt::math::wrapToPi(p1.yaw()-p2.yaw() ) )+
569  square( mrpt::math::wrapToPi(p1.pitch()-p2.pitch() ) )+
570  square( mrpt::math::wrapToPi(p1.roll()-p2.roll() ) );
571  }
572 
573  // --------------------------------------------------------------------------------
574  // Implements: detail::graph_edge_sqerror
575  //
576  // Compute the square error of a single edge, in comparison to the nodes global poses.
577  // --------------------------------------------------------------------------------
578  static double graph_edge_sqerror(
579  const graph_t *g,
581  bool ignoreCovariances )
582  {
583  MRPT_START
584 
585  // Get node IDs:
586  const TNodeID from_id = itEdge->first.first;
587  const TNodeID to_id = itEdge->first.second;
588 
589  // And their global poses as stored in "nodes"
590  typename graph_t::global_poses_t::const_iterator itPoseFrom = g->nodes.find(from_id);
591  typename graph_t::global_poses_t::const_iterator itPoseTo = g->nodes.find(to_id);
592  ASSERTMSG_(itPoseFrom!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(from_id)))
593  ASSERTMSG_(itPoseTo!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(to_id)))
594 
595  // The global poses:
596  typedef typename graph_t::constraint_t constraint_t;
597 
598  const typename constraint_t::type_value &from_mean = itPoseFrom->second;
599  const typename constraint_t::type_value &to_mean = itPoseTo->second;
600 
601  // The delta_pose as stored in the edge:
602  const constraint_t &edge_delta_pose = itEdge->second;
603  const typename constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();
604 
605  if (ignoreCovariances)
606  { // Square Euclidean distance: Just use the mean values, ignore covs.
607  // from_plus_delta = from_mean (+) edge_delta_pose_mean
608  typename constraint_t::type_value from_plus_delta(UNINITIALIZED_POSE);
609  from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
610 
611  // (auxMaha2Dist will also take into account the 2PI wrapping)
612  return auxEuclid2Dist(from_plus_delta,to_mean);
613  }
614  else
615  {
616  // Square Mahalanobis distance
617  // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
618  constraint_t from_plus_delta = edge_delta_pose;
619  from_plus_delta.changeCoordinatesReference(from_mean);
620 
621  // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to "to_mean":
622  // We want to compute the squared Mahalanobis distance:
623  // err^t * INV_COV * err
624  //
625  CArrayDouble<constraint_t::type_value::static_size> err;
626  for (size_t i=0;i<constraint_t::type_value::static_size;i++)
627  err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
628 
629  // (auxMaha2Dist will also take into account the 2PI wrapping)
630  return auxMaha2Dist(err,from_plus_delta);
631  }
632  MRPT_END
633  }
634 
635  }; // end of graph_ops<graph_t>
636 
637  }// end NS
638  }// end NS
639 } // end NS
640 
641 #endif
#define ASSERT_EQUAL_(__A, __B)
A directed graph with the argument of the template specifying the type of the annotations in the edge...
CMatrixDouble66 cov
The 6x6 covariance matrix.
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:116
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
static double auxMaha2Dist(VEC &err, const CPose3D &p)
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:46
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3D &edge, std::ofstream &f)
void copyFrom(const CPosePDF &o)
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
static void write_VERTEX_line(const TNodeID id, const CPose3D &p, std::ofstream &f)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
static double auxEuclid2Dist(const CPose2D &p1, const CPose2D &p2)
Scalar * iterator
Definition: eigen_plugins.h:23
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:388
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:387
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:141
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ofstream &f)
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ofstream &f)
static void graph_of_poses_dijkstra_init(graph_t *g)
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
void rewind()
Reset the read pointer to the beginning of the file.
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ofstream &f)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
static double auxMaha2Dist(VEC &err, const CPose2D &p)
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose2D &edge, std::ofstream &f)
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
static void write_VERTEX_line(const TNodeID id, const CPose2D &p, std::ofstream &f)
CMatrixDouble33 cov
The 3x3 covariance matrix.
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:389
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
#define MRPT_START
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:158
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
A class used to store a 2D pose.
Definition: CPose2D.h:35
uint64_t TNodeID
The type for node IDs in graphs of different types.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:71
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:83
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
static double auxEuclid2Dist(const CPose3D &p1, const CPose3D &p2)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
void copyFrom(const CPose3DPDF &o)
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:40
CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
#define ASSERTMSG_(f, __ERROR_MSG)
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ofstream &f)
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo