42#include <pcl/common/transforms.h>
66template<
typename Scalar>
69 const Eigen::Matrix<Scalar, 4, 4>&
tf;
74 Transformer (
const Eigen::Matrix<Scalar, 4, 4>& transform) :
tf (transform) { };
81 const Scalar p[3] = {
src[0],
src[1],
src[2] };
82 tgt[0] =
static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2]);
83 tgt[1] =
static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2]);
84 tgt[2] =
static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2]);
93 const Scalar p[3] = {
src[0],
src[1],
src[2] };
94 tgt[0] =
static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2] +
tf (0, 3));
95 tgt[1] =
static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2] +
tf (1, 3));
96 tgt[2] =
static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2] +
tf (2, 3));
105struct Transformer<
float>
112 for (std::size_t i = 0; i < 4; ++i)
116 void so3 (
const float* src,
float* tgt)
const
118 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
119 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
120 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
121 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
124 void se3 (
const float* src,
float* tgt)
const
126 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
127 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
128 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
129 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
137struct Transformer<double>
144 for (std::size_t i = 0; i < 4; ++i)
146 c[i][0] = _mm_load_pd (
tf.col (i).data () + 0);
147 c[i][1] = _mm_load_pd (
tf.col (i).data () + 2);
151 void so3 (
const float* src,
float* tgt)
const
153 __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
154 __m128d p0 = _mm_mul_pd (xx, c[0][0]);
155 __m128d p1 = _mm_mul_pd (xx, c[0][1]);
157 for (std::size_t i = 1; i < 3; ++i)
159 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
160 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
161 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
164 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
167 void se3 (
const float* src,
float* tgt)
const
169 __m128d p0 = c[3][0];
170 __m128d p1 = c[3][1];
172 for (std::size_t i = 0; i < 3; ++i)
174 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
175 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
176 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
179 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
187struct Transformer<double>
193 for (std::size_t i = 0; i < 4; ++i)
194 c[i] = _mm256_load_pd (
tf.col (i).data ());
197 void so3 (
const float* src,
float* tgt)
const
199 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
200 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
201 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
202 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
205 void se3 (
const float* src,
float* tgt)
const
207 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
208 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
209 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
210 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
220template <
typename Po
intT,
typename Scalar>
void
223 const Eigen::Matrix<Scalar, 4, 4> &transform,
252 if (!std::isfinite (
cloud_in[i].x) ||
262template <
typename Po
intT,
typename Scalar>
void
266 const Eigen::Matrix<Scalar, 4, 4> &transform,
283 for (std::size_t i = 0; i <
npts; ++i)
295 for (std::size_t i = 0; i <
npts; ++i)
299 if (!std::isfinite (
cloud_in[indices[i]].x) ||
300 !std::isfinite (
cloud_in[indices[i]].y) ||
301 !std::isfinite (
cloud_in[indices[i]].z))
309template <
typename Po
intT,
typename Scalar>
void
312 const Eigen::Matrix<Scalar, 4, 4> &transform,
344 if (!std::isfinite (
cloud_in[i].x) ||
355template <
typename Po
intT,
typename Scalar>
void
359 const Eigen::Matrix<Scalar, 4, 4> &transform,
394 if (!std::isfinite (
cloud_in[indices[i]].x) ||
395 !std::isfinite (
cloud_in[indices[i]].y) ||
396 !std::isfinite (
cloud_in[indices[i]].z))
406template <
typename Po
intT,
typename Scalar>
inline void
409 const Eigen::Matrix<Scalar, 3, 1> &offset,
410 const Eigen::Quaternion<Scalar> &
rotation,
413 Eigen::Translation<Scalar, 3>
translation (offset);
420template <
typename Po
intT,
typename Scalar>
inline void
423 const Eigen::Matrix<Scalar, 3, 1> &offset,
424 const Eigen::Quaternion<Scalar> &
rotation,
427 Eigen::Translation<Scalar, 3>
translation (offset);
434template <
typename Po
intT,
typename Scalar>
inline PointT
439 tf.
se3 (point.data,
ret.data);
444template <
typename Po
intT,
typename Scalar>
inline PointT
449 tf.
se3 (point.data,
ret.data);
450 tf.
so3 (point.data_n,
ret.data_n);
455template <
typename Po
intT,
typename Scalar>
double
457 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
460 Eigen::Matrix<Scalar, 4, 1> centroid;
471 transform.translation () = centroid.head (3);
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Transform a point cloud and rotate its normals using an Eigen transform.
PointT transformPointWithNormal(const PointT &point, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Transform a point with members x,y,z,normal_x,normal_y,normal_z.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
void transformPoint(const Eigen::Matrix< Scalar, 3, 1 > &point_in, Eigen::Matrix< Scalar, 3, 1 > &point_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a point using an affine matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.
double getPrincipalTransformation(const pcl::PointCloud< PointT > &cloud, Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Calculates the principal (PCA-based) alignment of the point cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.