1 #ifndef PCL_TRACKING_IMPL_TRACKING_H_
2 #define PCL_TRACKING_IMPL_TRACKING_H_
4 #include <pcl/memory.h>
5 #include <pcl/pcl_macros.h>
6 #include <pcl/common/eigen.h>
8 #include <pcl/tracking/tracking.h>
35 x = y = z = roll = pitch = yaw = 0.0;
41 x = _x; y = _y; z = _z;
42 roll = pitch = yaw = 0.0;
46 inline ParticleXYZRPY (
float _x,
float _y,
float _z,
float _roll,
float _pitch,
float _yaw)
48 x = _x; y = _y; z = _z;
49 roll = _roll; pitch = _pitch; yaw = _yaw;
57 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
59 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
60 y +=
static_cast<float> (
sampleNormal (mean[1], cov[1]));
61 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
74 Eigen::Matrix3f current_rotation;
76 Eigen::Quaternionf q_current_rotation (current_rotation);
78 Eigen::Matrix3f mean_rotation;
80 mean[3], mean[4], mean[5]).rotation ();
81 Eigen::Quaternionf q_mean_rotation (mean_rotation);
84 const float scale_factor = 0.2862;
90 Eigen::Vector4f vec_sample_mean_0 (a, b, c, 1);
91 Eigen::Quaternionf q_sample_mean_0 (vec_sample_mean_0);
92 q_sample_mean_0.normalize ();
94 Eigen::Quaternionf q_sample_user_mean = q_sample_mean_0 * q_mean_rotation * q_current_rotation;
96 Eigen::Affine3f affine_R (q_sample_user_mean.toRotationMatrix ());
111 inline Eigen::Affine3f
120 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
122 trans_x, trans_y, trans_z,
123 trans_roll, trans_pitch, trans_yaw);
124 return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
128 inline float operator [] (
unsigned int i)
136 case 4:
return pitch;
147 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
156 newp.x =
static_cast<float> (p.x * val);
157 newp.y =
static_cast<float> (p.y * val);
158 newp.z =
static_cast<float> (p.z * val);
159 newp.
roll =
static_cast<float> (p.
roll * val);
160 newp.
pitch =
static_cast<float> (p.
pitch * val);
161 newp.
yaw =
static_cast<float> (p.
yaw * val);
223 x = y = z = roll = pitch = yaw = 0.0;
229 x = _x; y = _y; z = _z;
230 roll = pitch = yaw = 0.0;
234 inline ParticleXYZR (
float _x,
float _y,
float _z,
float,
float _pitch,
float)
236 x = _x; y = _y; z = _z;
237 roll = 0; pitch = _pitch; yaw = 0;
245 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
247 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
248 y +=
static_cast<float> (
sampleNormal (mean[1], cov[1]));
249 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
251 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
266 inline Eigen::Affine3f
275 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
277 trans_x, trans_y, trans_z,
278 trans_roll, trans_pitch, trans_yaw);
283 inline float operator [] (
unsigned int i)
291 case 4:
return pitch;
302 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
311 newp.x =
static_cast<float> (p.x * val);
312 newp.y =
static_cast<float> (p.y * val);
313 newp.z =
static_cast<float> (p.z * val);
314 newp.
roll =
static_cast<float> (p.
roll * val);
315 newp.
pitch =
static_cast<float> (p.
pitch * val);
316 newp.
yaw =
static_cast<float> (p.
yaw * val);
378 x = y = z = roll = pitch = yaw = 0.0;
384 x = _x; y = 0; z = _z;
385 roll = pitch = yaw = 0.0;
389 inline ParticleXYRPY (
float _x,
float,
float _z,
float _roll,
float _pitch,
float _yaw)
391 x = _x; y = 0; z = _z;
392 roll = _roll; pitch = _pitch; yaw = _yaw;
400 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
402 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
404 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
405 roll +=
static_cast<float> (
sampleNormal (mean[3], cov[3]));
406 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
407 yaw +=
static_cast<float> (
sampleNormal (mean[5], cov[5]));
421 inline Eigen::Affine3f
430 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
432 trans_x, trans_y, trans_z,
433 trans_roll, trans_pitch, trans_yaw);
438 inline float operator [] (
unsigned int i)
446 case 4:
return pitch;
457 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
466 newp.x =
static_cast<float> (p.x * val);
467 newp.y =
static_cast<float> (p.y * val);
468 newp.z =
static_cast<float> (p.z * val);
469 newp.
roll =
static_cast<float> (p.
roll * val);
470 newp.
pitch =
static_cast<float> (p.
pitch * val);
471 newp.
yaw =
static_cast<float> (p.
yaw * val);
531 x = y = z = roll = pitch = yaw = 0.0;
537 x = _x; y = 0; z = _z;
538 roll = pitch = yaw = 0.0;
542 inline ParticleXYRP (
float _x,
float,
float _z,
float,
float _pitch,
float _yaw)
544 x = _x; y = 0; z = _z;
545 roll = 0; pitch = _pitch; yaw = _yaw;
553 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
555 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
557 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
559 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
560 yaw +=
static_cast<float> (
sampleNormal (mean[5], cov[5]));
574 inline Eigen::Affine3f
583 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
585 trans_x, trans_y, trans_z,
586 trans_roll, trans_pitch, trans_yaw);
591 inline float operator [] (
unsigned int i)
599 case 4:
return pitch;
610 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
619 newp.x =
static_cast<float> (p.x * val);
620 newp.y =
static_cast<float> (p.y * val);
621 newp.z =
static_cast<float> (p.z * val);
622 newp.
roll =
static_cast<float> (p.
roll * val);
623 newp.
pitch =
static_cast<float> (p.
pitch * val);
624 newp.
yaw =
static_cast<float> (p.
yaw * val);
684 x = y = z = roll = pitch = yaw = 0.0;
690 x = _x; y = 0; z = _z;
691 roll = pitch = yaw = 0.0;
695 inline ParticleXYR (
float _x,
float,
float _z,
float,
float _pitch,
float)
697 x = _x; y = 0; z = _z;
698 roll = 0; pitch = _pitch; yaw = 0;
706 sample (
const std::vector<double>& mean,
const std::vector<double>& cov)
708 x +=
static_cast<float> (
sampleNormal (mean[0], cov[0]));
710 z +=
static_cast<float> (
sampleNormal (mean[2], cov[2]));
712 pitch +=
static_cast<float> (
sampleNormal (mean[4], cov[4]));
727 inline Eigen::Affine3f
736 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
738 trans_x, trans_y, trans_z,
739 trans_roll, trans_pitch, trans_yaw);
744 inline float operator [] (
unsigned int i)
752 case 4:
return pitch;
763 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
","
772 newp.x =
static_cast<float> (p.x * val);
773 newp.y =
static_cast<float> (p.y * val);
774 newp.z =
static_cast<float> (p.z * val);
775 newp.
roll =
static_cast<float> (p.
roll * val);
776 newp.
pitch =
static_cast<float> (p.
pitch * val);
777 newp.
yaw =
static_cast<float> (p.
yaw * val);
810 #define PCL_STATE_POINT_TYPES \
811 (pcl::tracking::ParticleXYR) \
812 (pcl::tracking::ParticleXYZRPY) \
813 (pcl::tracking::ParticleXYZR) \
814 (pcl::tracking::ParticleXYRPY) \
815 (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)
static int stateDimension()
Eigen::Affine3f toEigenMatrix() const
static int stateDimension()
static pcl::tracking::ParticleXYRPY toState(const Eigen::Affine3f &trans)
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.
pcl::tracking::ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
static int stateDimension()
static pcl::tracking::ParticleXYZR toState(const Eigen::Affine3f &trans)
static pcl::tracking::ParticleXYRP toState(const Eigen::Affine3f &trans)
static pcl::tracking::ParticleXYR toState(const Eigen::Affine3f &trans)
Eigen::Affine3f toEigenMatrix() const
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)
ParticleXYRP(float _x, float, float _z)
static int stateDimension()
static int stateDimension()
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
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)
Eigen::Affine3f toEigenMatrix() const
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) ...
pcl::tracking::ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
ParticleXYZR(float _x, float _y, float _z)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
pcl::tracking::ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
static pcl::tracking::ParticleXYZRPY toState(const Eigen::Affine3f &trans)
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
Eigen::Affine3f toEigenMatrix() const
PCL_EXPORTS double sampleNormal(double mean, double sigma)
ParticleXYR(float _x, float, float _z)
ParticleXYRPY(float _x, float, float _z)