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>
30 x = y = z = roll = pitch = yaw = 0.0;
39 roll = pitch = yaw = 0.0;
44 float _x,
float _y,
float _z,
float _roll,
float _pitch,
float _yaw)
62 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
79 Eigen::Matrix3f current_rotation;
81 Eigen::Quaternionf q_current_rotation(current_rotation);
83 Eigen::Matrix3f mean_rotation;
87 Eigen::Quaternionf q_mean_rotation(mean_rotation);
91 const float scale_factor = 0.2862;
97 Eigen::Vector4f vec_sample_mean_0(a, b, c, 1);
98 Eigen::Quaternionf q_sample_mean_0(vec_sample_mean_0);
99 q_sample_mean_0.normalize();
101 Eigen::Quaternionf q_sample_user_mean =
102 q_sample_mean_0 * q_mean_rotation * q_current_rotation;
104 Eigen::Affine3f affine_R(q_sample_user_mean.toRotationMatrix());
119 inline Eigen::Affine3f
128 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
130 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
131 return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
162 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
168 inline ParticleXYZRPY
172 newp.x =
static_cast<float>(p.x * val);
173 newp.y =
static_cast<float>(p.y * val);
174 newp.z =
static_cast<float>(p.z * val);
175 newp.
roll =
static_cast<float>(p.
roll * val);
176 newp.
pitch =
static_cast<float>(p.
pitch * val);
177 newp.
yaw =
static_cast<float>(p.
yaw * val);
182 inline ParticleXYZRPY
196 inline ParticleXYZRPY
233 x = y = z = roll = pitch = yaw = 0.0;
242 roll = pitch = yaw = 0.0;
246 inline ParticleXYZR(
float _x,
float _y,
float _z,
float,
float _pitch,
float)
264 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
270 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
285 inline Eigen::Affine3f
294 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
296 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
328 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
338 newp.x =
static_cast<float>(p.x * val);
339 newp.y =
static_cast<float>(p.y * val);
340 newp.z =
static_cast<float>(p.z * val);
341 newp.
roll =
static_cast<float>(p.
roll * val);
342 newp.
pitch =
static_cast<float>(p.
pitch * val);
343 newp.
yaw =
static_cast<float>(p.
yaw * val);
399 x = y = z = roll = pitch = yaw = 0.0;
408 roll = pitch = yaw = 0.0;
412 inline ParticleXYRPY(
float _x,
float,
float _z,
float _roll,
float _pitch,
float _yaw)
430 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
435 roll +=
static_cast<float>(
sampleNormal(mean[3], cov[3]));
436 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
437 yaw +=
static_cast<float>(
sampleNormal(mean[5], cov[5]));
451 inline Eigen::Affine3f
460 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
462 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
464 trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
495 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
505 newp.x =
static_cast<float>(p.x * val);
506 newp.y =
static_cast<float>(p.y * val);
507 newp.z =
static_cast<float>(p.z * val);
508 newp.
roll =
static_cast<float>(p.
roll * val);
509 newp.
pitch =
static_cast<float>(p.
pitch * val);
510 newp.
yaw =
static_cast<float>(p.
yaw * val);
566 x = y = z = roll = pitch = yaw = 0.0;
575 roll = pitch = yaw = 0.0;
579 inline ParticleXYRP(
float _x,
float,
float _z,
float,
float _pitch,
float _yaw)
597 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
603 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
604 yaw +=
static_cast<float>(
sampleNormal(mean[5], cov[5]));
618 inline Eigen::Affine3f
627 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
629 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
662 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
672 newp.x =
static_cast<float>(p.x * val);
673 newp.y =
static_cast<float>(p.y * val);
674 newp.z =
static_cast<float>(p.z * val);
675 newp.
roll =
static_cast<float>(p.
roll * val);
676 newp.
pitch =
static_cast<float>(p.
pitch * val);
677 newp.
yaw =
static_cast<float>(p.
yaw * val);
733 x = y = z = roll = pitch = yaw = 0.0;
742 roll = pitch = yaw = 0.0;
746 inline ParticleXYR(
float _x,
float,
float _z,
float,
float _pitch,
float)
764 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
770 pitch +=
static_cast<float>(
sampleNormal(mean[4], cov[4]));
785 inline Eigen::Affine3f
794 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
796 trans, trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw);
828 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
838 newp.x =
static_cast<float>(p.x * val);
839 newp.y =
static_cast<float>(p.y * val);
840 newp.z =
static_cast<float>(p.z * val);
841 newp.
roll =
static_cast<float>(p.
roll * val);
842 newp.
pitch =
static_cast<float>(p.
pitch * val);
843 newp.
yaw =
static_cast<float>(p.
yaw * val);
878 #define PCL_STATE_POINT_TYPES \
879 (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
880 pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
881 pcl::tracking::ParticleXYRP)