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));
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__) 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)));
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
uint32_t width
The point cloud width (if organized as an image-structure).
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).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
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...
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
A point structure representing Euclidean xyz coordinates, and the RGB color.