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 (std::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 (std::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 (std::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 (std::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 (std::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 (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
240 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
246 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
248 if (!std::isfinite (cloud_in.
points[i].x) ||
249 !std::isfinite (cloud_in.
points[i].y) ||
250 !std::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 std::size_t npts = indices.size ();
269 cloud_out.
width =
static_cast<int> (npts);
271 cloud_out.
points.resize (npts);
279 for (std::size_t i = 0; i < npts; ++i)
284 tf.
se3 (cloud_in[indices[i]].data, cloud_out[i].data);
291 for (std::size_t i = 0; i < npts; ++i)
295 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
296 !std::isfinite (cloud_in.
points[indices[i]].y) ||
297 !std::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 (std::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 (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
342 if (!std::isfinite (cloud_in.
points[i].x) ||
343 !std::isfinite (cloud_in.
points[i].y) ||
344 !std::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 std::size_t npts = indices.size ();
364 cloud_out.
width =
static_cast<int> (npts);
366 cloud_out.
points.resize (npts);
374 for (std::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 (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
392 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
393 !std::isfinite (cloud_in.
points[indices[i]].y) ||
394 !std::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)
459 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
460 Eigen::Matrix<Scalar, 4, 1> centroid;
464 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
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));