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