Loading...
Searching...
No Matches
tracking.hpp
#ifndef PCL_TRACKING_IMPL_TRACKING_H_
#define PCL_TRACKING_IMPL_TRACKING_H_
#include <pcl/common/eigen.h>
#include <pcl/tracking/tracking.h>
#include <pcl/memory.h>
#include <pcl/pcl_macros.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.
constexpr 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 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};
}
template <class InputIterator>
static ParticleXYZRPY
weightedAverage(InputIterator first, InputIterator last)
{
ParticleXYZRPY wa;
float wa_roll_sin = 0.0, wa_roll_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0,
wa_yaw_sin = 0.0, wa_yaw_cos = 0.0;
for (auto point = first; point != last; ++point) {
wa.x += point->x * point->weight;
wa.y += point->y * point->weight;
wa.z += point->z * point->weight;
wa_pitch_cos = std::cos(point->pitch);
wa_roll_sin += wa_pitch_cos * std::sin(point->roll) * point->weight;
wa_roll_cos += wa_pitch_cos * std::cos(point->roll) * point->weight;
wa_pitch_sin += std::sin(point->pitch) * point->weight;
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
}
wa.roll = std::atan2(wa_roll_sin, wa_roll_cos);
wa.pitch = std::asin(wa_pitch_sin);
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
return wa;
}
// 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 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 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 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);
}
} // namespace tracking
} // namespace pcl
//########################################################################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 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 {trans_x, trans_y, trans_z, 0, trans_pitch, 0};
}
template <class InputIterator>
static ParticleXYZR
weightedAverage(InputIterator first, InputIterator last)
{
ParticleXYZR wa;
float wa_pitch_sin = 0.0;
for (auto point = first; point != last; ++point) {
wa.x += point->x * point->weight;
wa.y += point->y * point->weight;
wa.z += point->z * point->weight;
wa_pitch_sin += std::sin(point->pitch) * point->weight;
}
wa.roll = 0.0;
wa.pitch = std::asin(wa_pitch_sin);
wa.yaw = 0.0;
return wa;
}
// 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 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 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 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);
}
} // namespace tracking
} // namespace pcl
//########################################################################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 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 {trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw};
}
template <class InputIterator>
static ParticleXYRPY
weightedAverage(InputIterator first, InputIterator last)
{
ParticleXYRPY wa;
float wa_roll_sin = 0.0, wa_roll_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0,
wa_yaw_sin = 0.0, wa_yaw_cos = 0.0;
for (auto point = first; point != last; ++point) {
wa.x += point->x * point->weight;
wa.z += point->z * point->weight;
wa_pitch_cos = std::cos(point->pitch);
wa_roll_sin += wa_pitch_cos * std::sin(point->roll) * point->weight;
wa_roll_cos += wa_pitch_cos * std::cos(point->roll) * point->weight;
wa_pitch_sin += std::sin(point->pitch) * point->weight;
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
}
wa.y = 0;
wa.roll = std::atan2(wa_roll_sin, wa_roll_cos);
wa.pitch = std::asin(wa_pitch_sin);
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
return wa;
}
// 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 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 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 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);
}
} // namespace tracking
} // namespace pcl
//########################################################################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 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 {trans_x, 0, trans_z, 0, trans_pitch, trans_yaw};
}
template <class InputIterator>
static ParticleXYRP
weightedAverage(InputIterator first, InputIterator last)
{
ParticleXYRP wa;
float wa_yaw_sin = 0.0, wa_yaw_cos = 0.0, wa_pitch_sin = 0.0, wa_pitch_cos = 0.0;
for (auto point = first; point != last; ++point) {
wa.x += point->x * point->weight;
wa.z += point->z * point->weight;
wa_pitch_cos = std::cos(point->pitch);
wa_pitch_sin += std::sin(point->pitch) * point->weight;
wa_yaw_sin += wa_pitch_cos * std::sin(point->yaw) * point->weight;
wa_yaw_cos += wa_pitch_cos * std::cos(point->yaw) * point->weight;
}
wa.y = 0.0;
wa.roll = 0.0;
wa.pitch = std::asin(wa_pitch_sin);
wa.yaw = std::atan2(wa_yaw_sin, wa_yaw_cos);
return wa;
}
// 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 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 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 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);
}
} // namespace tracking
} // namespace pcl
//########################################################################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 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 {trans_x, 0, trans_z, 0, trans_pitch, 0};
}
template <class InputIterator>
static ParticleXYR
weightedAverage(InputIterator first, InputIterator last)
{
ParticleXYR wa;
float wa_pitch_sin = 0.0;
for (auto point = first; point != last; ++point) {
wa.x += point->x * point->weight;
wa.z += point->z * point->weight;
wa_pitch_sin += std::sin(point->pitch) * point->weight;
}
wa.y = 0.0;
wa.roll = 0.0;
wa.pitch = std::asin(wa_pitch_sin);
wa.yaw = 0.0;
return wa;
}
// 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 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 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 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);
}
} // namespace tracking
} // namespace pcl
#define PCL_STATE_POINT_TYPES \
(pcl::tracking::ParticleXYR)(pcl::tracking::ParticleXYZRPY)( \
pcl::tracking::ParticleXYZR)(pcl::tracking::ParticleXYRPY)( \
pcl::tracking::ParticleXYRP)
#endif //
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
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 (intrinsic rotations, ZYX-convention) from the given transformatio...
Definition eigen.hpp:593
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 (intrinsic rotations,...
Definition eigen.hpp:607
pcl::getEulerAngles
void getEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract the Euler angles (intrinsic rotations, ZYX-convention) from the given transformation.
Definition eigen.hpp:584
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::tracking::sampleNormal
PCL_EXPORTS double sampleNormal(double mean, double sigma)
pcl::tracking::operator*
ParticleXYZRPY operator*(const ParticleXYZRPY &p, double val)
Definition tracking.hpp:191
pcl::tracking::operator<<
std::ostream & operator<<(std::ostream &os, const ParticleXYZRPY &p)
Definition tracking.hpp:182
pcl::tracking::operator-
ParticleXYZRPY operator-(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition tracking.hpp:219
pcl::tracking::operator+
ParticleXYZRPY operator+(const ParticleXYZRPY &a, const ParticleXYZRPY &b)
Definition tracking.hpp:205
pcl
Definition convolution.h:46
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::tracking::_ParticleXYR
Definition tracking.hpp:799
pcl::tracking::_ParticleXYR::pitch
float pitch
Definition tracking.hpp:804
pcl::tracking::_ParticleXYR::yaw
float yaw
Definition tracking.hpp:805
pcl::tracking::_ParticleXYR::data_c
float data_c[4]
Definition tracking.hpp:808
pcl::tracking::_ParticleXYR::weight
float weight
Definition tracking.hpp:806
pcl::tracking::_ParticleXYR::roll
float roll
Definition tracking.hpp:803
pcl::tracking::_ParticleXYRP
Definition tracking.hpp:612
pcl::tracking::_ParticleXYRP::data_c
float data_c[4]
Definition tracking.hpp:621
pcl::tracking::_ParticleXYRP::pitch
float pitch
Definition tracking.hpp:617
pcl::tracking::_ParticleXYRP::weight
float weight
Definition tracking.hpp:619
pcl::tracking::_ParticleXYRP::roll
float roll
Definition tracking.hpp:616
pcl::tracking::_ParticleXYRP::yaw
float yaw
Definition tracking.hpp:618
pcl::tracking::_ParticleXYRPY
Definition tracking.hpp:422
pcl::tracking::_ParticleXYRPY::roll
float roll
Definition tracking.hpp:426
pcl::tracking::_ParticleXYRPY::weight
float weight
Definition tracking.hpp:429
pcl::tracking::_ParticleXYRPY::pitch
float pitch
Definition tracking.hpp:427
pcl::tracking::_ParticleXYRPY::data_c
float data_c[4]
Definition tracking.hpp:431
pcl::tracking::_ParticleXYRPY::yaw
float yaw
Definition tracking.hpp:428
pcl::tracking::_ParticleXYZR
Definition tracking.hpp:238
pcl::tracking::_ParticleXYZR::pitch
float pitch
Definition tracking.hpp:243
pcl::tracking::_ParticleXYZR::yaw
float yaw
Definition tracking.hpp:244
pcl::tracking::_ParticleXYZR::data_c
float data_c[4]
Definition tracking.hpp:247
pcl::tracking::_ParticleXYZR::weight
float weight
Definition tracking.hpp:245
pcl::tracking::_ParticleXYZR::roll
float roll
Definition tracking.hpp:242
pcl::tracking::_ParticleXYZRPY
Definition tracking.hpp:11
pcl::tracking::_ParticleXYZRPY::weight
float weight
Definition tracking.hpp:18
pcl::tracking::_ParticleXYZRPY::data_c
float data_c[4]
Definition tracking.hpp:20
pcl::tracking::_ParticleXYZRPY::yaw
float yaw
Definition tracking.hpp:17
pcl::tracking::_ParticleXYZRPY::roll
float roll
Definition tracking.hpp:15
pcl::tracking::_ParticleXYZRPY::pitch
float pitch
Definition tracking.hpp:16
pcl::tracking::ParticleXYR
Definition tracking.hpp:813
pcl::tracking::ParticleXYR::weightedAverage
static ParticleXYR weightedAverage(InputIterator first, InputIterator last)
Definition tracking.hpp:885
pcl::tracking::ParticleXYR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:869
pcl::tracking::ParticleXYR::stateDimension
static int stateDimension()
Definition tracking.hpp:841
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z, float, float _pitch, float)
Definition tracking.hpp:829
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR()
Definition tracking.hpp:814
pcl::tracking::ParticleXYR::operator[]
float operator[](unsigned int i)
Definition tracking.hpp:903
pcl::tracking::ParticleXYR::ParticleXYR
ParticleXYR(float _x, float, float _z)
Definition tracking.hpp:820
pcl::tracking::ParticleXYR::zero
void zero()
Definition tracking.hpp:858
pcl::tracking::ParticleXYR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:847
pcl::tracking::ParticleXYR::toState
static ParticleXYR toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:875
pcl::tracking::ParticleXYRP
Definition tracking.hpp:626
pcl::tracking::ParticleXYRP::stateDimension
static int stateDimension()
Definition tracking.hpp:654
pcl::tracking::ParticleXYRP::zero
void zero()
Definition tracking.hpp:671
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP()
Definition tracking.hpp:627
pcl::tracking::ParticleXYRP::weightedAverage
static ParticleXYRP weightedAverage(InputIterator first, InputIterator last)
Definition tracking.hpp:698
pcl::tracking::ParticleXYRP::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:660
pcl::tracking::ParticleXYRP::operator[]
float operator[](unsigned int i)
Definition tracking.hpp:719
pcl::tracking::ParticleXYRP::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:682
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z)
Definition tracking.hpp:633
pcl::tracking::ParticleXYRP::toState
static ParticleXYRP toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:688
pcl::tracking::ParticleXYRP::ParticleXYRP
ParticleXYRP(float _x, float, float _z, float, float _pitch, float _yaw)
Definition tracking.hpp:642
pcl::tracking::ParticleXYRPY
Definition tracking.hpp:436
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY()
Definition tracking.hpp:437
pcl::tracking::ParticleXYRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:470
pcl::tracking::ParticleXYRPY::stateDimension
static int stateDimension()
Definition tracking.hpp:464
pcl::tracking::ParticleXYRPY::zero
void zero()
Definition tracking.hpp:481
pcl::tracking::ParticleXYRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:492
pcl::tracking::ParticleXYRPY::weightedAverage
static ParticleXYRPY weightedAverage(InputIterator first, InputIterator last)
Definition tracking.hpp:508
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z)
Definition tracking.hpp:443
pcl::tracking::ParticleXYRPY::toState
static ParticleXYRPY toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:498
pcl::tracking::ParticleXYRPY::ParticleXYRPY
ParticleXYRPY(float _x, float, float _z, float _roll, float _pitch, float _yaw)
Definition tracking.hpp:452
pcl::tracking::ParticleXYRPY::operator[]
float operator[](unsigned int i)
Definition tracking.hpp:532
pcl::tracking::ParticleXYZR
Definition tracking.hpp:252
pcl::tracking::ParticleXYZR::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:308
pcl::tracking::ParticleXYZR::stateDimension
static int stateDimension()
Definition tracking.hpp:280
pcl::tracking::ParticleXYZR::zero
void zero()
Definition tracking.hpp:297
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z, float, float _pitch, float)
Definition tracking.hpp:268
pcl::tracking::ParticleXYZR::toState
static ParticleXYZR toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:314
pcl::tracking::ParticleXYZR::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:286
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR(float _x, float _y, float _z)
Definition tracking.hpp:259
pcl::tracking::ParticleXYZR::operator[]
float operator[](unsigned int i)
Definition tracking.hpp:342
pcl::tracking::ParticleXYZR::ParticleXYZR
ParticleXYZR()
Definition tracking.hpp:253
pcl::tracking::ParticleXYZR::weightedAverage
static ParticleXYZR weightedAverage(InputIterator first, InputIterator last)
Definition tracking.hpp:324
pcl::tracking::ParticleXYZRPY
Definition tracking.hpp:25
pcl::tracking::ParticleXYZRPY::toState
static ParticleXYZRPY toState(const Eigen::Affine3f &trans)
Definition tracking.hpp:124
pcl::tracking::ParticleXYZRPY::stateDimension
static int stateDimension()
Definition tracking.hpp:54
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY()
Definition tracking.hpp:26
pcl::tracking::ParticleXYZRPY::zero
void zero()
Definition tracking.hpp:107
pcl::tracking::ParticleXYZRPY::sample
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
Definition tracking.hpp:60
pcl::tracking::ParticleXYZRPY::weightedAverage
static ParticleXYZRPY weightedAverage(InputIterator first, InputIterator last)
Definition tracking.hpp:134
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
Definition tracking.hpp:41
pcl::tracking::ParticleXYZRPY::toEigenMatrix
Eigen::Affine3f toEigenMatrix() const
Definition tracking.hpp:118
pcl::tracking::ParticleXYZRPY::operator[]
float operator[](unsigned int i)
Definition tracking.hpp:158
pcl::tracking::ParticleXYZRPY::ParticleXYZRPY
ParticleXYZRPY(float _x, float _y, float _z)
Definition tracking.hpp:32