1 #ifndef PCL_TRACKING_IMPL_TRACKING_H_
2 #define PCL_TRACKING_IMPL_TRACKING_H_
4 #include <pcl/common/eigen.h>
5 #include <pcl/tracking/tracking.h>
28 x = y = z = roll = pitch = yaw = 0.0;
37 roll = pitch = yaw = 0.0;
42 float _x,
float _y,
float _z,
float _roll,
float _pitch,
float _yaw)
60 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
77 Eigen::Matrix3f current_rotation;
79 Eigen::Quaternionf q_current_rotation(current_rotation);
81 Eigen::Matrix3f mean_rotation;
85 Eigen::Quaternionf q_mean_rotation(mean_rotation);
89 const float scale_factor = 0.2862;
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();
99 Eigen::Quaternionf q_sample_user_mean =
100 q_sample_mean_0 * q_mean_rotation * q_current_rotation;
102 Eigen::Affine3f affine_R(q_sample_user_mean.toRotationMatrix());
117 inline Eigen::Affine3f
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};
160 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
166 inline ParticleXYZRPY
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);
180 inline ParticleXYZRPY
194 inline ParticleXYZRPY
231 x = y = z = roll = pitch = yaw = 0.0;
240 roll = pitch = yaw = 0.0;
244 inline ParticleXYZR(
float _x,
float _y,
float _z,
float,
float _pitch,
float)
262 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
268 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
283 inline Eigen::Affine3f
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);
326 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
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);
397 x = y = z = roll = pitch = yaw = 0.0;
406 roll = pitch = yaw = 0.0;
410 inline ParticleXYRPY(
float _x,
float,
float _z,
float _roll,
float _pitch,
float _yaw)
428 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
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]));
449 inline Eigen::Affine3f
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));
493 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
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);
564 x = y = z = roll = pitch = yaw = 0.0;
573 roll = pitch = yaw = 0.0;
577 inline ParticleXYRP(
float _x,
float,
float _z,
float,
float _pitch,
float _yaw)
595 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
601 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
602 yaw +=
static_cast<float>(
sampleNormal(mean[5], cov[5]));
616 inline Eigen::Affine3f
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);
660 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
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);
731 x = y = z = roll = pitch = yaw = 0.0;
740 roll = pitch = yaw = 0.0;
744 inline ParticleXYR(
float _x,
float,
float _z,
float,
float _pitch,
float)
762 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
768 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
783 inline Eigen::Affine3f
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);
826 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
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);
876 #define PCL_STATE_POINT_TYPES \
877 (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
878 pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
879 pcl::tracking::ParticleXYRP)