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


Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015