42 #include <pcl/common/transforms.h>
45 #include <xmmintrin.h>
49 #include <immintrin.h>
66 template<
typename Scalar>
69 const Eigen::Matrix<Scalar, 4, 4>&
tf;
74 Transformer (
const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };
79 void so3 (
const float* src,
float* tgt)
const
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]);
91 void se3 (
const float* src,
float* tgt)
const
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));
101 #if defined(__SSE2__)
105 struct Transformer<float>
112 for (std::size_t i = 0; i < 4; ++i)
113 c[i] = _mm_load_ps (tf.col (i).data ());
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]))));
133 #if !defined(__AVX__)
137 struct 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)));
187 struct 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])))));
214 #endif // !defined(__AVX__)
215 #endif // defined(__SSE2__)
220 template <
typename Po
intT,
typename Scalar>
void
223 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
224 bool copy_all_fields)
226 if (&cloud_in != &cloud_out)
245 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
246 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
252 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
254 if (!std::isfinite (cloud_in.
points[i].x) ||
255 !std::isfinite (cloud_in.
points[i].y) ||
256 !std::isfinite (cloud_in.
points[i].z))
258 tf.se3 (cloud_in[i].data, cloud_out[i].data);
264 template <
typename Po
intT,
typename Scalar>
void
266 const std::vector<int> &indices,
268 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
269 bool copy_all_fields)
271 std::size_t npts = indices.size ();
275 cloud_out.
width =
static_cast<int> (npts);
277 cloud_out.
points.resize (npts);
285 for (std::size_t i = 0; i < npts; ++i)
290 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
297 for (std::size_t i = 0; i < npts; ++i)
301 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
302 !std::isfinite (cloud_in.
points[indices[i]].y) ||
303 !std::isfinite (cloud_in.
points[indices[i]].z))
305 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
311 template <
typename Po
intT,
typename Scalar>
void
314 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
315 bool copy_all_fields)
317 if (&cloud_in != &cloud_out)
337 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
339 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
340 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
346 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
348 if (!std::isfinite (cloud_in.
points[i].x) ||
349 !std::isfinite (cloud_in.
points[i].y) ||
350 !std::isfinite (cloud_in.
points[i].z))
352 tf.se3 (cloud_in[i].data, cloud_out[i].data);
353 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
359 template <
typename Po
intT,
typename Scalar>
void
361 const std::vector<int> &indices,
363 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
364 bool copy_all_fields)
366 std::size_t npts = indices.size ();
370 cloud_out.
width =
static_cast<int> (npts);
372 cloud_out.
points.resize (npts);
380 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
385 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
386 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
392 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
398 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
399 !std::isfinite (cloud_in.
points[indices[i]].y) ||
400 !std::isfinite (cloud_in.
points[indices[i]].z))
403 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
404 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
410 template <
typename Po
intT,
typename Scalar>
inline void
413 const Eigen::Matrix<Scalar, 3, 1> &offset,
414 const Eigen::Quaternion<Scalar> &rotation,
415 bool copy_all_fields)
417 Eigen::Translation<Scalar, 3> translation (offset);
419 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
424 template <
typename Po
intT,
typename Scalar>
inline void
427 const Eigen::Matrix<Scalar, 3, 1> &offset,
428 const Eigen::Quaternion<Scalar> &rotation,
429 bool copy_all_fields)
431 Eigen::Translation<Scalar, 3> translation (offset);
433 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
438 template <
typename Po
intT,
typename Scalar>
inline PointT
443 tf.
se3 (point.data, ret.data);
448 template <
typename Po
intT,
typename Scalar>
inline PointT
453 tf.
se3 (point.data, ret.data);
454 tf.so3 (point.data_n, ret.data_n);
459 template <
typename Po
intT,
typename Scalar>
double
461 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
463 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
464 Eigen::Matrix<Scalar, 4, 1> centroid;
468 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
469 Eigen::Matrix<Scalar, 3, 1> eigen_vals;
470 pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
472 double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
473 double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
475 transform.translation () = centroid.head (3);
476 transform.linear () = eigen_vects;
478 return (std::min (rel1, rel2));
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.
double getPrincipalTransformation(const pcl::PointCloud< PointT > &cloud, Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Calculates the principal (PCA-based) alignment of the point cloud.
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 transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Transform a point cloud and rotate its normals using an Eigen transform.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
pcl::PCLHeader header
The point cloud header.