tracking.hpp
#ifndef PCL_TRACKING_IMPL_TRACKING_H_
#define PCL_TRACKING_IMPL_TRACKING_H_
#include <pcl/pcl_macros.h>
#include <pcl/common/eigen.h>
#include <ctime>
#include <pcl/tracking/tracking.h>
namespace pcl
{
namespace tracking
{
struct _ParticleXYZRPY
{
PCL_ADD_POINT4D;
union
{
struct
{
float roll;
float pitch;
float yaw;
float weight;
};
float data_c[4];
};
};
// particle definition
struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY
{
inline ParticleXYZRPY ()
{
x = y = z = roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYZRPY (float _x, float _y, float _z)
{
x = _x; y = _y; z = _z;
roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYZRPY (float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
{
x = _x; y = _y; z = _z;
roll = _roll; pitch = _pitch; yaw = _yaw;
data[3] = 1.0f;
}
inline static int
stateDimension () { return 6; }
void
sample (const std::vector<double>& mean, const std::vector<double>& cov)
{
x += static_cast<float> (sampleNormal (mean[0], cov[0]));
y += static_cast<float> (sampleNormal (mean[1], cov[1]));
z += static_cast<float> (sampleNormal (mean[2], cov[2]));
// The roll, pitch, yaw space is not Euclidean, so if we sample roll,
// pitch, and yaw independently, we bias our sampling in a complicated
// way that depends on where in the space we are sampling.
//
// A solution is to always sample around mean = 0 and project our
// samples onto the space of rotations, SO(3). We rely on the fact
// that we are sampling with small variance, so we do not bias
// ourselves too much with this projection. We then rotate our
// samples by the user's requested mean. The benefit of this approach
// is that our distribution's properties are consistent over the space
// of rotations.
Eigen::Matrix3f current_rotation;
current_rotation = getTransformation (x, y, z, roll, pitch, yaw).rotation ();
Eigen::Quaternionf q_current_rotation (current_rotation);
Eigen::Matrix3f mean_rotation;
mean_rotation = getTransformation (mean[0], mean[1], mean[2],
mean[3], mean[4], mean[5]).rotation ();
Eigen::Quaternionf q_mean_rotation (mean_rotation);
// Scales 1.0 radians of variance in RPY sampling into equivalent units for quaternion sampling.
const float scale_factor = 0.2862;
float a = sampleNormal (0, scale_factor*cov[3]);
float b = sampleNormal (0, scale_factor*cov[4]);
float c = sampleNormal (0, scale_factor*cov[5]);
Eigen::Vector4f vec_sample_mean_0 (a, b, c, 1);
Eigen::Quaternionf q_sample_mean_0 (vec_sample_mean_0);
q_sample_mean_0.normalize ();
Eigen::Quaternionf q_sample_user_mean = q_sample_mean_0 * q_mean_rotation * q_current_rotation;
Eigen::Affine3f affine_R (q_sample_user_mean.toRotationMatrix ());
pcl::getEulerAngles (affine_R, roll, pitch, yaw);
}
void
zero ()
{
x = 0.0;
y = 0.0;
z = 0.0;
roll = 0.0;
pitch = 0.0;
yaw = 0.0;
}
inline Eigen::Affine3f
toEigenMatrix () const
{
return getTransformation(x, y, z, roll, pitch, yaw);
}
static pcl::tracking::ParticleXYZRPY
toState (const Eigen::Affine3f &trans)
{
float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
getTranslationAndEulerAngles (trans,
trans_x, trans_y, trans_z,
trans_roll, trans_pitch, trans_yaw);
return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw};
}
// a[i]
inline float operator [] (unsigned int i)
{
switch (i)
{
case 0: return x;
case 1: return y;
case 2: return z;
case 3: return roll;
case 4: return pitch;
case 5: return yaw;
default: return 0.0;
}
}
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::ostream& operator << (std::ostream& os, const ParticleXYZRPY& p)
{
os << "(" << p.x << "," << p.y << "," << p.z << ","
<< p.roll << "," << p.pitch << "," << p.yaw << ")";
return (os);
}
// a * k
inline pcl::tracking::ParticleXYZRPY operator * (const ParticleXYZRPY& p, double val)
{
pcl::tracking::ParticleXYZRPY newp;
newp.x = static_cast<float> (p.x * val);
newp.y = static_cast<float> (p.y * val);
newp.z = static_cast<float> (p.z * val);
newp.roll = static_cast<float> (p.roll * val);
newp.pitch = static_cast<float> (p.pitch * val);
newp.yaw = static_cast<float> (p.yaw * val);
return (newp);
}
// a + b
inline pcl::tracking::ParticleXYZRPY operator + (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
{
pcl::tracking::ParticleXYZRPY newp;
newp.x = a.x + b.x;
newp.y = a.y + b.y;
newp.z = a.z + b.z;
newp.roll = a.roll + b.roll;
newp.pitch = a.pitch + b.pitch;
newp.yaw = a.yaw + b.yaw;
return (newp);
}
// a - b
inline pcl::tracking::ParticleXYZRPY operator - (const ParticleXYZRPY& a, const ParticleXYZRPY& b)
{
pcl::tracking::ParticleXYZRPY newp;
newp.x = a.x - b.x;
newp.y = a.y - b.y;
newp.z = a.z - b.z;
newp.roll = a.roll - b.roll;
newp.pitch = a.pitch - b.pitch;
newp.yaw = a.yaw - b.yaw;
return (newp);
}
}
}
//########################################################################33
namespace pcl
{
namespace tracking
{
struct _ParticleXYZR
{
PCL_ADD_POINT4D;
union
{
struct
{
float roll;
float pitch;
float yaw;
float weight;
};
float data_c[4];
};
};
// particle definition
struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR
{
inline ParticleXYZR ()
{
x = y = z = roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYZR (float _x, float _y, float _z)
{
x = _x; y = _y; z = _z;
roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYZR (float _x, float _y, float _z, float, float _pitch, float)
{
x = _x; y = _y; z = _z;
roll = 0; pitch = _pitch; yaw = 0;
data[3] = 1.0f;
}
inline static int
stateDimension () { return 6; }
void
sample (const std::vector<double>& mean, const std::vector<double>& cov)
{
x += static_cast<float> (sampleNormal (mean[0], cov[0]));
y += static_cast<float> (sampleNormal (mean[1], cov[1]));
z += static_cast<float> (sampleNormal (mean[2], cov[2]));
roll = 0;
pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
yaw = 0;
}
void
zero ()
{
x = 0.0;
y = 0.0;
z = 0.0;
roll = 0.0;
pitch = 0.0;
yaw = 0.0;
}
inline Eigen::Affine3f
toEigenMatrix () const
{
return getTransformation(x, y, z, roll, pitch, yaw);
}
static pcl::tracking::ParticleXYZR
toState (const Eigen::Affine3f &trans)
{
float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
getTranslationAndEulerAngles (trans,
trans_x, trans_y, trans_z,
trans_roll, trans_pitch, trans_yaw);
return (pcl::tracking::ParticleXYZR (trans_x, trans_y, trans_z, 0, trans_pitch, 0));
}
// a[i]
inline float operator [] (unsigned int i)
{
switch (i)
{
case 0: return x;
case 1: return y;
case 2: return z;
case 3: return roll;
case 4: return pitch;
case 5: return yaw;
default: return 0.0;
}
}
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::ostream& operator << (std::ostream& os, const ParticleXYZR& p)
{
os << "(" << p.x << "," << p.y << "," << p.z << ","
<< p.roll << "," << p.pitch << "," << p.yaw << ")";
return (os);
}
// a * k
inline pcl::tracking::ParticleXYZR operator * (const ParticleXYZR& p, double val)
{
pcl::tracking::ParticleXYZR newp;
newp.x = static_cast<float> (p.x * val);
newp.y = static_cast<float> (p.y * val);
newp.z = static_cast<float> (p.z * val);
newp.roll = static_cast<float> (p.roll * val);
newp.pitch = static_cast<float> (p.pitch * val);
newp.yaw = static_cast<float> (p.yaw * val);
return (newp);
}
// a + b
inline pcl::tracking::ParticleXYZR operator + (const ParticleXYZR& a, const ParticleXYZR& b)
{
pcl::tracking::ParticleXYZR newp;
newp.x = a.x + b.x;
newp.y = a.y + b.y;
newp.z = a.z + b.z;
newp.roll = 0;
newp.pitch = a.pitch + b.pitch;
newp.yaw = 0.0;
return (newp);
}
// a - b
inline pcl::tracking::ParticleXYZR operator - (const ParticleXYZR& a, const ParticleXYZR& b)
{
pcl::tracking::ParticleXYZR newp;
newp.x = a.x - b.x;
newp.y = a.y - b.y;
newp.z = a.z - b.z;
newp.roll = 0.0;
newp.pitch = a.pitch - b.pitch;
newp.yaw = 0.0;
return (newp);
}
}
}
//########################################################################33
namespace pcl
{
namespace tracking
{
struct _ParticleXYRPY
{
PCL_ADD_POINT4D;
union
{
struct
{
float roll;
float pitch;
float yaw;
float weight;
};
float data_c[4];
};
};
// particle definition
struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY
{
inline ParticleXYRPY ()
{
x = y = z = roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYRPY (float _x, float, float _z)
{
x = _x; y = 0; z = _z;
roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYRPY (float _x, float, float _z, float _roll, float _pitch, float _yaw)
{
x = _x; y = 0; z = _z;
roll = _roll; pitch = _pitch; yaw = _yaw;
data[3] = 1.0f;
}
inline static int
stateDimension () { return 6; }
void
sample (const std::vector<double>& mean, const std::vector<double>& cov)
{
x += static_cast<float> (sampleNormal (mean[0], cov[0]));
y = 0;
z += static_cast<float> (sampleNormal (mean[2], cov[2]));
roll += static_cast<float> (sampleNormal (mean[3], cov[3]));
pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
}
void
zero ()
{
x = 0.0;
y = 0.0;
z = 0.0;
roll = 0.0;
pitch = 0.0;
yaw = 0.0;
}
inline Eigen::Affine3f
toEigenMatrix () const
{
return getTransformation(x, y, z, roll, pitch, yaw);
}
static pcl::tracking::ParticleXYRPY
toState (const Eigen::Affine3f &trans)
{
float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
getTranslationAndEulerAngles (trans,
trans_x, trans_y, trans_z,
trans_roll, trans_pitch, trans_yaw);
return (pcl::tracking::ParticleXYRPY (trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw));
}
// a[i]
inline float operator [] (unsigned int i)
{
switch (i)
{
case 0: return x;
case 1: return y;
case 2: return z;
case 3: return roll;
case 4: return pitch;
case 5: return yaw;
default: return 0.0;
}
}
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::ostream& operator << (std::ostream& os, const ParticleXYRPY& p)
{
os << "(" << p.x << "," << p.y << "," << p.z << ","
<< p.roll << "," << p.pitch << "," << p.yaw << ")";
return (os);
}
// a * k
inline pcl::tracking::ParticleXYRPY operator * (const ParticleXYRPY& p, double val)
{
pcl::tracking::ParticleXYRPY newp;
newp.x = static_cast<float> (p.x * val);
newp.y = static_cast<float> (p.y * val);
newp.z = static_cast<float> (p.z * val);
newp.roll = static_cast<float> (p.roll * val);
newp.pitch = static_cast<float> (p.pitch * val);
newp.yaw = static_cast<float> (p.yaw * val);
return (newp);
}
// a + b
inline pcl::tracking::ParticleXYRPY operator + (const ParticleXYRPY& a, const ParticleXYRPY& b)
{
pcl::tracking::ParticleXYRPY newp;
newp.x = a.x + b.x;
newp.y = 0;
newp.z = a.z + b.z;
newp.roll = a.roll + b.roll;
newp.pitch = a.pitch + b.pitch;
newp.yaw = a.yaw + b.yaw;
return (newp);
}
// a - b
inline pcl::tracking::ParticleXYRPY operator - (const ParticleXYRPY& a, const ParticleXYRPY& b)
{
pcl::tracking::ParticleXYRPY newp;
newp.x = a.x - b.x;
newp.z = a.z - b.z;
newp.y = 0;
newp.roll = a.roll - b.roll;
newp.pitch = a.pitch - b.pitch;
newp.yaw = a.yaw - b.yaw;
return (newp);
}
}
}
//########################################################################33
namespace pcl
{
namespace tracking
{
struct _ParticleXYRP
{
PCL_ADD_POINT4D;
union
{
struct
{
float roll;
float pitch;
float yaw;
float weight;
};
float data_c[4];
};
};
// particle definition
struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP
{
inline ParticleXYRP ()
{
x = y = z = roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYRP (float _x, float, float _z)
{
x = _x; y = 0; z = _z;
roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYRP (float _x, float, float _z, float, float _pitch, float _yaw)
{
x = _x; y = 0; z = _z;
roll = 0; pitch = _pitch; yaw = _yaw;
data[3] = 1.0f;
}
inline static int
stateDimension () { return 6; }
void
sample (const std::vector<double>& mean, const std::vector<double>& cov)
{
x += static_cast<float> (sampleNormal (mean[0], cov[0]));
y = 0;
z += static_cast<float> (sampleNormal (mean[2], cov[2]));
roll = 0;
pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
yaw += static_cast<float> (sampleNormal (mean[5], cov[5]));
}
void
zero ()
{
x = 0.0;
y = 0.0;
z = 0.0;
roll = 0.0;
pitch = 0.0;
yaw = 0.0;
}
inline Eigen::Affine3f
toEigenMatrix () const
{
return getTransformation(x, y, z, roll, pitch, yaw);
}
static pcl::tracking::ParticleXYRP
toState (const Eigen::Affine3f &trans)
{
float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
getTranslationAndEulerAngles (trans,
trans_x, trans_y, trans_z,
trans_roll, trans_pitch, trans_yaw);
return (pcl::tracking::ParticleXYRP (trans_x, 0, trans_z, 0, trans_pitch, trans_yaw));
}
// a[i]
inline float operator [] (unsigned int i)
{
switch (i)
{
case 0: return x;
case 1: return y;
case 2: return z;
case 3: return roll;
case 4: return pitch;
case 5: return yaw;
default: return 0.0;
}
}
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::ostream& operator << (std::ostream& os, const ParticleXYRP& p)
{
os << "(" << p.x << "," << p.y << "," << p.z << ","
<< p.roll << "," << p.pitch << "," << p.yaw << ")";
return (os);
}
// a * k
inline pcl::tracking::ParticleXYRP operator * (const ParticleXYRP& p, double val)
{
pcl::tracking::ParticleXYRP newp;
newp.x = static_cast<float> (p.x * val);
newp.y = static_cast<float> (p.y * val);
newp.z = static_cast<float> (p.z * val);
newp.roll = static_cast<float> (p.roll * val);
newp.pitch = static_cast<float> (p.pitch * val);
newp.yaw = static_cast<float> (p.yaw * val);
return (newp);
}
// a + b
inline pcl::tracking::ParticleXYRP operator + (const ParticleXYRP& a, const ParticleXYRP& b)
{
pcl::tracking::ParticleXYRP newp;
newp.x = a.x + b.x;
newp.y = 0;
newp.z = a.z + b.z;
newp.roll = 0;
newp.pitch = a.pitch + b.pitch;
newp.yaw = a.yaw + b.yaw;
return (newp);
}
// a - b
inline pcl::tracking::ParticleXYRP operator - (const ParticleXYRP& a, const ParticleXYRP& b)
{
pcl::tracking::ParticleXYRP newp;
newp.x = a.x - b.x;
newp.z = a.z - b.z;
newp.y = 0;
newp.roll = 0.0;
newp.pitch = a.pitch - b.pitch;
newp.yaw = a.yaw - b.yaw;
return (newp);
}
}
}
//########################################################################33
namespace pcl
{
namespace tracking
{
struct _ParticleXYR
{
PCL_ADD_POINT4D;
union
{
struct
{
float roll;
float pitch;
float yaw;
float weight;
};
float data_c[4];
};
};
// particle definition
struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR
{
inline ParticleXYR ()
{
x = y = z = roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYR (float _x, float, float _z)
{
x = _x; y = 0; z = _z;
roll = pitch = yaw = 0.0;
data[3] = 1.0f;
}
inline ParticleXYR (float _x, float, float _z, float, float _pitch, float)
{
x = _x; y = 0; z = _z;
roll = 0; pitch = _pitch; yaw = 0;
data[3] = 1.0f;
}
inline static int
stateDimension () { return 6; }
void
sample (const std::vector<double>& mean, const std::vector<double>& cov)
{
x += static_cast<float> (sampleNormal (mean[0], cov[0]));
y = 0;
z += static_cast<float> (sampleNormal (mean[2], cov[2]));
roll = 0;
pitch += static_cast<float> (sampleNormal (mean[4], cov[4]));
yaw = 0;
}
void
zero ()
{
x = 0.0;
y = 0.0;
z = 0.0;
roll = 0.0;
pitch = 0.0;
yaw = 0.0;
}
inline Eigen::Affine3f
toEigenMatrix () const
{
return getTransformation(x, y, z, roll, pitch, yaw);
}
static pcl::tracking::ParticleXYR
toState (const Eigen::Affine3f &trans)
{
float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
getTranslationAndEulerAngles (trans,
trans_x, trans_y, trans_z,
trans_roll, trans_pitch, trans_yaw);
return (pcl::tracking::ParticleXYR (trans_x, 0, trans_z, 0, trans_pitch, 0));
}
// a[i]
inline float operator [] (unsigned int i)
{
switch (i)
{
case 0: return x;
case 1: return y;
case 2: return z;
case 3: return roll;
case 4: return pitch;
case 5: return yaw;
default: return 0.0;
}
}
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::ostream& operator << (std::ostream& os, const ParticleXYR& p)
{
os << "(" << p.x << "," << p.y << "," << p.z << ","
<< p.roll << "," << p.pitch << "," << p.yaw << ")";
return (os);
}
// a * k
inline pcl::tracking::ParticleXYR operator * (const ParticleXYR& p, double val)
{
pcl::tracking::ParticleXYR newp;
newp.x = static_cast<float> (p.x * val);
newp.y = static_cast<float> (p.y * val);
newp.z = static_cast<float> (p.z * val);
newp.roll = static_cast<float> (p.roll * val);
newp.pitch = static_cast<float> (p.pitch * val);
newp.yaw = static_cast<float> (p.yaw * val);
return (newp);
}
// a + b
inline pcl::tracking::ParticleXYR operator + (const ParticleXYR& a, const ParticleXYR& b)
{
pcl::tracking::ParticleXYR newp;
newp.x = a.x + b.x;
newp.y = 0;
newp.z = a.z + b.z;
newp.roll = 0;
newp.pitch = a.pitch + b.pitch;
newp.yaw = 0.0;
return (newp);
}
// a - b
inline pcl::tracking::ParticleXYR operator - (const ParticleXYR& a, const ParticleXYR& b)
{
pcl::tracking::ParticleXYR newp;
newp.x = a.x - b.x;
newp.z = a.z - b.z;
newp.y = 0;
newp.roll = 0.0;
newp.pitch = a.pitch - b.pitch;
newp.yaw = 0.0;
return (newp);
}
}
}
#define PCL_STATE_POINT_TYPES \
(pcl::tracking::ParticleXYR) \
(pcl::tracking::ParticleXYZRPY) \
(pcl::tracking::ParticleXYZR) \
(pcl::tracking::ParticleXYRPY) \
(pcl::tracking::ParticleXYRP)
#endif //
pcl::tracking::ParticleXYRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:421
pcl::tracking::_ParticleXYRPY::yaw
float yaw
Definition: tracking.hpp:365
pcl::tracking::_ParticleXYZRPY::data_c
float data_c[4]
Definition: tracking.hpp:25
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z)
Definition: tracking.hpp:226
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z)
Definition: tracking.hpp:687
pcl::tracking::ParticleXYRP
Definition: tracking.hpp:526
pcl::tracking::_ParticleXYRPY::weight
float weight
Definition: tracking.hpp:366
pcl::tracking::_ParticleXYZR
Definition: tracking.hpp:201
pcl::tracking::ParticleXYZR::zero
void zero()
Definition: tracking.hpp:255
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
Definition: tracking.hpp:45
pcl::tracking::_ParticleXYR::yaw
float yaw
Definition: tracking.hpp:671
pcl::tracking::ParticleXYR::stateDimension
static int stateDimension()
Definition: tracking.hpp:702
pcl::tracking::_ParticleXYZR::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:203
pcl::tracking::ParticleXYR
Definition: tracking.hpp:679
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP()
Definition: tracking.hpp:528
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY()
Definition: tracking.hpp:32
pcl::tracking::_ParticleXYZRPY::pitch
float pitch
Definition: tracking.hpp:21
pcl::tracking::ParticleXYRPY::zero
void zero()
Definition: tracking.hpp:410
pcl::tracking::_ParticleXYZR::weight
float weight
Definition: tracking.hpp:211
pcl::tracking::_ParticleXYZRPY::yaw
float yaw
Definition: tracking.hpp:22
pcl::tracking::operator<<
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
Definition: tracking.hpp:144
pcl::tracking::_ParticleXYRPY::roll
float roll
Definition: tracking.hpp:363
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY()
Definition: tracking.hpp:375
pcl::getTransformation
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)
Definition: eigen.hpp:602
pcl::tracking::ParticleXYZRPY
Definition: tracking.hpp:30
pcl::tracking::ParticleXYZR::toState
static pcl::tracking::ParticleXYZR toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:272
pcl::tracking::ParticleXYR::toState
static pcl::tracking::ParticleXYR toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:733
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
Definition: tracking.hpp:694
pcl::tracking::_ParticleXYRP::pitch
float pitch
Definition: tracking.hpp:517
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR()
Definition: tracking.hpp:220
pcl::tracking::_ParticleXYR::weight
float weight
Definition: tracking.hpp:672
pcl::tracking::_ParticleXYR::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:664
pcl::tracking::_ParticleXYZRPY
Definition: tracking.hpp:13
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z)
Definition: tracking.hpp:38
pcl::tracking::_ParticleXYR::roll
float roll
Definition: tracking.hpp:669
pcl::getTranslationAndEulerAngles
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.
Definition: eigen.hpp:588
pcl::tracking::ParticleXYZR
Definition: tracking.hpp:218
pcl::tracking::ParticleXYRPY
Definition: tracking.hpp:373
pcl::tracking::_ParticleXYRPY::data_c
float data_c[4]
Definition: tracking.hpp:368
pcl::tracking::_ParticleXYR
Definition: tracking.hpp:662
pcl::tracking::ParticleXYZRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:111
pcl::tracking::_ParticleXYRPY::pitch
float pitch
Definition: tracking.hpp:364
pcl::tracking::_ParticleXYRP::weight
float weight
Definition: tracking.hpp:519
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR()
Definition: tracking.hpp:681
pcl::tracking::_ParticleXYRP::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:511
pcl::tracking::operator*
pcl::tracking::ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
Definition: tracking.hpp:152
pcl::tracking::ParticleXYZR::stateDimension
static int stateDimension()
Definition: tracking.hpp:241
pcl::tracking::ParticleXYR::zero
void zero()
Definition: tracking.hpp:716
pcl::tracking::_ParticleXYZR::pitch
float pitch
Definition: tracking.hpp:209
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:371
pcl::getEulerAngles
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.
Definition: eigen.hpp:579
pcl::tracking::_ParticleXYZRPY::weight
float weight
Definition: tracking.hpp:23
pcl::tracking::ParticleXYZRPY::stateDimension
static int stateDimension()
Definition: tracking.hpp:53
pcl::tracking::_ParticleXYRP
Definition: tracking.hpp:509
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Definition: tracking.hpp:541
pcl::tracking::operator-
pcl::tracking::ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition: tracking.hpp:178
pcl::tracking::_ParticleXYRP::yaw
float yaw
Definition: tracking.hpp:518
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z)
Definition: tracking.hpp:381
pcl::tracking::ParticleXYRP::zero
void zero()
Definition: tracking.hpp:563
pcl::tracking::ParticleXYZRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:56
pcl::tracking::ParticleXYRP::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:552
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
Definition: tracking.hpp:233
pcl::tracking::ParticleXYRP::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:574
pcl::tracking::ParticleXYZRPY::zero
void zero()
Definition: tracking.hpp:100
pcl::tracking::ParticleXYZR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:266
pcl::tracking::ParticleXYR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition: tracking.hpp:727
pcl::tracking::_ParticleXYR::data_c
float data_c[4]
Definition: tracking.hpp:674
pcl::tracking::ParticleXYRP::toState
static pcl::tracking::ParticleXYRP toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:580
pcl::tracking::ParticleXYRP::stateDimension
static int stateDimension()
Definition: tracking.hpp:549
pcl::tracking::_ParticleXYZR::data_c
float data_c[4]
Definition: tracking.hpp:213
pcl::tracking::ParticleXYZR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:244
pcl::tracking::ParticleXYR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:705
pcl::tracking::ParticleXYRPY::stateDimension
static int stateDimension()
Definition: tracking.hpp:396
pcl::tracking::ParticleXYRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition: tracking.hpp:399
pcl::tracking::_ParticleXYZRPY::roll
float roll
Definition: tracking.hpp:20
pcl::tracking::_ParticleXYRP::data_c
float data_c[4]
Definition: tracking.hpp:521
pcl::tracking::_ParticleXYRPY::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:358
pcl::tracking::_ParticleXYZR::roll
float roll
Definition: tracking.hpp:208
pcl::tracking::_ParticleXYR::pitch
float pitch
Definition: tracking.hpp:670
pcl::tracking::ParticleXYRPY::toState
static pcl::tracking::ParticleXYRPY toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:427
pcl::tracking::sampleNormal
PCL_EXPORTS double sampleNormal(double mean, double sigma)
pcl::tracking::operator+
pcl::tracking::ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition: tracking.hpp:165
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z)
Definition: tracking.hpp:534
pcl::tracking::_ParticleXYZRPY::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: tracking.hpp:15
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
Definition: tracking.hpp:388
pcl::tracking::ParticleXYZRPY::toState
static pcl::tracking::ParticleXYZRPY toState(const Eigen::Affine3f &trans)
Definition: tracking.hpp:117
pcl::tracking::_ParticleXYRPY
Definition: tracking.hpp:356
pcl::tracking::_ParticleXYRP::roll
float roll
Definition: tracking.hpp:516
pcl::tracking::_ParticleXYZR::yaw
float yaw
Definition: tracking.hpp:210