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)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
ParticleXYZRPY(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
Eigen::Affine3f toEigenMatrix() const
Defines functions, macros and traits for allocating and using memory.
static int stateDimension()
ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
static ParticleXYR toState(const Eigen::Affine3f &trans)
static int stateDimension()
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
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.
float operator[](unsigned int i)
Eigen::Affine3f toEigenMatrix() const
float operator[](unsigned int i)
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
static int stateDimension()
float operator[](unsigned int i)
ParticleXYZRPY(float _x, float _y, float _z)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
static ParticleXYZRPY toState(const Eigen::Affine3f &trans)
ParticleXYRP(float _x, float, float _z)
Eigen::Affine3f toEigenMatrix() const
static int stateDimension()
static int stateDimension()
Eigen::Affine3f toEigenMatrix() const
static ParticleXYZR toState(const Eigen::Affine3f &trans)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
float operator[](unsigned int i)
static ParticleXYRPY toState(const Eigen::Affine3f &trans)
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.
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Eigen::Affine3f toEigenMatrix() const
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
float operator[](unsigned int i)
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) ...
ParticleXYZR(float _x, float _y, float _z)
static ParticleXYRP toState(const Eigen::Affine3f &trans)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
PCL_EXPORTS double sampleNormal(double mean, double sigma)
Defines all the PCL and non-PCL macros used.
ParticleXYR(float _x, float, float _z)
ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
ParticleXYRPY(float _x, float, float _z)