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



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:37:47 UTC 2022