41 #include <xmmintrin.h>
45 #include <immintrin.h>
61 template<
typename Scalar>
64 const Eigen::Matrix<Scalar, 4, 4>&
tf;
69 Transformer (
const Eigen::Matrix<Scalar, 4, 4>& transform) :
tf (transform) { };
74 void so3 (
const float* src,
float* tgt)
const
76 const Scalar p[3] = { src[0], src[1], src[2] };
77 tgt[0] = static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2]);
78 tgt[1] = static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2]);
79 tgt[2] = static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2]);
86 void se3 (
const float* src,
float* tgt)
const
88 const Scalar p[3] = { src[0], src[1], src[2] };
89 tgt[0] = static_cast<float> (
tf (0, 0) * p[0] +
tf (0, 1) * p[1] +
tf (0, 2) * p[2] +
tf (0, 3));
90 tgt[1] = static_cast<float> (
tf (1, 0) * p[0] +
tf (1, 1) * p[1] +
tf (1, 2) * p[2] +
tf (1, 3));
91 tgt[2] = static_cast<float> (
tf (2, 0) * p[0] +
tf (2, 1) * p[1] +
tf (2, 2) * p[2] +
tf (2, 3));
100 struct Transformer<float>
107 for (std::size_t i = 0; i < 4; ++i)
108 c[i] = _mm_load_ps (
tf.col (i).data ());
111 void so3 (
const float* src,
float* tgt)
const
113 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
114 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
115 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
116 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2)));
119 void se3 (
const float* src,
float* tgt)
const
121 __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]);
122 __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]);
123 __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]);
124 _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3]))));
128 #if !defined(__AVX__)
132 struct Transformer<double>
139 for (std::size_t i = 0; i < 4; ++i)
141 c[i][0] = _mm_load_pd (
tf.col (i).data () + 0);
142 c[i][1] = _mm_load_pd (
tf.col (i).data () + 2);
146 void so3 (
const float* src,
float* tgt)
const
148 __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0]));
149 __m128d p0 = _mm_mul_pd (xx, c[0][0]);
150 __m128d p1 = _mm_mul_pd (xx, c[0][1]);
152 for (std::size_t i = 1; i < 3; ++i)
154 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
155 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
156 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
159 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
162 void se3 (
const float* src,
float* tgt)
const
164 __m128d p0 = c[3][0];
165 __m128d p1 = c[3][1];
167 for (std::size_t i = 0; i < 3; ++i)
169 __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i]));
170 p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0);
171 p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1);
174 _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
183 struct Transformer<double>
189 for (std::size_t i = 0; i < 4; ++i)
190 c[i] = _mm256_load_pd (
tf.col (i).data ());
193 void so3 (
const float* src,
float* tgt)
const
195 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
196 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
197 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
198 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2))));
201 void se3 (
const float* src,
float* tgt)
const
203 __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]);
204 __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]);
205 __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]);
206 _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3])))));
219 template <
typename Po
intT,
typename Scalar>
void
222 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
223 bool copy_all_fields)
225 if (&cloud_in != &cloud_out)
244 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
245 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
251 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
253 if (!std::isfinite (cloud_in.
points[i].x) ||
254 !std::isfinite (cloud_in.
points[i].y) ||
255 !std::isfinite (cloud_in.
points[i].z))
257 tf.se3 (cloud_in[i].data, cloud_out[i].data);
263 template <
typename Po
intT,
typename Scalar>
void
265 const std::vector<int> &indices,
267 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
268 bool copy_all_fields)
270 std::size_t npts = indices.size ();
274 cloud_out.
width = static_cast<int> (npts);
276 cloud_out.
points.resize (npts);
284 for (std::size_t i = 0; i < npts; ++i)
289 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
296 for (std::size_t i = 0; i < npts; ++i)
300 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
301 !std::isfinite (cloud_in.
points[indices[i]].y) ||
302 !std::isfinite (cloud_in.
points[indices[i]].z))
304 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
310 template <
typename Po
intT,
typename Scalar>
void
313 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
314 bool copy_all_fields)
316 if (&cloud_in != &cloud_out)
336 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
338 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
339 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
345 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
347 if (!std::isfinite (cloud_in.
points[i].x) ||
348 !std::isfinite (cloud_in.
points[i].y) ||
349 !std::isfinite (cloud_in.
points[i].z))
351 tf.se3 (cloud_in[i].data, cloud_out[i].data);
352 tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
358 template <
typename Po
intT,
typename Scalar>
void
360 const std::vector<int> &indices,
362 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
363 bool copy_all_fields)
365 std::size_t npts = indices.size ();
369 cloud_out.
width = static_cast<int> (npts);
371 cloud_out.
points.resize (npts);
379 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
384 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
385 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
391 for (std::size_t i = 0; i < cloud_out.
points.size (); ++i)
397 if (!std::isfinite (cloud_in.
points[indices[i]].x) ||
398 !std::isfinite (cloud_in.
points[indices[i]].y) ||
399 !std::isfinite (cloud_in.
points[indices[i]].z))
402 tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
403 tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
409 template <
typename Po
intT,
typename Scalar>
inline void
412 const Eigen::Matrix<Scalar, 3, 1> &offset,
413 const Eigen::Quaternion<Scalar> &rotation,
414 bool copy_all_fields)
416 Eigen::Translation<Scalar, 3> translation (offset);
418 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
423 template <
typename Po
intT,
typename Scalar>
inline void
426 const Eigen::Matrix<Scalar, 3, 1> &offset,
427 const Eigen::Quaternion<Scalar> &rotation,
428 bool copy_all_fields)
430 Eigen::Translation<Scalar, 3> translation (offset);
432 Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
437 template <
typename Po
intT,
typename Scalar>
inline PointT
439 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
443 tf.
se3 (point.data, ret.data);
448 template <
typename Po
intT,
typename Scalar>
inline PointT
450 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
454 tf.
se3 (point.data, ret.data);
455 tf.so3 (point.data_n, ret.data_n);
460 template <
typename Po
intT,
typename Scalar>
double
462 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
464 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
465 Eigen::Matrix<Scalar, 4, 1> centroid;
469 EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
470 Eigen::Matrix<Scalar, 3, 1> eigen_vals;
471 pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
473 double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
474 double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
476 transform.translation () = centroid.head (3);
477 transform.linear () = eigen_vects;
479 return (std::min (rel1, rel2));