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