Point Cloud Library (PCL)  1.11.1-dev
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 <pcl/tracking/tracking.h>
6 #include <pcl/memory.h>
7 #include <pcl/pcl_macros.h>
8 
9 namespace pcl {
10 namespace tracking {
13  union {
14  struct {
15  float roll;
16  float pitch;
17  float yaw;
18  float weight;
19  };
20  float data_c[4];
21  };
22 };
23 
24 // particle definition
25 struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY {
26  inline ParticleXYZRPY()
27  {
28  x = y = z = roll = pitch = yaw = 0.0;
29  data[3] = 1.0f;
30  }
31 
32  inline ParticleXYZRPY(float _x, float _y, float _z)
33  {
34  x = _x;
35  y = _y;
36  z = _z;
37  roll = pitch = yaw = 0.0;
38  data[3] = 1.0f;
39  }
40 
42  float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
43  {
44  x = _x;
45  y = _y;
46  z = _z;
47  roll = _roll;
48  pitch = _pitch;
49  yaw = _yaw;
50  data[3] = 1.0f;
51  }
52 
53  inline static int
55  {
56  return 6;
57  }
58 
59  void
60  sample(const std::vector<double>& mean, const std::vector<double>& cov)
61  {
62  x += static_cast<float>(sampleNormal(mean[0], cov[0]));
63  y += static_cast<float>(sampleNormal(mean[1], cov[1]));
64  z += static_cast<float>(sampleNormal(mean[2], cov[2]));
65 
66  // The roll, pitch, yaw space is not Euclidean, so if we sample roll,
67  // pitch, and yaw independently, we bias our sampling in a complicated
68  // way that depends on where in the space we are sampling.
69  //
70  // A solution is to always sample around mean = 0 and project our
71  // samples onto the space of rotations, SO(3). We rely on the fact
72  // that we are sampling with small variance, so we do not bias
73  // ourselves too much with this projection. We then rotate our
74  // samples by the user's requested mean. The benefit of this approach
75  // is that our distribution's properties are consistent over the space
76  // of rotations.
77  Eigen::Matrix3f current_rotation;
78  current_rotation = getTransformation(x, y, z, roll, pitch, yaw).rotation();
79  Eigen::Quaternionf q_current_rotation(current_rotation);
80 
81  Eigen::Matrix3f mean_rotation;
82  mean_rotation =
83  getTransformation(mean[0], mean[1], mean[2], mean[3], mean[4], mean[5])
84  .rotation();
85  Eigen::Quaternionf q_mean_rotation(mean_rotation);
86 
87  // Scales 1.0 radians of variance in RPY sampling into equivalent units for
88  // quaternion sampling.
89  const float scale_factor = 0.2862;
90 
91  float a = sampleNormal(0, scale_factor * cov[3]);
92  float b = sampleNormal(0, scale_factor * cov[4]);
93  float c = sampleNormal(0, scale_factor * cov[5]);
94 
95  Eigen::Vector4f vec_sample_mean_0(a, b, c, 1);
96  Eigen::Quaternionf q_sample_mean_0(vec_sample_mean_0);
97  q_sample_mean_0.normalize();
98 
99  Eigen::Quaternionf q_sample_user_mean =
100  q_sample_mean_0 * q_mean_rotation * q_current_rotation;
101 
102  Eigen::Affine3f affine_R(q_sample_user_mean.toRotationMatrix());
103  pcl::getEulerAngles(affine_R, roll, pitch, yaw);
104  }
105 
106  void
108  {
109  x = 0.0;
110  y = 0.0;
111  z = 0.0;
112  roll = 0.0;
113  pitch = 0.0;
114  yaw = 0.0;
115  }
116 
117  inline Eigen::Affine3f
119  {
120  return getTransformation(x, y, z, roll, pitch, yaw);
121  }
122 
123  static ParticleXYZRPY
124  toState(const Eigen::Affine3f& trans)
125  {
126  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
128  trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
129  return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
130  }
131 
132  // a[i]
133  inline float
134  operator[](unsigned int i)
135  {
136  switch (i) {
137  case 0:
138  return x;
139  case 1:
140  return y;
141  case 2:
142  return z;
143  case 3:
144  return roll;
145  case 4:
146  return pitch;
147  case 5:
148  return yaw;
149  default:
150  return 0.0;
151  }
152  }
153 
155 };
156 
157 inline std::ostream&
158 operator<<(std::ostream& os, const ParticleXYZRPY& p)
159 {
160  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
161  << p.yaw << ")";
162  return (os);
163 }
164 
165 // a * k
166 inline ParticleXYZRPY
167 operator*(const ParticleXYZRPY& p, double val)
168 {
170  newp.x = static_cast<float>(p.x * val);
171  newp.y = static_cast<float>(p.y * val);
172  newp.z = static_cast<float>(p.z * val);
173  newp.roll = static_cast<float>(p.roll * val);
174  newp.pitch = static_cast<float>(p.pitch * val);
175  newp.yaw = static_cast<float>(p.yaw * val);
176  return (newp);
177 }
178 
179 // a + b
180 inline ParticleXYZRPY
182 {
184  newp.x = a.x + b.x;
185  newp.y = a.y + b.y;
186  newp.z = a.z + b.z;
187  newp.roll = a.roll + b.roll;
188  newp.pitch = a.pitch + b.pitch;
189  newp.yaw = a.yaw + b.yaw;
190  return (newp);
191 }
192 
193 // a - b
194 inline ParticleXYZRPY
196 {
198  newp.x = a.x - b.x;
199  newp.y = a.y - b.y;
200  newp.z = a.z - b.z;
201  newp.roll = a.roll - b.roll;
202  newp.pitch = a.pitch - b.pitch;
203  newp.yaw = a.yaw - b.yaw;
204  return (newp);
205 }
206 
207 } // namespace tracking
208 } // namespace pcl
209 
210 //########################################################################33
211 
212 namespace pcl {
213 namespace tracking {
216  union {
217  struct {
218  float roll;
219  float pitch;
220  float yaw;
221  float weight;
222  };
223  float data_c[4];
224  };
225 };
226 
227 // particle definition
228 struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR {
229  inline ParticleXYZR()
230  {
231  x = y = z = roll = pitch = yaw = 0.0;
232  data[3] = 1.0f;
233  }
234 
235  inline ParticleXYZR(float _x, float _y, float _z)
236  {
237  x = _x;
238  y = _y;
239  z = _z;
240  roll = pitch = yaw = 0.0;
241  data[3] = 1.0f;
242  }
243 
244  inline ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
245  {
246  x = _x;
247  y = _y;
248  z = _z;
249  roll = 0;
250  pitch = _pitch;
251  yaw = 0;
252  data[3] = 1.0f;
253  }
254 
255  inline static int
257  {
258  return 6;
259  }
260 
261  void
262  sample(const std::vector<double>& mean, const std::vector<double>& cov)
263  {
264  x += static_cast<float>(sampleNormal(mean[0], cov[0]));
265  y += static_cast<float>(sampleNormal(mean[1], cov[1]));
266  z += static_cast<float>(sampleNormal(mean[2], cov[2]));
267  roll = 0;
268  pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
269  yaw = 0;
270  }
271 
272  void
274  {
275  x = 0.0;
276  y = 0.0;
277  z = 0.0;
278  roll = 0.0;
279  pitch = 0.0;
280  yaw = 0.0;
281  }
282 
283  inline Eigen::Affine3f
285  {
286  return getTransformation(x, y, z, roll, pitch, yaw);
287  }
288 
289  static ParticleXYZR
290  toState(const Eigen::Affine3f& trans)
291  {
292  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
294  trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
295  return (pcl::tracking::ParticleXYZR(trans_x, trans_y, trans_z, 0, trans_pitch, 0));
296  }
297 
298  // a[i]
299  inline float
300  operator[](unsigned int i)
301  {
302  switch (i) {
303  case 0:
304  return x;
305  case 1:
306  return y;
307  case 2:
308  return z;
309  case 3:
310  return roll;
311  case 4:
312  return pitch;
313  case 5:
314  return yaw;
315  default:
316  return 0.0;
317  }
318  }
319 
321 };
322 
323 inline std::ostream&
324 operator<<(std::ostream& os, const ParticleXYZR& p)
325 {
326  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
327  << p.yaw << ")";
328  return (os);
329 }
330 
331 // a * k
332 inline ParticleXYZR
333 operator*(const ParticleXYZR& p, double val)
334 {
336  newp.x = static_cast<float>(p.x * val);
337  newp.y = static_cast<float>(p.y * val);
338  newp.z = static_cast<float>(p.z * val);
339  newp.roll = static_cast<float>(p.roll * val);
340  newp.pitch = static_cast<float>(p.pitch * val);
341  newp.yaw = static_cast<float>(p.yaw * val);
342  return (newp);
343 }
344 
345 // a + b
346 inline ParticleXYZR
348 {
350  newp.x = a.x + b.x;
351  newp.y = a.y + b.y;
352  newp.z = a.z + b.z;
353  newp.roll = 0;
354  newp.pitch = a.pitch + b.pitch;
355  newp.yaw = 0.0;
356  return (newp);
357 }
358 
359 // a - b
360 inline ParticleXYZR
362 {
364  newp.x = a.x - b.x;
365  newp.y = a.y - b.y;
366  newp.z = a.z - b.z;
367  newp.roll = 0.0;
368  newp.pitch = a.pitch - b.pitch;
369  newp.yaw = 0.0;
370  return (newp);
371 }
372 
373 } // namespace tracking
374 } // namespace pcl
375 
376 //########################################################################33
377 
378 namespace pcl {
379 namespace tracking {
382  union {
383  struct {
384  float roll;
385  float pitch;
386  float yaw;
387  float weight;
388  };
389  float data_c[4];
390  };
391 };
392 
393 // particle definition
394 struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY {
395  inline ParticleXYRPY()
396  {
397  x = y = z = roll = pitch = yaw = 0.0;
398  data[3] = 1.0f;
399  }
400 
401  inline ParticleXYRPY(float _x, float, float _z)
402  {
403  x = _x;
404  y = 0;
405  z = _z;
406  roll = pitch = yaw = 0.0;
407  data[3] = 1.0f;
408  }
409 
410  inline ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
411  {
412  x = _x;
413  y = 0;
414  z = _z;
415  roll = _roll;
416  pitch = _pitch;
417  yaw = _yaw;
418  data[3] = 1.0f;
419  }
420 
421  inline static int
423  {
424  return 6;
425  }
426 
427  void
428  sample(const std::vector<double>& mean, const std::vector<double>& cov)
429  {
430  x += static_cast<float>(sampleNormal(mean[0], cov[0]));
431  y = 0;
432  z += static_cast<float>(sampleNormal(mean[2], cov[2]));
433  roll += static_cast<float>(sampleNormal(mean[3], cov[3]));
434  pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
435  yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
436  }
437 
438  void
440  {
441  x = 0.0;
442  y = 0.0;
443  z = 0.0;
444  roll = 0.0;
445  pitch = 0.0;
446  yaw = 0.0;
447  }
448 
449  inline Eigen::Affine3f
451  {
452  return getTransformation(x, y, z, roll, pitch, yaw);
453  }
454 
455  static ParticleXYRPY
456  toState(const Eigen::Affine3f& trans)
457  {
458  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
460  trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
462  trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
463  }
464 
465  // a[i]
466  inline float
467  operator[](unsigned int i)
468  {
469  switch (i) {
470  case 0:
471  return x;
472  case 1:
473  return y;
474  case 2:
475  return z;
476  case 3:
477  return roll;
478  case 4:
479  return pitch;
480  case 5:
481  return yaw;
482  default:
483  return 0.0;
484  }
485  }
486 
488 };
489 
490 inline std::ostream&
491 operator<<(std::ostream& os, const ParticleXYRPY& p)
492 {
493  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
494  << p.yaw << ")";
495  return (os);
496 }
497 
498 // a * k
499 inline ParticleXYRPY
500 operator*(const ParticleXYRPY& p, double val)
501 {
503  newp.x = static_cast<float>(p.x * val);
504  newp.y = static_cast<float>(p.y * val);
505  newp.z = static_cast<float>(p.z * val);
506  newp.roll = static_cast<float>(p.roll * val);
507  newp.pitch = static_cast<float>(p.pitch * val);
508  newp.yaw = static_cast<float>(p.yaw * val);
509  return (newp);
510 }
511 
512 // a + b
513 inline ParticleXYRPY
515 {
517  newp.x = a.x + b.x;
518  newp.y = 0;
519  newp.z = a.z + b.z;
520  newp.roll = a.roll + b.roll;
521  newp.pitch = a.pitch + b.pitch;
522  newp.yaw = a.yaw + b.yaw;
523  return (newp);
524 }
525 
526 // a - b
527 inline ParticleXYRPY
529 {
531  newp.x = a.x - b.x;
532  newp.z = a.z - b.z;
533  newp.y = 0;
534  newp.roll = a.roll - b.roll;
535  newp.pitch = a.pitch - b.pitch;
536  newp.yaw = a.yaw - b.yaw;
537  return (newp);
538 }
539 
540 } // namespace tracking
541 } // namespace pcl
542 
543 //########################################################################33
544 
545 namespace pcl {
546 namespace tracking {
549  union {
550  struct {
551  float roll;
552  float pitch;
553  float yaw;
554  float weight;
555  };
556  float data_c[4];
557  };
558 };
559 
560 // particle definition
561 struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP {
562  inline ParticleXYRP()
563  {
564  x = y = z = roll = pitch = yaw = 0.0;
565  data[3] = 1.0f;
566  }
567 
568  inline ParticleXYRP(float _x, float, float _z)
569  {
570  x = _x;
571  y = 0;
572  z = _z;
573  roll = pitch = yaw = 0.0;
574  data[3] = 1.0f;
575  }
576 
577  inline ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
578  {
579  x = _x;
580  y = 0;
581  z = _z;
582  roll = 0;
583  pitch = _pitch;
584  yaw = _yaw;
585  data[3] = 1.0f;
586  }
587 
588  inline static int
590  {
591  return 6;
592  }
593 
594  void
595  sample(const std::vector<double>& mean, const std::vector<double>& cov)
596  {
597  x += static_cast<float>(sampleNormal(mean[0], cov[0]));
598  y = 0;
599  z += static_cast<float>(sampleNormal(mean[2], cov[2]));
600  roll = 0;
601  pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
602  yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
603  }
604 
605  void
607  {
608  x = 0.0;
609  y = 0.0;
610  z = 0.0;
611  roll = 0.0;
612  pitch = 0.0;
613  yaw = 0.0;
614  }
615 
616  inline Eigen::Affine3f
618  {
619  return getTransformation(x, y, z, roll, pitch, yaw);
620  }
621 
622  static ParticleXYRP
623  toState(const Eigen::Affine3f& trans)
624  {
625  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
627  trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
628  return (
629  pcl::tracking::ParticleXYRP(trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
630  }
631 
632  // a[i]
633  inline float
634  operator[](unsigned int i)
635  {
636  switch (i) {
637  case 0:
638  return x;
639  case 1:
640  return y;
641  case 2:
642  return z;
643  case 3:
644  return roll;
645  case 4:
646  return pitch;
647  case 5:
648  return yaw;
649  default:
650  return 0.0;
651  }
652  }
653 
655 };
656 
657 inline std::ostream&
658 operator<<(std::ostream& os, const ParticleXYRP& p)
659 {
660  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
661  << p.yaw << ")";
662  return (os);
663 }
664 
665 // a * k
666 inline ParticleXYRP
667 operator*(const ParticleXYRP& p, double val)
668 {
670  newp.x = static_cast<float>(p.x * val);
671  newp.y = static_cast<float>(p.y * val);
672  newp.z = static_cast<float>(p.z * val);
673  newp.roll = static_cast<float>(p.roll * val);
674  newp.pitch = static_cast<float>(p.pitch * val);
675  newp.yaw = static_cast<float>(p.yaw * val);
676  return (newp);
677 }
678 
679 // a + b
680 inline ParticleXYRP
682 {
684  newp.x = a.x + b.x;
685  newp.y = 0;
686  newp.z = a.z + b.z;
687  newp.roll = 0;
688  newp.pitch = a.pitch + b.pitch;
689  newp.yaw = a.yaw + b.yaw;
690  return (newp);
691 }
692 
693 // a - b
694 inline ParticleXYRP
696 {
698  newp.x = a.x - b.x;
699  newp.z = a.z - b.z;
700  newp.y = 0;
701  newp.roll = 0.0;
702  newp.pitch = a.pitch - b.pitch;
703  newp.yaw = a.yaw - b.yaw;
704  return (newp);
705 }
706 
707 } // namespace tracking
708 } // namespace pcl
709 
710 //########################################################################33
711 
712 namespace pcl {
713 namespace tracking {
714 struct _ParticleXYR {
716  union {
717  struct {
718  float roll;
719  float pitch;
720  float yaw;
721  float weight;
722  };
723  float data_c[4];
724  };
725 };
726 
727 // particle definition
728 struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR {
729  inline ParticleXYR()
730  {
731  x = y = z = roll = pitch = yaw = 0.0;
732  data[3] = 1.0f;
733  }
734 
735  inline ParticleXYR(float _x, float, float _z)
736  {
737  x = _x;
738  y = 0;
739  z = _z;
740  roll = pitch = yaw = 0.0;
741  data[3] = 1.0f;
742  }
743 
744  inline ParticleXYR(float _x, float, float _z, float, float _pitch, float)
745  {
746  x = _x;
747  y = 0;
748  z = _z;
749  roll = 0;
750  pitch = _pitch;
751  yaw = 0;
752  data[3] = 1.0f;
753  }
754 
755  inline static int
757  {
758  return 6;
759  }
760 
761  void
762  sample(const std::vector<double>& mean, const std::vector<double>& cov)
763  {
764  x += static_cast<float>(sampleNormal(mean[0], cov[0]));
765  y = 0;
766  z += static_cast<float>(sampleNormal(mean[2], cov[2]));
767  roll = 0;
768  pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
769  yaw = 0;
770  }
771 
772  void
774  {
775  x = 0.0;
776  y = 0.0;
777  z = 0.0;
778  roll = 0.0;
779  pitch = 0.0;
780  yaw = 0.0;
781  }
782 
783  inline Eigen::Affine3f
785  {
786  return getTransformation(x, y, z, roll, pitch, yaw);
787  }
788 
789  static ParticleXYR
790  toState(const Eigen::Affine3f& trans)
791  {
792  float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
794  trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
795  return (pcl::tracking::ParticleXYR(trans_x, 0, trans_z, 0, trans_pitch, 0));
796  }
797 
798  // a[i]
799  inline float
800  operator[](unsigned int i)
801  {
802  switch (i) {
803  case 0:
804  return x;
805  case 1:
806  return y;
807  case 2:
808  return z;
809  case 3:
810  return roll;
811  case 4:
812  return pitch;
813  case 5:
814  return yaw;
815  default:
816  return 0.0;
817  }
818  }
819 
821 };
822 
823 inline std::ostream&
824 operator<<(std::ostream& os, const ParticleXYR& p)
825 {
826  os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
827  << p.yaw << ")";
828  return (os);
829 }
830 
831 // a * k
832 inline ParticleXYR
833 operator*(const ParticleXYR& p, double val)
834 {
836  newp.x = static_cast<float>(p.x * val);
837  newp.y = static_cast<float>(p.y * val);
838  newp.z = static_cast<float>(p.z * val);
839  newp.roll = static_cast<float>(p.roll * val);
840  newp.pitch = static_cast<float>(p.pitch * val);
841  newp.yaw = static_cast<float>(p.yaw * val);
842  return (newp);
843 }
844 
845 // a + b
846 inline ParticleXYR
847 operator+(const ParticleXYR& a, const ParticleXYR& b)
848 {
850  newp.x = a.x + b.x;
851  newp.y = 0;
852  newp.z = a.z + b.z;
853  newp.roll = 0;
854  newp.pitch = a.pitch + b.pitch;
855  newp.yaw = 0.0;
856  return (newp);
857 }
858 
859 // a - b
860 inline ParticleXYR
861 operator-(const ParticleXYR& a, const ParticleXYR& b)
862 {
864  newp.x = a.x - b.x;
865  newp.z = a.z - b.z;
866  newp.y = 0;
867  newp.roll = 0.0;
868  newp.pitch = a.pitch - b.pitch;
869  newp.yaw = 0.0;
870  return (newp);
871 }
872 
873 } // namespace tracking
874 } // namespace pcl
875 
876 #define PCL_STATE_POINT_TYPES \
877  (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
878  pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
879  pcl::tracking::ParticleXYRP)
880 
881 #endif //
pcl::tracking::ParticleXYRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:450
pcl::tracking::_ParticleXYRPY::yaw
float yaw
Definition: tracking.hpp:386
pcl::tracking::_ParticleXYZRPY::data_c
float data_c[4]
Definition: tracking.hpp:20
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z)
Definition: tracking.hpp:235
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z)
Definition: tracking.hpp:735
pcl::tracking::ParticleXYRP
Definition: tracking.hpp:561
pcl::tracking::_ParticleXYRPY::weight
float weight
Definition: tracking.hpp:387
pcl::tracking::_ParticleXYZR
Definition: tracking.hpp:214
pcl::tracking::ParticleXYZR::zero
void zero()
Definition: tracking.hpp:273
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
Definition: tracking.hpp:41
pcl::tracking::_ParticleXYR::yaw
float yaw
Definition: tracking.hpp:720
pcl::tracking::ParticleXYR::toState
static ParticleXYR toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:790
pcl::tracking::ParticleXYR::stateDimension
static int stateDimension()
Definition: tracking.hpp:756
pcl::tracking::_ParticleXYZR::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:215
pcl::tracking::ParticleXYR
Definition: tracking.hpp:728
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP()
Definition: tracking.hpp:562
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY()
Definition: tracking.hpp:26
pcl::tracking::_ParticleXYZRPY::pitch
float pitch
Definition: tracking.hpp:16
pcl::tracking::ParticleXYRPY::zero
void zero()
Definition: tracking.hpp:439
pcl::tracking::_ParticleXYZR::weight
float weight
Definition: tracking.hpp:221
pcl::tracking::_ParticleXYZRPY::yaw
float yaw
Definition: tracking.hpp:17
pcl::tracking::operator<<
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
Definition: tracking.hpp:158
pcl::tracking::_ParticleXYRPY::roll
float roll
Definition: tracking.hpp:384
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY()
Definition: tracking.hpp:395
pcl::getTransformation
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (XYZ-convention)
Definition: eigen.hpp:608
pcl::tracking::ParticleXYZRPY
Definition: tracking.hpp:25
pcl::tracking::ParticleXYRPY::toState
static ParticleXYRPY toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:456
pcl::tracking::operator-
ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition: tracking.hpp:195
pcl::tracking::ParticleXYZRPY::operator[]
float operator[](unsigned int i)
Definition: tracking.hpp:134
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
Definition: tracking.hpp:744
pcl::tracking::_ParticleXYRP::pitch
float pitch
Definition: tracking.hpp:552
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR()
Definition: tracking.hpp:229
pcl::tracking::ParticleXYZR::operator[]
float operator[](unsigned int i)
Definition: tracking.hpp:300
pcl::tracking::_ParticleXYR::weight
float weight
Definition: tracking.hpp:721
pcl::tracking::_ParticleXYR::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:715
pcl::tracking::_ParticleXYZRPY
Definition: tracking.hpp:11
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z)
Definition: tracking.hpp:32
pcl::tracking::_ParticleXYR::roll
float roll
Definition: tracking.hpp:718
pcl::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &x, Scalar &y, Scalar &z, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation.
Definition: eigen.hpp:594
pcl::tracking::ParticleXYZR
Definition: tracking.hpp:228
pcl::tracking::operator+
ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition: tracking.hpp:181
pcl::tracking::ParticleXYRPY
Definition: tracking.hpp:394
pcl::tracking::ParticleXYZRPY::toState
static ParticleXYZRPY toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:124
pcl::tracking::_ParticleXYRPY::data_c
float data_c[4]
Definition: tracking.hpp:389
pcl::tracking::_ParticleXYR
Definition: tracking.hpp:714
pcl::tracking::ParticleXYZRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:118
pcl::tracking::_ParticleXYRPY::pitch
float pitch
Definition: tracking.hpp:385
pcl::tracking::_ParticleXYRP::weight
float weight
Definition: tracking.hpp:554
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR()
Definition: tracking.hpp:729
pcl::tracking::_ParticleXYRP::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:548
pcl::tracking::ParticleXYZR::stateDimension
static int stateDimension()
Definition: tracking.hpp:256
pcl::tracking::ParticleXYR::zero
void zero()
Definition: tracking.hpp:773
pcl::tracking::_ParticleXYZR::pitch
float pitch
Definition: tracking.hpp:219
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::getEulerAngles
void getEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract the Euler angles (XYZ-convention) from the given transformation.
Definition: eigen.hpp:585
pcl::tracking::_ParticleXYZRPY::weight
float weight
Definition: tracking.hpp:18
pcl::tracking::ParticleXYZRPY::stateDimension
static int stateDimension()
Definition: tracking.hpp:54
pcl::tracking::_ParticleXYRP
Definition: tracking.hpp:547
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Definition: tracking.hpp:577
pcl::tracking::_ParticleXYRP::yaw
float yaw
Definition: tracking.hpp:553
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z)
Definition: tracking.hpp:401
pcl::tracking::ParticleXYRP::zero
void zero()
Definition: tracking.hpp:606
pcl::tracking::ParticleXYZRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:60
pcl::tracking::ParticleXYRP::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:595
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
Definition: tracking.hpp:244
pcl::tracking::ParticleXYR::operator[]
float operator[](unsigned int i)
Definition: tracking.hpp:800
pcl::tracking::ParticleXYRP::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:617
pcl::tracking::ParticleXYZRPY::zero
void zero()
Definition: tracking.hpp:107
pcl::tracking::ParticleXYZR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:284
pcl::tracking::ParticleXYR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:784
pcl::tracking::ParticleXYRP::operator[]
float operator[](unsigned int i)
Definition: tracking.hpp:634
pcl::tracking::_ParticleXYR::data_c
float data_c[4]
Definition: tracking.hpp:723
pcl::tracking::ParticleXYRP::stateDimension
static int stateDimension()
Definition: tracking.hpp:589
pcl::tracking::_ParticleXYZR::data_c
float data_c[4]
Definition: tracking.hpp:223
pcl::tracking::ParticleXYRP::toState
static ParticleXYRP toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:623
pcl::tracking::ParticleXYZR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:262
pcl::tracking::ParticleXYR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:762
pcl::tracking::ParticleXYRPY::stateDimension
static int stateDimension()
Definition: tracking.hpp:422
pcl::tracking::ParticleXYRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:428
pcl::tracking::_ParticleXYZRPY::roll
float roll
Definition: tracking.hpp:15
pcl::tracking::_ParticleXYRP::data_c
float data_c[4]
Definition: tracking.hpp:556
pcl::tracking::_ParticleXYRPY::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:381
pcl::tracking::_ParticleXYZR::roll
float roll
Definition: tracking.hpp:218
pcl::tracking::operator*
ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
Definition: tracking.hpp:167
pcl::tracking::_ParticleXYR::pitch
float pitch
Definition: tracking.hpp:719
pcl::tracking::sampleNormal
PCL_EXPORTS double sampleNormal(double mean, double sigma)
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z)
Definition: tracking.hpp:568
pcl::tracking::_ParticleXYZRPY::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:12
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
Definition: tracking.hpp:410
pcl::tracking::_ParticleXYRPY
Definition: tracking.hpp:380
pcl::tracking::ParticleXYRPY::operator[]
float operator[](unsigned int i)
Definition: tracking.hpp:467
pcl::tracking::_ParticleXYRP::roll
float roll
Definition: tracking.hpp:551
pcl::tracking::ParticleXYZR::toState
static ParticleXYZR toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:290
pcl::tracking::_ParticleXYZR::yaw
float yaw
Definition: tracking.hpp:220