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