Point Cloud Library (PCL) 1.13.1
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 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 // 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 {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);
461 return {trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw};
462 }
463
464 // a[i]
465 inline float
466 operator[](unsigned int i)
467 {
468 switch (i) {
469 case 0:
470 return x;
471 case 1:
472 return y;
473 case 2:
474 return z;
475 case 3:
476 return roll;
477 case 4:
478 return pitch;
479 case 5:
480 return yaw;
481 default:
482 return 0.0;
483 }
484 }
485
487};
488
489inline std::ostream&
490operator<<(std::ostream& os, const ParticleXYRPY& p)
491{
492 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
493 << p.yaw << ")";
494 return (os);
495}
496
497// a * k
498inline ParticleXYRPY
499operator*(const ParticleXYRPY& p, double val)
500{
502 newp.x = static_cast<float>(p.x * val);
503 newp.y = static_cast<float>(p.y * val);
504 newp.z = static_cast<float>(p.z * val);
505 newp.roll = static_cast<float>(p.roll * val);
506 newp.pitch = static_cast<float>(p.pitch * val);
507 newp.yaw = static_cast<float>(p.yaw * val);
508 return (newp);
509}
510
511// a + b
512inline ParticleXYRPY
514{
516 newp.x = a.x + b.x;
517 newp.y = 0;
518 newp.z = a.z + b.z;
519 newp.roll = a.roll + b.roll;
520 newp.pitch = a.pitch + b.pitch;
521 newp.yaw = a.yaw + b.yaw;
522 return (newp);
523}
524
525// a - b
526inline ParticleXYRPY
528{
530 newp.x = a.x - b.x;
531 newp.z = a.z - b.z;
532 newp.y = 0;
533 newp.roll = a.roll - b.roll;
534 newp.pitch = a.pitch - b.pitch;
535 newp.yaw = a.yaw - b.yaw;
536 return (newp);
537}
538
539} // namespace tracking
540} // namespace pcl
541
542//########################################################################33
543
544namespace pcl {
545namespace tracking {
548 union {
549 struct {
550 float roll;
551 float pitch;
552 float yaw;
553 float weight;
554 };
555 float data_c[4];
556 };
557};
558
559// particle definition
560struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP {
562 {
563 x = y = z = roll = pitch = yaw = 0.0;
564 data[3] = 1.0f;
565 }
566
567 inline ParticleXYRP(float _x, float, float _z)
568 {
569 x = _x;
570 y = 0;
571 z = _z;
572 roll = pitch = yaw = 0.0;
573 data[3] = 1.0f;
574 }
575
576 inline ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
577 {
578 x = _x;
579 y = 0;
580 z = _z;
581 roll = 0;
582 pitch = _pitch;
583 yaw = _yaw;
584 data[3] = 1.0f;
585 }
586
587 inline static int
589 {
590 return 6;
591 }
592
593 void
594 sample(const std::vector<double>& mean, const std::vector<double>& cov)
595 {
596 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
597 y = 0;
598 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
599 roll = 0;
600 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
601 yaw += static_cast<float>(sampleNormal(mean[5], cov[5]));
602 }
603
604 void
606 {
607 x = 0.0;
608 y = 0.0;
609 z = 0.0;
610 roll = 0.0;
611 pitch = 0.0;
612 yaw = 0.0;
613 }
614
615 inline Eigen::Affine3f
617 {
618 return getTransformation(x, y, z, roll, pitch, yaw);
619 }
620
621 static ParticleXYRP
622 toState(const Eigen::Affine3f& trans)
623 {
624 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
626 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
627 return {trans_x, 0, trans_z, 0, trans_pitch, trans_yaw};
628 }
629
630 // a[i]
631 inline float
632 operator[](unsigned int i)
633 {
634 switch (i) {
635 case 0:
636 return x;
637 case 1:
638 return y;
639 case 2:
640 return z;
641 case 3:
642 return roll;
643 case 4:
644 return pitch;
645 case 5:
646 return yaw;
647 default:
648 return 0.0;
649 }
650 }
651
653};
654
655inline std::ostream&
656operator<<(std::ostream& os, const ParticleXYRP& p)
657{
658 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
659 << p.yaw << ")";
660 return (os);
661}
662
663// a * k
664inline ParticleXYRP
665operator*(const ParticleXYRP& p, double val)
666{
668 newp.x = static_cast<float>(p.x * val);
669 newp.y = static_cast<float>(p.y * val);
670 newp.z = static_cast<float>(p.z * val);
671 newp.roll = static_cast<float>(p.roll * val);
672 newp.pitch = static_cast<float>(p.pitch * val);
673 newp.yaw = static_cast<float>(p.yaw * val);
674 return (newp);
675}
676
677// a + b
678inline ParticleXYRP
680{
682 newp.x = a.x + b.x;
683 newp.y = 0;
684 newp.z = a.z + b.z;
685 newp.roll = 0;
686 newp.pitch = a.pitch + b.pitch;
687 newp.yaw = a.yaw + b.yaw;
688 return (newp);
689}
690
691// a - b
692inline ParticleXYRP
694{
696 newp.x = a.x - b.x;
697 newp.z = a.z - b.z;
698 newp.y = 0;
699 newp.roll = 0.0;
700 newp.pitch = a.pitch - b.pitch;
701 newp.yaw = a.yaw - b.yaw;
702 return (newp);
703}
704
705} // namespace tracking
706} // namespace pcl
707
708//########################################################################33
709
710namespace pcl {
711namespace tracking {
714 union {
715 struct {
716 float roll;
717 float pitch;
718 float yaw;
719 float weight;
720 };
721 float data_c[4];
722 };
723};
724
725// particle definition
726struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR {
727 inline ParticleXYR()
728 {
729 x = y = z = roll = pitch = yaw = 0.0;
730 data[3] = 1.0f;
731 }
732
733 inline ParticleXYR(float _x, float, float _z)
734 {
735 x = _x;
736 y = 0;
737 z = _z;
738 roll = pitch = yaw = 0.0;
739 data[3] = 1.0f;
740 }
741
742 inline ParticleXYR(float _x, float, float _z, float, float _pitch, float)
743 {
744 x = _x;
745 y = 0;
746 z = _z;
747 roll = 0;
748 pitch = _pitch;
749 yaw = 0;
750 data[3] = 1.0f;
751 }
752
753 inline static int
755 {
756 return 6;
757 }
758
759 void
760 sample(const std::vector<double>& mean, const std::vector<double>& cov)
761 {
762 x += static_cast<float>(sampleNormal(mean[0], cov[0]));
763 y = 0;
764 z += static_cast<float>(sampleNormal(mean[2], cov[2]));
765 roll = 0;
766 pitch += static_cast<float>(sampleNormal(mean[4], cov[4]));
767 yaw = 0;
768 }
769
770 void
772 {
773 x = 0.0;
774 y = 0.0;
775 z = 0.0;
776 roll = 0.0;
777 pitch = 0.0;
778 yaw = 0.0;
779 }
780
781 inline Eigen::Affine3f
783 {
784 return getTransformation(x, y, z, roll, pitch, yaw);
785 }
786
787 static ParticleXYR
788 toState(const Eigen::Affine3f& trans)
789 {
790 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
792 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
793 return {trans_x, 0, trans_z, 0, trans_pitch, 0};
794 }
795
796 // a[i]
797 inline float
798 operator[](unsigned int i)
799 {
800 switch (i) {
801 case 0:
802 return x;
803 case 1:
804 return y;
805 case 2:
806 return z;
807 case 3:
808 return roll;
809 case 4:
810 return pitch;
811 case 5:
812 return yaw;
813 default:
814 return 0.0;
815 }
816 }
817
819};
820
821inline std::ostream&
822operator<<(std::ostream& os, const ParticleXYR& p)
823{
824 os << "(" << p.x << "," << p.y << "," << p.z << "," << p.roll << "," << p.pitch << ","
825 << p.yaw << ")";
826 return (os);
827}
828
829// a * k
830inline ParticleXYR
831operator*(const ParticleXYR& p, double val)
832{
834 newp.x = static_cast<float>(p.x * val);
835 newp.y = static_cast<float>(p.y * val);
836 newp.z = static_cast<float>(p.z * val);
837 newp.roll = static_cast<float>(p.roll * val);
838 newp.pitch = static_cast<float>(p.pitch * val);
839 newp.yaw = static_cast<float>(p.yaw * val);
840 return (newp);
841}
842
843// a + b
844inline ParticleXYR
846{
848 newp.x = a.x + b.x;
849 newp.y = 0;
850 newp.z = a.z + b.z;
851 newp.roll = 0;
852 newp.pitch = a.pitch + b.pitch;
853 newp.yaw = 0.0;
854 return (newp);
855}
856
857// a - b
858inline ParticleXYR
860{
862 newp.x = a.x - b.x;
863 newp.z = a.z - b.z;
864 newp.y = 0;
865 newp.roll = 0.0;
866 newp.pitch = a.pitch - b.pitch;
867 newp.yaw = 0.0;
868 return (newp);
869}
870
871} // namespace tracking
872} // namespace pcl
873
874#define PCL_STATE_POINT_TYPES \
875 (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
876 pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
877 pcl::tracking::ParticleXYRP)
878
879#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:593
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:607
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:584
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:782
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
Definition tracking.hpp:742
float operator[](unsigned int i)
Definition tracking.hpp:798
ParticleXYR(float _x, float, float _z)
Definition tracking.hpp:733
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:760
static ParticleXYR toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:788
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:594
float operator[](unsigned int i)
Definition tracking.hpp:632
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:616
ParticleXYRP(float _x, float, float _z)
Definition tracking.hpp:567
static ParticleXYRP toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:622
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Definition tracking.hpp:576
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:466
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