Point Cloud Library (PCL)  1.7.1
tracking.hpp
1 #ifndef PCL_TRACKING_IMPL_TRACKING_H_
2 #define PCL_TRACKING_IMPL_TRACKING_H_
3 
4 #include <pcl/common/eigen.h>
5 #include <ctime>
6 #include <pcl/tracking/boost.h>
7 #include <pcl/tracking/tracking.h>
8 
9 namespace pcl
10 {
11  namespace tracking
12  {
13  struct _ParticleXYZRPY
14  {
15  PCL_ADD_POINT4D;
16  union
17  {
18  struct
19  {
20  float roll;
21  float pitch;
22  float yaw;
23  float weight;
24  };
25  float data_c[4];
26  };
27  };
28 
29  // particle definition
30  struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY
31  {
32  inline ParticleXYZRPY ()
33  {
34  x = y = z = roll = pitch = yaw = 0.0;
35  data[3] = 1.0f;
36  }
37 
38  inline ParticleXYZRPY (float _x, float _y, float _z)
39  {
40  x = _x; y = _y; z = _z;
41  roll = pitch = yaw = 0.0;
42  data[3] = 1.0f;
43  }
44 
45  inline ParticleXYZRPY (float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
46  {
47  x = _x; y = _y; z = _z;
48  roll = _roll; pitch = _pitch; yaw = _yaw;
49  data[3] = 1.0f;
50  }
51 
52  inline static int
53  stateDimension () { return 6; }
54 
55  void
56  sample (const std::vector<double>& mean, const std::vector<double>& cov)
57  {
58  x += static_cast<float> (sampleNormal (mean[0], cov[0]));
59  y += static_cast<float> (sampleNormal (mean[1], cov[1]));
60  z += static_cast<float> (sampleNormal (mean[2], cov[2]));
61  roll += static_cast<float> (sampleNormal (mean[3], cov[3]));
62  pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
63  yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
64  }
65 
66  void
67  zero ()
68  {
69  x = 0.0;
70  y = 0.0;
71  z = 0.0;
72  roll = 0.0;
73  pitch = 0.0;
74  yaw = 0.0;
75  }
76 
77  inline Eigen::Affine3f
78  toEigenMatrix () const
79  {
80  return getTransformation(x, y, z, roll, pitch, yaw);
81  }
82 
83  static pcl::tracking::ParticleXYZRPY
84  toState (const Eigen::Affine3f &trans)
85  {
86  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
87  getTranslationAndEulerAngles (trans,
88  trans_x, trans_y, trans_z,
89  trans_roll, trans_pitch, trans_yaw);
90  return pcl::tracking::ParticleXYZRPY (trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
91  }
92 
93  // a[i]
94  inline float operator [] (unsigned int i)
95  {
96  switch (i)
97  {
98  case 0: return x;
99  case 1: return y;
100  case 2: return z;
101  case 3: return roll;
102  case 4: return pitch;
103  case 5: return yaw;
104  default: return 0.0;
105  }
106  }
107 
108  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109  };
110 
111  inline std::ostream& operator << (std::ostream& os, const ParticleXYZRPY& p)
112  {
113  os << "(" << p.x << "," << p.y << "," << p.z << ","
114  << p.roll << "," << p.pitch << "," << p.yaw << ")";
115  return (os);
116  }
117 
118  // a * k
119  inline pcl::tracking::ParticleXYZRPY operator * (const ParticleXYZRPY& p, double val)
120  {
121  pcl::tracking::ParticleXYZRPY newp;
122  newp.x = static_cast<float> (p.x * val);
123  newp.y = static_cast<float> (p.y * val);
124  newp.z = static_cast<float> (p.z * val);
125  newp.roll = static_cast<float> (p.roll * val);
126  newp.pitch = static_cast<float> (p.pitch * val);
127  newp.yaw = static_cast<float> (p.yaw * val);
128  return (newp);
129  }
130 
131  // a + b
132  inline pcl::tracking::ParticleXYZRPY operator + (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
133  {
134  pcl::tracking::ParticleXYZRPY newp;
135  newp.x = a.x + b.x;
136  newp.y = a.y + b.y;
137  newp.z = a.z + b.z;
138  newp.roll = a.roll + b.roll;
139  newp.pitch = a.pitch + b.pitch;
140  newp.yaw = a.yaw + b.yaw;
141  return (newp);
142  }
143 
144  // a - b
145  inline pcl::tracking::ParticleXYZRPY operator - (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
146  {
147  pcl::tracking::ParticleXYZRPY newp;
148  newp.x = a.x - b.x;
149  newp.y = a.y - b.y;
150  newp.z = a.z - b.z;
151  newp.roll = a.roll - b.roll;
152  newp.pitch = a.pitch - b.pitch;
153  newp.yaw = a.yaw - b.yaw;
154  return (newp);
155  }
156 
157  }
158 }
159 
160 
161 //########################################################################33
162 
163 
164 namespace pcl
165 {
166  namespace tracking
167  {
168  struct _ParticleXYZR
169  {
170  PCL_ADD_POINT4D;
171  union
172  {
173  struct
174  {
175  float roll;
176  float pitch;
177  float yaw;
178  float weight;
179  };
180  float data_c[4];
181  };
182  };
183 
184  // particle definition
185  struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR
186  {
187  inline ParticleXYZR ()
188  {
189  x = y = z = roll = pitch = yaw = 0.0;
190  data[3] = 1.0f;
191  }
192 
193  inline ParticleXYZR (float _x, float _y, float _z)
194  {
195  x = _x; y = _y; z = _z;
196  roll = pitch = yaw = 0.0;
197  data[3] = 1.0f;
198  }
199 
200  inline ParticleXYZR (float _x, float _y, float _z, float, float _pitch, float)
201  {
202  x = _x; y = _y; z = _z;
203  roll = 0; pitch = _pitch; yaw = 0;
204  data[3] = 1.0f;
205  }
206 
207  inline static int
208  stateDimension () { return 6; }
209 
210  void
211  sample (const std::vector<double>& mean, const std::vector<double>& cov)
212  {
213  x += static_cast<float> (sampleNormal (mean[0], cov[0]));
214  y += static_cast<float> (sampleNormal (mean[1], cov[1]));
215  z += static_cast<float> (sampleNormal (mean[2], cov[2]));
216  roll = 0;
217  pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
218  yaw = 0;
219  }
220 
221  void
222  zero ()
223  {
224  x = 0.0;
225  y = 0.0;
226  z = 0.0;
227  roll = 0.0;
228  pitch = 0.0;
229  yaw = 0.0;
230  }
231 
232  inline Eigen::Affine3f
233  toEigenMatrix () const
234  {
235  return getTransformation(x, y, z, roll, pitch, yaw);
236  }
237 
238  static pcl::tracking::ParticleXYZR
239  toState (const Eigen::Affine3f &trans)
240  {
241  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
242  getTranslationAndEulerAngles (trans,
243  trans_x, trans_y, trans_z,
244  trans_roll, trans_pitch, trans_yaw);
245  return (pcl::tracking::ParticleXYZR (trans_x, trans_y, trans_z, 0, trans_pitch, 0));
246  }
247 
248  // a[i]
249  inline float operator [] (unsigned int i)
250  {
251  switch (i)
252  {
253  case 0: return x;
254  case 1: return y;
255  case 2: return z;
256  case 3: return roll;
257  case 4: return pitch;
258  case 5: return yaw;
259  default: return 0.0;
260  }
261  }
262 
263  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
264  };
265 
266  inline std::ostream& operator << (std::ostream& os, const ParticleXYZR& p)
267  {
268  os << "(" << p.x << "," << p.y << "," << p.z << ","
269  << p.roll << "," << p.pitch << "," << p.yaw << ")";
270  return (os);
271  }
272 
273  // a * k
274  inline pcl::tracking::ParticleXYZR operator * (const ParticleXYZR& p, double val)
275  {
276  pcl::tracking::ParticleXYZR newp;
277  newp.x = static_cast<float> (p.x * val);
278  newp.y = static_cast<float> (p.y * val);
279  newp.z = static_cast<float> (p.z * val);
280  newp.roll = static_cast<float> (p.roll * val);
281  newp.pitch = static_cast<float> (p.pitch * val);
282  newp.yaw = static_cast<float> (p.yaw * val);
283  return (newp);
284  }
285 
286  // a + b
287  inline pcl::tracking::ParticleXYZR operator + (const ParticleXYZR& a, const ParticleXYZR& b)
288  {
289  pcl::tracking::ParticleXYZR newp;
290  newp.x = a.x + b.x;
291  newp.y = a.y + b.y;
292  newp.z = a.z + b.z;
293  newp.roll = 0;
294  newp.pitch = a.pitch + b.pitch;
295  newp.yaw = 0.0;
296  return (newp);
297  }
298 
299  // a - b
300  inline pcl::tracking::ParticleXYZR operator - (const ParticleXYZR& a, const ParticleXYZR& b)
301  {
302  pcl::tracking::ParticleXYZR newp;
303  newp.x = a.x - b.x;
304  newp.y = a.y - b.y;
305  newp.z = a.z - b.z;
306  newp.roll = 0.0;
307  newp.pitch = a.pitch - b.pitch;
308  newp.yaw = 0.0;
309  return (newp);
310  }
311 
312  }
313 }
314 
315 //########################################################################33
316 
317 
318 
319 namespace pcl
320 {
321  namespace tracking
322  {
323  struct _ParticleXYRPY
324  {
325  PCL_ADD_POINT4D;
326  union
327  {
328  struct
329  {
330  float roll;
331  float pitch;
332  float yaw;
333  float weight;
334  };
335  float data_c[4];
336  };
337  };
338 
339  // particle definition
340  struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY
341  {
342  inline ParticleXYRPY ()
343  {
344  x = y = z = roll = pitch = yaw = 0.0;
345  data[3] = 1.0f;
346  }
347 
348  inline ParticleXYRPY (float _x, float, float _z)
349  {
350  x = _x; y = 0; z = _z;
351  roll = pitch = yaw = 0.0;
352  data[3] = 1.0f;
353  }
354 
355  inline ParticleXYRPY (float _x, float, float _z, float _roll, float _pitch, float _yaw)
356  {
357  x = _x; y = 0; z = _z;
358  roll = _roll; pitch = _pitch; yaw = _yaw;
359  data[3] = 1.0f;
360  }
361 
362  inline static int
363  stateDimension () { return 6; }
364 
365  void
366  sample (const std::vector<double>& mean, const std::vector<double>& cov)
367  {
368  x += static_cast<float> (sampleNormal (mean[0], cov[0]));
369  y = 0;
370  z += static_cast<float> (sampleNormal (mean[2], cov[2]));
371  roll += static_cast<float> (sampleNormal (mean[3], cov[3]));
372  pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
373  yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
374  }
375 
376  void
377  zero ()
378  {
379  x = 0.0;
380  y = 0.0;
381  z = 0.0;
382  roll = 0.0;
383  pitch = 0.0;
384  yaw = 0.0;
385  }
386 
387  inline Eigen::Affine3f
388  toEigenMatrix () const
389  {
390  return getTransformation(x, y, z, roll, pitch, yaw);
391  }
392 
393  static pcl::tracking::ParticleXYRPY
394  toState (const Eigen::Affine3f &trans)
395  {
396  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
397  getTranslationAndEulerAngles (trans,
398  trans_x, trans_y, trans_z,
399  trans_roll, trans_pitch, trans_yaw);
400  return (pcl::tracking::ParticleXYRPY (trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
401  }
402 
403  // a[i]
404  inline float operator [] (unsigned int i)
405  {
406  switch (i)
407  {
408  case 0: return x;
409  case 1: return y;
410  case 2: return z;
411  case 3: return roll;
412  case 4: return pitch;
413  case 5: return yaw;
414  default: return 0.0;
415  }
416  }
417 
418  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
419  };
420 
421  inline std::ostream& operator << (std::ostream& os, const ParticleXYRPY& p)
422  {
423  os << "(" << p.x << "," << p.y << "," << p.z << ","
424  << p.roll << "," << p.pitch << "," << p.yaw << ")";
425  return (os);
426  }
427 
428  // a * k
429  inline pcl::tracking::ParticleXYRPY operator * (const ParticleXYRPY& p, double val)
430  {
431  pcl::tracking::ParticleXYRPY newp;
432  newp.x = static_cast<float> (p.x * val);
433  newp.y = static_cast<float> (p.y * val);
434  newp.z = static_cast<float> (p.z * val);
435  newp.roll = static_cast<float> (p.roll * val);
436  newp.pitch = static_cast<float> (p.pitch * val);
437  newp.yaw = static_cast<float> (p.yaw * val);
438  return (newp);
439  }
440 
441  // a + b
442  inline pcl::tracking::ParticleXYRPY operator + (const ParticleXYRPY& a, const ParticleXYRPY& b)
443  {
444  pcl::tracking::ParticleXYRPY newp;
445  newp.x = a.x + b.x;
446  newp.y = 0;
447  newp.z = a.z + b.z;
448  newp.roll = a.roll + b.roll;
449  newp.pitch = a.pitch + b.pitch;
450  newp.yaw = a.yaw + b.yaw;
451  return (newp);
452  }
453 
454  // a - b
455  inline pcl::tracking::ParticleXYRPY operator - (const ParticleXYRPY& a, const ParticleXYRPY& b)
456  {
457  pcl::tracking::ParticleXYRPY newp;
458  newp.x = a.x - b.x;
459  newp.z = a.z - b.z;
460  newp.y = 0;
461  newp.roll = a.roll - b.roll;
462  newp.pitch = a.pitch - b.pitch;
463  newp.yaw = a.yaw - b.yaw;
464  return (newp);
465  }
466 
467  }
468 }
469 
470 //########################################################################33
471 
472 namespace pcl
473 {
474  namespace tracking
475  {
476  struct _ParticleXYRP
477  {
478  PCL_ADD_POINT4D;
479  union
480  {
481  struct
482  {
483  float roll;
484  float pitch;
485  float yaw;
486  float weight;
487  };
488  float data_c[4];
489  };
490  };
491 
492  // particle definition
493  struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP
494  {
495  inline ParticleXYRP ()
496  {
497  x = y = z = roll = pitch = yaw = 0.0;
498  data[3] = 1.0f;
499  }
500 
501  inline ParticleXYRP (float _x, float, float _z)
502  {
503  x = _x; y = 0; z = _z;
504  roll = pitch = yaw = 0.0;
505  data[3] = 1.0f;
506  }
507 
508  inline ParticleXYRP (float _x, float, float _z, float, float _pitch, float _yaw)
509  {
510  x = _x; y = 0; z = _z;
511  roll = 0; pitch = _pitch; yaw = _yaw;
512  data[3] = 1.0f;
513  }
514 
515  inline static int
516  stateDimension () { return 6; }
517 
518  void
519  sample (const std::vector<double>& mean, const std::vector<double>& cov)
520  {
521  x += static_cast<float> (sampleNormal (mean[0], cov[0]));
522  y = 0;
523  z += static_cast<float> (sampleNormal (mean[2], cov[2]));
524  roll = 0;
525  pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
526  yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
527  }
528 
529  void
530  zero ()
531  {
532  x = 0.0;
533  y = 0.0;
534  z = 0.0;
535  roll = 0.0;
536  pitch = 0.0;
537  yaw = 0.0;
538  }
539 
540  inline Eigen::Affine3f
541  toEigenMatrix () const
542  {
543  return getTransformation(x, y, z, roll, pitch, yaw);
544  }
545 
546  static pcl::tracking::ParticleXYRP
547  toState (const Eigen::Affine3f &trans)
548  {
549  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
550  getTranslationAndEulerAngles (trans,
551  trans_x, trans_y, trans_z,
552  trans_roll, trans_pitch, trans_yaw);
553  return (pcl::tracking::ParticleXYRP (trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
554  }
555 
556  // a[i]
557  inline float operator [] (unsigned int i)
558  {
559  switch (i)
560  {
561  case 0: return x;
562  case 1: return y;
563  case 2: return z;
564  case 3: return roll;
565  case 4: return pitch;
566  case 5: return yaw;
567  default: return 0.0;
568  }
569  }
570 
571  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
572  };
573 
574  inline std::ostream& operator << (std::ostream& os, const ParticleXYRP& p)
575  {
576  os << "(" << p.x << "," << p.y << "," << p.z << ","
577  << p.roll << "," << p.pitch << "," << p.yaw << ")";
578  return (os);
579  }
580 
581  // a * k
582  inline pcl::tracking::ParticleXYRP operator * (const ParticleXYRP& p, double val)
583  {
584  pcl::tracking::ParticleXYRP newp;
585  newp.x = static_cast<float> (p.x * val);
586  newp.y = static_cast<float> (p.y * val);
587  newp.z = static_cast<float> (p.z * val);
588  newp.roll = static_cast<float> (p.roll * val);
589  newp.pitch = static_cast<float> (p.pitch * val);
590  newp.yaw = static_cast<float> (p.yaw * val);
591  return (newp);
592  }
593 
594  // a + b
595  inline pcl::tracking::ParticleXYRP operator + (const ParticleXYRP& a, const ParticleXYRP& b)
596  {
597  pcl::tracking::ParticleXYRP newp;
598  newp.x = a.x + b.x;
599  newp.y = 0;
600  newp.z = a.z + b.z;
601  newp.roll = 0;
602  newp.pitch = a.pitch + b.pitch;
603  newp.yaw = a.yaw + b.yaw;
604  return (newp);
605  }
606 
607  // a - b
608  inline pcl::tracking::ParticleXYRP operator - (const ParticleXYRP& a, const ParticleXYRP& b)
609  {
610  pcl::tracking::ParticleXYRP newp;
611  newp.x = a.x - b.x;
612  newp.z = a.z - b.z;
613  newp.y = 0;
614  newp.roll = 0.0;
615  newp.pitch = a.pitch - b.pitch;
616  newp.yaw = a.yaw - b.yaw;
617  return (newp);
618  }
619 
620  }
621 }
622 
623 //########################################################################33
624 
625 namespace pcl
626 {
627  namespace tracking
628  {
629  struct _ParticleXYR
630  {
631  PCL_ADD_POINT4D;
632  union
633  {
634  struct
635  {
636  float roll;
637  float pitch;
638  float yaw;
639  float weight;
640  };
641  float data_c[4];
642  };
643  };
644 
645  // particle definition
646  struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR
647  {
648  inline ParticleXYR ()
649  {
650  x = y = z = roll = pitch = yaw = 0.0;
651  data[3] = 1.0f;
652  }
653 
654  inline ParticleXYR (float _x, float, float _z)
655  {
656  x = _x; y = 0; z = _z;
657  roll = pitch = yaw = 0.0;
658  data[3] = 1.0f;
659  }
660 
661  inline ParticleXYR (float _x, float, float _z, float, float _pitch, float)
662  {
663  x = _x; y = 0; z = _z;
664  roll = 0; pitch = _pitch; yaw = 0;
665  data[3] = 1.0f;
666  }
667 
668  inline static int
669  stateDimension () { return 6; }
670 
671  void
672  sample (const std::vector<double>& mean, const std::vector<double>& cov)
673  {
674  x += static_cast<float> (sampleNormal (mean[0], cov[0]));
675  y = 0;
676  z += static_cast<float> (sampleNormal (mean[2], cov[2]));
677  roll = 0;
678  pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
679  yaw = 0;
680  }
681 
682  void
683  zero ()
684  {
685  x = 0.0;
686  y = 0.0;
687  z = 0.0;
688  roll = 0.0;
689  pitch = 0.0;
690  yaw = 0.0;
691  }
692 
693  inline Eigen::Affine3f
694  toEigenMatrix () const
695  {
696  return getTransformation(x, y, z, roll, pitch, yaw);
697  }
698 
699  static pcl::tracking::ParticleXYR
700  toState (const Eigen::Affine3f &trans)
701  {
702  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
703  getTranslationAndEulerAngles (trans,
704  trans_x, trans_y, trans_z,
705  trans_roll, trans_pitch, trans_yaw);
706  return (pcl::tracking::ParticleXYR (trans_x, 0, trans_z, 0, trans_pitch, 0));
707  }
708 
709  // a[i]
710  inline float operator [] (unsigned int i)
711  {
712  switch (i)
713  {
714  case 0: return x;
715  case 1: return y;
716  case 2: return z;
717  case 3: return roll;
718  case 4: return pitch;
719  case 5: return yaw;
720  default: return 0.0;
721  }
722  }
723 
724  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
725  };
726 
727  inline std::ostream& operator << (std::ostream& os, const ParticleXYR& p)
728  {
729  os << "(" << p.x << "," << p.y << "," << p.z << ","
730  << p.roll << "," << p.pitch << "," << p.yaw << ")";
731  return (os);
732  }
733 
734  // a * k
735  inline pcl::tracking::ParticleXYR operator * (const ParticleXYR& p, double val)
736  {
737  pcl::tracking::ParticleXYR newp;
738  newp.x = static_cast<float> (p.x * val);
739  newp.y = static_cast<float> (p.y * val);
740  newp.z = static_cast<float> (p.z * val);
741  newp.roll = static_cast<float> (p.roll * val);
742  newp.pitch = static_cast<float> (p.pitch * val);
743  newp.yaw = static_cast<float> (p.yaw * val);
744  return (newp);
745  }
746 
747  // a + b
748  inline pcl::tracking::ParticleXYR operator + (const ParticleXYR& a, const ParticleXYR& b)
749  {
750  pcl::tracking::ParticleXYR newp;
751  newp.x = a.x + b.x;
752  newp.y = 0;
753  newp.z = a.z + b.z;
754  newp.roll = 0;
755  newp.pitch = a.pitch + b.pitch;
756  newp.yaw = 0.0;
757  return (newp);
758  }
759 
760  // a - b
761  inline pcl::tracking::ParticleXYR operator - (const ParticleXYR& a, const ParticleXYR& b)
762  {
763  pcl::tracking::ParticleXYR newp;
764  newp.x = a.x - b.x;
765  newp.z = a.z - b.z;
766  newp.y = 0;
767  newp.roll = 0.0;
768  newp.pitch = a.pitch - b.pitch;
769  newp.yaw = 0.0;
770  return (newp);
771  }
772 
773  }
774 }
775 
776 #define PCL_STATE_POINT_TYPES \
777  (pcl::tracking::ParticleXYR) \
778  (pcl::tracking::ParticleXYZRPY) \
779  (pcl::tracking::ParticleXYZR) \
780  (pcl::tracking::ParticleXYRPY) \
781  (pcl::tracking::ParticleXYRP)
782 
783 #endif //