41 #include <xmmintrin.h>
45 #include <immintrin.h>
56 template<
typename Scalar>
59 const Eigen::Matrix<Scalar, 4, 4>&
tf;
64 Transformer (
const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };
69 void so3 (
const float* src,
float* tgt)
const
71 const Scalar p[3] = { src[0], src[1], src[2] };
72 tgt[0] =
static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2]);
73 tgt[1] =
static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2]);
74 tgt[2] =
static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2]);
81 void se3 (
const float* src,
float* tgt)
const
83 const Scalar p[3] = { src[0], src[1], src[2] };
84 tgt[0] =
static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2] +
tf (0, 3));
85 tgt[1] =
static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2] +
tf (1, 3));
86 tgt[2] =
static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2] +
tf (2, 3));
95 struct Transformer<float>
102 for (
size_t i = 0; i < 4; ++i)
103 c[i] = _mm_load_ps (tf.col (i).data ());
106 void so3 (
const float* src,
float* tgt)
const
108 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
109 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
110 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
111 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
114 void se3 (
const float* src,
float* tgt)
const
116 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
117 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
118 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
119 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
123 #if !defined(__AVX__)
127 struct Transformer<double>
134 for (
size_t i = 0; i < 4; ++i)
136 c[i][0] = _mm_load_pd (tf.col (i).data () + 0);
137 c[i][1] = _mm_load_pd (tf.col (i).data () + 2);
141 void so3 (
const float* src,
float* tgt)
const
143 __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
144 __m128d p0 = _mm_mul_pd (xx, c[0][0]);
145 __m128d p1 = _mm_mul_pd (xx, c[0][1]);
147 for (
size_t i = 1; i < 3; ++i)
149 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
150 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
151 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
154 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
157 void se3 (
const float* src,
float* tgt)
const
159 __m128d p0 = c[3][0];
160 __m128d p1 = c[3][1];
162 for (
size_t i = 0; i < 3; ++i)
164 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
165 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
166 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
169 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
178 struct Transformer<double>
184 for (
size_t i = 0; i < 4; ++i)
185 c[i] = _mm256_load_pd (tf.col (i).data ());
188 void so3 (
const float* src,
float* tgt)
const
190 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
191 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
192 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
193 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
196 void se3 (
const float* src,
float* tgt)
const
198 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
199 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
200 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
201 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
214 template <
typename Po
intT,
typename Scalar>
void
217 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
218 bool copy_all_fields)
220 if (&cloud_in != &cloud_out)
239 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
240 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
246 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
248 if (!pcl_isfinite (cloud_in.
points[i].x) ||
249 !pcl_isfinite (cloud_in.
points[i].y) ||
250 !pcl_isfinite (cloud_in.
points[i].z))
252 tf.se3 (cloud_in[i].data, cloud_out[i].data);
258 template <
typename Po
intT,
typename Scalar>
void
260 const std::vector<int> &indices,
262 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
263 bool copy_all_fields)
265 size_t npts = indices.size ();
269 cloud_out.
width =
static_cast<int> (npts);
271 cloud_out.
points.resize (npts);
279 for (
size_t i = 0; i < npts; ++i)
284 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
291 for (
size_t i = 0; i < npts; ++i)
295 if (!pcl_isfinite (cloud_in.
points[indices[i]].x) ||
296 !pcl_isfinite (cloud_in.
points[indices[i]].y) ||
297 !pcl_isfinite (cloud_in.
points[indices[i]].z))
299 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
305 template <
typename Po
intT,
typename Scalar>
void
308 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
309 bool copy_all_fields)
311 if (&cloud_in != &cloud_out)
331 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
333 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
334 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
340 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
342 if (!pcl_isfinite (cloud_in.
points[i].x) ||
343 !pcl_isfinite (cloud_in.
points[i].y) ||
344 !pcl_isfinite (cloud_in.
points[i].z))
346 tf.se3 (cloud_in[i].data, cloud_out[i].data);
347 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
353 template <
typename Po
intT,
typename Scalar>
void
355 const std::vector<int> &indices,
357 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
358 bool copy_all_fields)
360 size_t npts = indices.size ();
364 cloud_out.
width =
static_cast<int> (npts);
366 cloud_out.
points.resize (npts);
374 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
379 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
380 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
386 for (
size_t i = 0; i < cloud_out.
points.size (); ++i)
392 if (!pcl_isfinite (cloud_in.
points[indices[i]].x) ||
393 !pcl_isfinite (cloud_in.
points[indices[i]].y) ||
394 !pcl_isfinite (cloud_in.
points[indices[i]].z))
397 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
398 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
404 template <
typename Po
intT,
typename Scalar>
inline void
407 const Eigen::Matrix<Scalar, 3, 1> &offset,
408 const Eigen::Quaternion<Scalar> &rotation,
409 bool copy_all_fields)
411 Eigen::Translation<Scalar, 3> translation (offset);
413 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
418 template <
typename Po
intT,
typename Scalar>
inline void
421 const Eigen::Matrix<Scalar, 3, 1> &offset,
422 const Eigen::Quaternion<Scalar> &rotation,
423 bool copy_all_fields)
425 Eigen::Translation<Scalar, 3> translation (offset);
427 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
432 template <
typename Po
intT,
typename Scalar>
inline PointT
434 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
438 tf.
se3 (point.data, ret.data);
443 template <
typename Po
intT,
typename Scalar>
inline PointT
445 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
449 tf.
se3 (point.data, ret.data);
450 tf.so3 (point.data_n, ret.data_n);
455 template <
typename Po
intT,
typename Scalar>
double
457 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
460 Eigen::Matrix<Scalar, 4, 1> centroid;
465 Eigen::Matrix<Scalar, 3, 1> eigen_vals;
466 pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
468 double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
469 double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
471 transform.translation () = centroid.head (3);
472 transform.linear () = eigen_vects;
474 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.
uint32_t width
The point cloud width (if organized as an image-structure).
struct pcl::PointXYZIEdge EIGEN_ALIGN16
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...
This file defines compatibility wrappers for low level I/O functions.
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=true)
Transform a point cloud and rotate its normals using an Eigen transform.
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=true)
Apply an affine transform defined by an Eigen Transform.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
uint32_t height
The point cloud height (if organized as an image-structure).
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).
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields=true)
Transform a point cloud and rotate its normals using an Eigen transform.
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.