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