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