Point Cloud Library (PCL) 1.12.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 {
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 const 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 // a[i]
133 inline float
134 operator[](unsigned int i)
135 {
136 switch (i) {
137 case 0:
138 return x;
139 case 1:
140 return y;
141 case 2:
142 return z;
143 case 3:
144 return roll;
145 case 4:
146 return pitch;
147 case 5:
148 return yaw;
149 default:
150 return 0.0;
151 }
152 }
153
155};
156
157inline std::ostream&
158operator<<(std::ostream& os, const ParticleXYZRPY& p)
159{
160 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
161 << p.yaw << ")";
162 return (os);
163}
164
165// a * k
166inline ParticleXYZRPY
167operator*(const ParticleXYZRPY& p, double val)
168{
170 newp.x = static_cast<float>(p.x * val);
171 newp.y = static_cast<float>(p.y * val);
172 newp.z = static_cast<float>(p.z * val);
173 newp.roll = static_cast<float>(p.roll * val);
174 newp.pitch = static_cast<float>(p.pitch * val);
175 newp.yaw = static_cast<float>(p.yaw * val);
176 return (newp);
177}
178
179// a + b
180inline ParticleXYZRPY
182{
184 newp.x = a.x + b.x;
185 newp.y = a.y + b.y;
186 newp.z = a.z + b.z;
187 newp.roll = a.roll + b.roll;
188 newp.pitch = a.pitch + b.pitch;
189 newp.yaw = a.yaw + b.yaw;
190 return (newp);
191}
192
193// a - b
194inline ParticleXYZRPY
196{
198 newp.x = a.x - b.x;
199 newp.y = a.y - b.y;
200 newp.z = a.z - b.z;
201 newp.roll = a.roll - b.roll;
202 newp.pitch = a.pitch - b.pitch;
203 newp.yaw = a.yaw - b.yaw;
204 return (newp);
205}
206
207} // namespace tracking
208} // namespace pcl
209
210//########################################################################33
211
212namespace pcl {
213namespace tracking {
216 union {
217 struct {
218 float roll;
219 float pitch;
220 float yaw;
221 float weight;
222 };
223 float data_c[4];
224 };
225};
226
227// particle definition
228struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR {
230 {
231 x = y = z = roll = pitch = yaw = 0.0;
232 data[3] = 1.0f;
233 }
234
235 inline ParticleXYZR(float _x, float _y, float _z)
236 {
237 x = _x;
238 y = _y;
239 z = _z;
240 roll = pitch = yaw = 0.0;
241 data[3] = 1.0f;
242 }
243
244 inline ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
245 {
246 x = _x;
247 y = _y;
248 z = _z;
249 roll = 0;
250 pitch = _pitch;
251 yaw = 0;
252 data[3] = 1.0f;
253 }
254
255 inline static int
257 {
258 return 6;
259 }
260
261 void
262 sample(const std::vector<double>& mean, const std::vector<double>& cov)
263 {
264 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
265 y += static_cast<float>(sampleNormal(mean[1], cov[1]));
266 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
267 roll = 0;
268 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
269 yaw = 0;
270 }
271
272 void
274 {
275 x = 0.0;
276 y = 0.0;
277 z = 0.0;
278 roll = 0.0;
279 pitch = 0.0;
280 yaw = 0.0;
281 }
282
283 inline Eigen::Affine3f
285 {
286 return getTransformation(x, y, z, roll, pitch, yaw);
287 }
288
289 static ParticleXYZR
290 toState(const Eigen::Affine3f& trans)
291 {
292 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
294 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
295 return (pcl::tracking::ParticleXYZR(trans_x, trans_y, trans_z, 0, trans_pitch, 0));
296 }
297
298 // a[i]
299 inline float
300 operator[](unsigned int i)
301 {
302 switch (i) {
303 case 0:
304 return x;
305 case 1:
306 return y;
307 case 2:
308 return z;
309 case 3:
310 return roll;
311 case 4:
312 return pitch;
313 case 5:
314 return yaw;
315 default:
316 return 0.0;
317 }
318 }
319
321};
322
323inline std::ostream&
324operator<<(std::ostream& os, const ParticleXYZR& p)
325{
326 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
327 << p.yaw << ")";
328 return (os);
329}
330
331// a * k
332inline ParticleXYZR
333operator*(const ParticleXYZR& p, double val)
334{
336 newp.x = static_cast<float>(p.x * val);
337 newp.y = static_cast<float>(p.y * val);
338 newp.z = static_cast<float>(p.z * val);
339 newp.roll = static_cast<float>(p.roll * val);
340 newp.pitch = static_cast<float>(p.pitch * val);
341 newp.yaw = static_cast<float>(p.yaw * val);
342 return (newp);
343}
344
345// a + b
346inline ParticleXYZR
348{
350 newp.x = a.x + b.x;
351 newp.y = a.y + b.y;
352 newp.z = a.z + b.z;
353 newp.roll = 0;
354 newp.pitch = a.pitch + b.pitch;
355 newp.yaw = 0.0;
356 return (newp);
357}
358
359// a - b
360inline ParticleXYZR
362{
364 newp.x = a.x - b.x;
365 newp.y = a.y - b.y;
366 newp.z = a.z - b.z;
367 newp.roll = 0.0;
368 newp.pitch = a.pitch - b.pitch;
369 newp.yaw = 0.0;
370 return (newp);
371}
372
373} // namespace tracking
374} // namespace pcl
375
376//########################################################################33
377
378namespace pcl {
379namespace tracking {
382 union {
383 struct {
384 float roll;
385 float pitch;
386 float yaw;
387 float weight;
388 };
389 float data_c[4];
390 };
391};
392
393// particle definition
394struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY {
396 {
397 x = y = z = roll = pitch = yaw = 0.0;
398 data[3] = 1.0f;
399 }
400
401 inline ParticleXYRPY(float _x, float, float _z)
402 {
403 x = _x;
404 y = 0;
405 z = _z;
406 roll = pitch = yaw = 0.0;
407 data[3] = 1.0f;
408 }
409
410 inline ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
411 {
412 x = _x;
413 y = 0;
414 z = _z;
415 roll = _roll;
416 pitch = _pitch;
417 yaw = _yaw;
418 data[3] = 1.0f;
419 }
420
421 inline static int
423 {
424 return 6;
425 }
426
427 void
428 sample(const std::vector<double>& mean, const std::vector<double>& cov)
429 {
430 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
431 y = 0;
432 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
433 roll += static_cast<float>(sampleNormal(mean[3], cov[3]));
434 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
435 yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
436 }
437
438 void
440 {
441 x = 0.0;
442 y = 0.0;
443 z = 0.0;
444 roll = 0.0;
445 pitch = 0.0;
446 yaw = 0.0;
447 }
448
449 inline Eigen::Affine3f
451 {
452 return getTransformation(x, y, z, roll, pitch, yaw);
453 }
454
455 static ParticleXYRPY
456 toState(const Eigen::Affine3f& trans)
457 {
458 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
460 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
462 trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
463 }
464
465 // a[i]
466 inline float
467 operator[](unsigned int i)
468 {
469 switch (i) {
470 case 0:
471 return x;
472 case 1:
473 return y;
474 case 2:
475 return z;
476 case 3:
477 return roll;
478 case 4:
479 return pitch;
480 case 5:
481 return yaw;
482 default:
483 return 0.0;
484 }
485 }
486
488};
489
490inline std::ostream&
491operator<<(std::ostream& os, const ParticleXYRPY& p)
492{
493 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
494 << p.yaw << ")";
495 return (os);
496}
497
498// a * k
499inline ParticleXYRPY
500operator*(const ParticleXYRPY& p, double val)
501{
503 newp.x = static_cast<float>(p.x * val);
504 newp.y = static_cast<float>(p.y * val);
505 newp.z = static_cast<float>(p.z * val);
506 newp.roll = static_cast<float>(p.roll * val);
507 newp.pitch = static_cast<float>(p.pitch * val);
508 newp.yaw = static_cast<float>(p.yaw * val);
509 return (newp);
510}
511
512// a + b
513inline ParticleXYRPY
515{
517 newp.x = a.x + b.x;
518 newp.y = 0;
519 newp.z = a.z + b.z;
520 newp.roll = a.roll + b.roll;
521 newp.pitch = a.pitch + b.pitch;
522 newp.yaw = a.yaw + b.yaw;
523 return (newp);
524}
525
526// a - b
527inline ParticleXYRPY
529{
531 newp.x = a.x - b.x;
532 newp.z = a.z - b.z;
533 newp.y = 0;
534 newp.roll = a.roll - b.roll;
535 newp.pitch = a.pitch - b.pitch;
536 newp.yaw = a.yaw - b.yaw;
537 return (newp);
538}
539
540} // namespace tracking
541} // namespace pcl
542
543//########################################################################33
544
545namespace pcl {
546namespace tracking {
549 union {
550 struct {
551 float roll;
552 float pitch;
553 float yaw;
554 float weight;
555 };
556 float data_c[4];
557 };
558};
559
560// particle definition
561struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP {
563 {
564 x = y = z = roll = pitch = yaw = 0.0;
565 data[3] = 1.0f;
566 }
567
568 inline ParticleXYRP(float _x, float, float _z)
569 {
570 x = _x;
571 y = 0;
572 z = _z;
573 roll = pitch = yaw = 0.0;
574 data[3] = 1.0f;
575 }
576
577 inline ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
578 {
579 x = _x;
580 y = 0;
581 z = _z;
582 roll = 0;
583 pitch = _pitch;
584 yaw = _yaw;
585 data[3] = 1.0f;
586 }
587
588 inline static int
590 {
591 return 6;
592 }
593
594 void
595 sample(const std::vector<double>& mean, const std::vector<double>& cov)
596 {
597 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
598 y = 0;
599 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
600 roll = 0;
601 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
602 yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
603 }
604
605 void
607 {
608 x = 0.0;
609 y = 0.0;
610 z = 0.0;
611 roll = 0.0;
612 pitch = 0.0;
613 yaw = 0.0;
614 }
615
616 inline Eigen::Affine3f
618 {
619 return getTransformation(x, y, z, roll, pitch, yaw);
620 }
621
622 static ParticleXYRP
623 toState(const Eigen::Affine3f& trans)
624 {
625 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
627 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
628 return (
629 pcl::tracking::ParticleXYRP(trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
630 }
631
632 // a[i]
633 inline float
634 operator[](unsigned int i)
635 {
636 switch (i) {
637 case 0:
638 return x;
639 case 1:
640 return y;
641 case 2:
642 return z;
643 case 3:
644 return roll;
645 case 4:
646 return pitch;
647 case 5:
648 return yaw;
649 default:
650 return 0.0;
651 }
652 }
653
655};
656
657inline std::ostream&
658operator<<(std::ostream& os, const ParticleXYRP& p)
659{
660 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
661 << p.yaw << ")";
662 return (os);
663}
664
665// a * k
666inline ParticleXYRP
667operator*(const ParticleXYRP& p, double val)
668{
670 newp.x = static_cast<float>(p.x * val);
671 newp.y = static_cast<float>(p.y * val);
672 newp.z = static_cast<float>(p.z * val);
673 newp.roll = static_cast<float>(p.roll * val);
674 newp.pitch = static_cast<float>(p.pitch * val);
675 newp.yaw = static_cast<float>(p.yaw * val);
676 return (newp);
677}
678
679// a + b
680inline ParticleXYRP
682{
684 newp.x = a.x + b.x;
685 newp.y = 0;
686 newp.z = a.z + b.z;
687 newp.roll = 0;
688 newp.pitch = a.pitch + b.pitch;
689 newp.yaw = a.yaw + b.yaw;
690 return (newp);
691}
692
693// a - b
694inline ParticleXYRP
696{
698 newp.x = a.x - b.x;
699 newp.z = a.z - b.z;
700 newp.y = 0;
701 newp.roll = 0.0;
702 newp.pitch = a.pitch - b.pitch;
703 newp.yaw = a.yaw - b.yaw;
704 return (newp);
705}
706
707} // namespace tracking
708} // namespace pcl
709
710//########################################################################33
711
712namespace pcl {
713namespace tracking {
716 union {
717 struct {
718 float roll;
719 float pitch;
720 float yaw;
721 float weight;
722 };
723 float data_c[4];
724 };
725};
726
727// particle definition
728struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR {
729 inline ParticleXYR()
730 {
731 x = y = z = roll = pitch = yaw = 0.0;
732 data[3] = 1.0f;
733 }
734
735 inline ParticleXYR(float _x, float, float _z)
736 {
737 x = _x;
738 y = 0;
739 z = _z;
740 roll = pitch = yaw = 0.0;
741 data[3] = 1.0f;
742 }
743
744 inline ParticleXYR(float _x, float, float _z, float, float _pitch, float)
745 {
746 x = _x;
747 y = 0;
748 z = _z;
749 roll = 0;
750 pitch = _pitch;
751 yaw = 0;
752 data[3] = 1.0f;
753 }
754
755 inline static int
757 {
758 return 6;
759 }
760
761 void
762 sample(const std::vector<double>& mean, const std::vector<double>& cov)
763 {
764 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
765 y = 0;
766 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
767 roll = 0;
768 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
769 yaw = 0;
770 }
771
772 void
774 {
775 x = 0.0;
776 y = 0.0;
777 z = 0.0;
778 roll = 0.0;
779 pitch = 0.0;
780 yaw = 0.0;
781 }
782
783 inline Eigen::Affine3f
785 {
786 return getTransformation(x, y, z, roll, pitch, yaw);
787 }
788
789 static ParticleXYR
790 toState(const Eigen::Affine3f& trans)
791 {
792 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
794 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
795 return (pcl::tracking::ParticleXYR(trans_x, 0, trans_z, 0, trans_pitch, 0));
796 }
797
798 // a[i]
799 inline float
800 operator[](unsigned int i)
801 {
802 switch (i) {
803 case 0:
804 return x;
805 case 1:
806 return y;
807 case 2:
808 return z;
809 case 3:
810 return roll;
811 case 4:
812 return pitch;
813 case 5:
814 return yaw;
815 default:
816 return 0.0;
817 }
818 }
819
821};
822
823inline std::ostream&
824operator<<(std::ostream& os, const ParticleXYR& p)
825{
826 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
827 << p.yaw << ")";
828 return (os);
829}
830
831// a * k
832inline ParticleXYR
833operator*(const ParticleXYR& p, double val)
834{
836 newp.x = static_cast<float>(p.x * val);
837 newp.y = static_cast<float>(p.y * val);
838 newp.z = static_cast<float>(p.z * val);
839 newp.roll = static_cast<float>(p.roll * val);
840 newp.pitch = static_cast<float>(p.pitch * val);
841 newp.yaw = static_cast<float>(p.yaw * val);
842 return (newp);
843}
844
845// a + b
846inline ParticleXYR
848{
850 newp.x = a.x + b.x;
851 newp.y = 0;
852 newp.z = a.z + b.z;
853 newp.roll = 0;
854 newp.pitch = a.pitch + b.pitch;
855 newp.yaw = 0.0;
856 return (newp);
857}
858
859// a - b
860inline ParticleXYR
862{
864 newp.x = a.x - b.x;
865 newp.z = a.z - b.z;
866 newp.y = 0;
867 newp.roll = 0.0;
868 newp.pitch = a.pitch - b.pitch;
869 newp.yaw = 0.0;
870 return (newp);
871}
872
873} // namespace tracking
874} // namespace pcl
875
876#define PCL_STATE_POINT_TYPES \
877 (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
878 pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
879 pcl::tracking::ParticleXYRP)
880
881#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 (XYZ-convention) from the given transformation.
Definition eigen.hpp:594
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:608
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:585
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:167
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
Definition tracking.hpp:158
ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition tracking.hpp:195
ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition tracking.hpp:181
Defines all the PCL and non-PCL macros used.
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:784
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
Definition tracking.hpp:744
float operator[](unsigned int i)
Definition tracking.hpp:800
ParticleXYR(float _x, float, float _z)
Definition tracking.hpp:735
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:762
static ParticleXYR toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:790
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:595
float operator[](unsigned int i)
Definition tracking.hpp:634
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:617
ParticleXYRP(float _x, float, float _z)
Definition tracking.hpp:568
static ParticleXYRP toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:623
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Definition tracking.hpp:577
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:428
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:450
ParticleXYRPY(float _x, float, float _z)
Definition tracking.hpp:401
static ParticleXYRPY toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:456
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
Definition tracking.hpp:410
float operator[](unsigned int i)
Definition tracking.hpp:467
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:284
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
Definition tracking.hpp:244
static ParticleXYZR toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:290
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:262
ParticleXYZR(float _x, float _y, float _z)
Definition tracking.hpp:235
float operator[](unsigned int i)
Definition tracking.hpp:300
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
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:134
ParticleXYZRPY(float _x, float _y, float _z)
Definition tracking.hpp:32