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;
60 sample(
const std::vector<double>& mean,
const std::vector<double>& cov)
117 inline Eigen::Affine3f
160 os <<
"(" << p.x <<
"," << p.y <<
"," << p.z <<
"," << p.
roll <<
"," << p.
pitch <<
","
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);
231 x = y = z = roll = pitch = yaw = 0.0;
240 roll = pitch = yaw = 0.0;
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
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);
397 x = y = z = roll = pitch = yaw = 0.0;
406 roll = pitch = yaw = 0.0;
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
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);
564 x = y = z = roll = pitch = yaw = 0.0;
573 roll = pitch = yaw = 0.0;
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
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);
731 x = y = z = roll = pitch = yaw = 0.0;
740 roll = pitch = yaw = 0.0;
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
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);
876#define PCL_STATE_POINT_TYPES \
877 (pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
878 pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
879 pcl::tracking::ParticleXYRP)
Iterator class for point clouds with or without given indices.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
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.
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)
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.
Defines functions, macros and traits for allocating and using memory.
PCL_EXPORTS double sampleNormal(double mean, double sigma)
ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Defines all the PCL and non-PCL macros used.
Eigen::Affine3f toEigenMatrix() const
static int stateDimension()
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
float operator[](unsigned int i)
ParticleXYR(float _x, float, float _z)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
static ParticleXYR toState(const Eigen::Affine3f &trans)
static int stateDimension()
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
float operator[](unsigned int i)
Eigen::Affine3f toEigenMatrix() const
ParticleXYRP(float _x, float, float _z)
static ParticleXYRP toState(const Eigen::Affine3f &trans)
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
static int stateDimension()
Eigen::Affine3f toEigenMatrix() const
ParticleXYRPY(float _x, float, float _z)
static ParticleXYRPY toState(const Eigen::Affine3f &trans)
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
float operator[](unsigned int i)
Eigen::Affine3f toEigenMatrix() const
static int stateDimension()
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
static ParticleXYZR toState(const Eigen::Affine3f &trans)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
ParticleXYZR(float _x, float _y, float _z)
float operator[](unsigned int i)
static ParticleXYZRPY toState(const Eigen::Affine3f &trans)
static int stateDimension()
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
float operator[](unsigned int i)
ParticleXYZRPY(float _x, float _y, float _z)