1 #ifndef PCL_TRACKING_IMPL_TRACKING_H_
2 #define PCL_TRACKING_IMPL_TRACKING_H_
4 #include <pcl/common/eigen.h>
6 #include <pcl/tracking/boost.h>
7 #include <pcl/tracking/tracking.h>
34 x = y = z = roll = pitch = yaw = 0.0;
40 x = _x; y = _y; z = _z;
41 roll = pitch = yaw = 0.0;
45 inline ParticleXYZRPY (
float _x,
float _y,
float _z,
float _roll,
float _pitch,
float _yaw)
47 x = _x; y = _y; z = _z;
48 roll = _roll; pitch = _pitch; yaw = _yaw;
56 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
58 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
59 y +=
static_cast<float> (
sampleNormal (mean[1], cov[1]));
60 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
73 Eigen::Matrix3f current_rotation;
75 Eigen::Quaternionf q_current_rotation (current_rotation);
77 Eigen::Matrix3f mean_rotation;
79 mean[3], mean[4], mean[5]).rotation ();
80 Eigen::Quaternionf q_mean_rotation (mean_rotation);
83 const float scale_factor = 0.2862;
89 Eigen::Vector4f vec_sample_mean_0 (a, b, c, 1);
90 Eigen::Quaternionf q_sample_mean_0 (vec_sample_mean_0);
91 q_sample_mean_0.normalize ();
93 Eigen::Quaternionf q_sample_user_mean = q_sample_mean_0 * q_mean_rotation * q_current_rotation;
95 Eigen::Affine3f affine_R (q_sample_user_mean.toRotationMatrix ());
110 inline Eigen::Affine3f
119 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
121 trans_x, trans_y, trans_z,
122 trans_roll, trans_pitch, trans_yaw);
127 inline float operator [] (
unsigned int i)
135 case 4:
return pitch;
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
155 newp.x =
static_cast<float> (p.x * val);
156 newp.y =
static_cast<float> (p.y * val);
157 newp.z =
static_cast<float> (p.z * val);
158 newp.
roll =
static_cast<float> (p.
roll * val);
159 newp.
pitch =
static_cast<float> (p.
pitch * val);
160 newp.
yaw =
static_cast<float> (p.
yaw * val);
222 x = y = z = roll = pitch = yaw = 0.0;
228 x = _x; y = _y; z = _z;
229 roll = pitch = yaw = 0.0;
233 inline ParticleXYZR (
float _x,
float _y,
float _z,
float,
float _pitch,
float)
235 x = _x; y = _y; z = _z;
236 roll = 0; pitch = _pitch; yaw = 0;
244 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
246 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
247 y +=
static_cast<float> (
sampleNormal (mean[1], cov[1]));
248 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
250 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
265 inline Eigen::Affine3f
274 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
276 trans_x, trans_y, trans_z,
277 trans_roll, trans_pitch, trans_yaw);
282 inline float operator [] (
unsigned int i)
290 case 4:
return pitch;
296 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
301 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
310 newp.x =
static_cast<float> (p.x * val);
311 newp.y =
static_cast<float> (p.y * val);
312 newp.z =
static_cast<float> (p.z * val);
313 newp.
roll =
static_cast<float> (p.
roll * val);
314 newp.
pitch =
static_cast<float> (p.
pitch * val);
315 newp.
yaw =
static_cast<float> (p.
yaw * val);
377 x = y = z = roll = pitch = yaw = 0.0;
383 x = _x; y = 0; z = _z;
384 roll = pitch = yaw = 0.0;
388 inline ParticleXYRPY (
float _x,
float,
float _z,
float _roll,
float _pitch,
float _yaw)
390 x = _x; y = 0; z = _z;
391 roll = _roll; pitch = _pitch; yaw = _yaw;
399 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
401 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
403 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
404 roll +=
static_cast<float> (
sampleNormal (mean[3], cov[3]));
405 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
406 yaw +=
static_cast<float> (
sampleNormal (mean[5], cov[5]));
420 inline Eigen::Affine3f
429 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
431 trans_x, trans_y, trans_z,
432 trans_roll, trans_pitch, trans_yaw);
437 inline float operator [] (
unsigned int i)
445 case 4:
return pitch;
451 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
456 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
465 newp.x =
static_cast<float> (p.x * val);
466 newp.y =
static_cast<float> (p.y * val);
467 newp.z =
static_cast<float> (p.z * val);
468 newp.
roll =
static_cast<float> (p.
roll * val);
469 newp.
pitch =
static_cast<float> (p.
pitch * val);
470 newp.
yaw =
static_cast<float> (p.
yaw * val);
530 x = y = z = roll = pitch = yaw = 0.0;
536 x = _x; y = 0; z = _z;
537 roll = pitch = yaw = 0.0;
541 inline ParticleXYRP (
float _x,
float,
float _z,
float,
float _pitch,
float _yaw)
543 x = _x; y = 0; z = _z;
544 roll = 0; pitch = _pitch; yaw = _yaw;
552 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
554 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
556 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
558 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
559 yaw +=
static_cast<float> (
sampleNormal (mean[5], cov[5]));
573 inline Eigen::Affine3f
582 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
584 trans_x, trans_y, trans_z,
585 trans_roll, trans_pitch, trans_yaw);
590 inline float operator [] (
unsigned int i)
598 case 4:
return pitch;
604 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
609 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
618 newp.x =
static_cast<float> (p.x * val);
619 newp.y =
static_cast<float> (p.y * val);
620 newp.z =
static_cast<float> (p.z * val);
621 newp.
roll =
static_cast<float> (p.
roll * val);
622 newp.
pitch =
static_cast<float> (p.
pitch * val);
623 newp.
yaw =
static_cast<float> (p.
yaw * val);
683 x = y = z = roll = pitch = yaw = 0.0;
689 x = _x; y = 0; z = _z;
690 roll = pitch = yaw = 0.0;
694 inline ParticleXYR (
float _x,
float,
float _z,
float,
float _pitch,
float)
696 x = _x; y = 0; z = _z;
697 roll = 0; pitch = _pitch; yaw = 0;
705 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
707 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
709 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
711 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
726 inline Eigen::Affine3f
735 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
737 trans_x, trans_y, trans_z,
738 trans_roll, trans_pitch, trans_yaw);
743 inline float operator [] (
unsigned int i)
751 case 4:
return pitch;
757 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
762 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
771 newp.x =
static_cast<float> (p.x * val);
772 newp.y =
static_cast<float> (p.y * val);
773 newp.z =
static_cast<float> (p.z * val);
774 newp.
roll =
static_cast<float> (p.
roll * val);
775 newp.
pitch =
static_cast<float> (p.
pitch * val);
776 newp.
yaw =
static_cast<float> (p.
yaw * val);
809 #define PCL_STATE_POINT_TYPES \
810 (pcl::tracking::ParticleXYR) \
811 (pcl::tracking::ParticleXYZRPY) \
812 (pcl::tracking::ParticleXYZR) \
813 (pcl::tracking::ParticleXYRPY) \
814 (pcl::tracking::ParticleXYRP)