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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013