Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
9namespace pcl {
10namespace 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
25struct EIGEN_ALIGN16 ParticleXYZRPY : public _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 {
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
181inline std::ostream&
182operator<<(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
190inline ParticleXYZRPY
191operator*(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
204inline 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
218inline 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
236namespace pcl {
237namespace 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
252struct EIGEN_ALIGN16 ParticleXYZR : public _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
365inline std::ostream&
366operator<<(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
374inline ParticleXYZR
375operator*(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
388inline 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
402inline 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
420namespace pcl {
421namespace 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
436struct EIGEN_ALIGN16 ParticleXYRPY : public _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
555inline std::ostream&
556operator<<(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
564inline ParticleXYRPY
565operator*(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
578inline 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
592inline 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
610namespace pcl {
611namespace 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
626struct EIGEN_ALIGN16 ParticleXYRP : public _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
742inline std::ostream&
743operator<<(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
751inline ParticleXYRP
752operator*(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
765inline 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
779inline 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
797namespace pcl {
798namespace tracking {
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
813struct 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
926inline std::ostream&
927operator<<(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
935inline ParticleXYR
936operator*(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
949inline ParticleXYR
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
963inline ParticleXYR
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:86
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
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