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::Matrix<Scalar, 4, 4> &transform,
224 bool copy_all_fields)
226 if (&cloud_in != &cloud_out)
245 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
252 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
254 if (!std::isfinite (cloud_in[i].x) ||
255 !std::isfinite (cloud_in[i].y) ||
256 !std::isfinite (cloud_in[i].z))
258 tf.
se3 (cloud_in[i].data, cloud_out[i].data);
264 template <
typename Po
intT,
typename Scalar>
void
268 const Eigen::Matrix<Scalar, 4, 4> &transform,
269 bool copy_all_fields)
271 std::size_t npts = indices.size ();
275 cloud_out.
width =
static_cast<int> (npts);
285 for (std::size_t i = 0; i < npts; ++i)
289 cloud_out[i] = cloud_in[indices[i]];
290 tf.
se3 (cloud_in[indices[i]].data, cloud_out[i].data);
297 for (std::size_t i = 0; i < npts; ++i)
300 cloud_out[i] = cloud_in[indices[i]];
301 if (!std::isfinite (cloud_in[indices[i]].x) ||
302 !std::isfinite (cloud_in[indices[i]].y) ||
303 !std::isfinite (cloud_in[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::Matrix<Scalar, 4, 4> &transform,
315 bool copy_all_fields)
317 if (&cloud_in != &cloud_out)
337 for (std::size_t i = 0; i < cloud_out.
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.
size (); ++i)
348 if (!std::isfinite (cloud_in[i].x) ||
349 !std::isfinite (cloud_in[i].y) ||
350 !std::isfinite (cloud_in[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
363 const Eigen::Matrix<Scalar, 4, 4> &transform,
364 bool copy_all_fields)
366 std::size_t npts = indices.size ();
370 cloud_out.
width =
static_cast<int> (npts);
380 for (std::size_t i = 0; i < cloud_out.
size (); ++i)
384 cloud_out[i] = cloud_in[indices[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.
size (); ++i)
396 cloud_out[i] = cloud_in[indices[i]];
398 if (!std::isfinite (cloud_in[indices[i]].x) ||
399 !std::isfinite (cloud_in[indices[i]].y) ||
400 !std::isfinite (cloud_in[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));