Point Cloud Library (PCL)  1.14.1-dev
transforms.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/common/transforms.h>
43 
44 #if defined(__SSE2__)
45 #include <xmmintrin.h>
46 #endif
47 
48 #if defined(__AVX__)
49 #include <immintrin.h>
50 #endif
51 
52 #include <algorithm>
53 #include <cmath>
54 #include <cstddef>
55 #include <vector>
56 
57 
58 namespace pcl
59 {
60 
61 namespace detail
62 {
63 
64 /** A helper struct to apply an SO3 or SE3 transform to a 3D point.
65  * Supports single and double precision transform matrices. */
66 template<typename Scalar>
68 {
69  const Eigen::Matrix<Scalar, 4, 4>& tf;
70 
71  /** Construct a transformer object.
72  * The transform matrix is captured by const reference. Make sure that it does not go out of scope before this
73  * object does. */
74  Transformer (const Eigen::Matrix<Scalar, 4, 4>& transform) : tf (transform) { };
75 
76  /** Apply SO3 transform (top-left corner of the transform matrix).
77  * \param[in] src input 3D point (pointer to 3 floats)
78  * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
79  void so3 (const float* src, float* tgt) const
80  {
81  const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
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]);
85  tgt[3] = 0;
86  }
87 
88  /** Apply SE3 transform.
89  * \param[in] src input 3D point (pointer to 3 floats)
90  * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */
91  void se3 (const float* src, float* tgt) const
92  {
93  const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt
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));
97  tgt[3] = 1;
98  }
99 };
100 
101 #if defined(__SSE2__)
102 
103 /** Optimized version for single-precision transforms using SSE2 intrinsics. */
104 template<>
105 struct Transformer<float>
106 {
107  /// Columns of the transform matrix stored in XMM registers.
108  __m128 c[4];
109 
110  Transformer(const Eigen::Matrix4f& tf)
111  {
112  for (std::size_t i = 0; i < 4; ++i)
113  c[i] = _mm_load_ps (tf.col (i).data ());
114  }
115 
116  void so3 (const float* src, float* tgt) const
117  {
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)));
122  }
123 
124  void se3 (const float* src, float* tgt) const
125  {
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]))));
130  }
131 };
132 
133 #if !defined(__AVX__)
134 
135 /** Optimized version for double-precision transform using SSE2 intrinsics. */
136 template<>
137 struct Transformer<double>
138 {
139  /// Columns of the transform matrix stored in XMM registers.
140  __m128d c[4][2];
141 
142  Transformer(const Eigen::Matrix4d& tf)
143  {
144  for (std::size_t i = 0; i < 4; ++i)
145  {
146  c[i][0] = _mm_load_pd (tf.col (i).data () + 0);
147  c[i][1] = _mm_load_pd (tf.col (i).data () + 2);
148  }
149  }
150 
151  void so3 (const float* src, float* tgt) const
152  {
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]);
156 
157  for (std::size_t i = 1; i < 3; ++i)
158  {
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);
162  }
163 
164  _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
165  }
166 
167  void se3 (const float* src, float* tgt) const
168  {
169  __m128d p0 = c[3][0];
170  __m128d p1 = c[3][1];
171 
172  for (std::size_t i = 0; i < 3; ++i)
173  {
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);
177  }
178 
179  _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1)));
180  }
181 };
182 
183 #else
184 
185 /** Optimized version for double-precision transform using AVX intrinsics. */
186 template<>
187 struct Transformer<double>
188 {
189  __m256d c[4];
190 
191  Transformer(const Eigen::Matrix4d& tf)
192  {
193  for (std::size_t i = 0; i < 4; ++i)
194  c[i] = _mm256_load_pd (tf.col (i).data ());
195  }
196 
197  void so3 (const float* src, float* tgt) const
198  {
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))));
203  }
204 
205  void se3 (const float* src, float* tgt) const
206  {
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])))));
211  }
212 };
213 
214 #endif // !defined(__AVX__)
215 #endif // defined(__SSE2__)
216 
217 } // namespace detail
218 
219 
220 template <typename PointT, typename Scalar> void
222  pcl::PointCloud<PointT> &cloud_out,
223  const Eigen::Matrix<Scalar, 4, 4> &transform,
224  bool copy_all_fields)
225 {
226  if (&cloud_in != &cloud_out)
227  {
228  cloud_out.header = cloud_in.header;
229  cloud_out.is_dense = cloud_in.is_dense;
230  cloud_out.reserve (cloud_in.size ());
231  if (copy_all_fields)
232  cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
233  else
234  cloud_out.resize (cloud_in.width, cloud_in.height);
235  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
236  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
237  }
238 
239  pcl::detail::Transformer<Scalar> tf (transform);
240  if (cloud_in.is_dense)
241  {
242  // If the dataset is dense, simply transform it!
243  for (std::size_t i = 0; i < cloud_out.size (); ++i)
244  tf.se3 (cloud_in[i].data, cloud_out[i].data);
245  }
246  else
247  {
248  // Dataset might contain NaNs and Infs, so check for them first,
249  // otherwise we get errors during the multiplication (?)
250  for (std::size_t i = 0; i < cloud_out.size (); ++i)
251  {
252  if (!std::isfinite (cloud_in[i].x) ||
253  !std::isfinite (cloud_in[i].y) ||
254  !std::isfinite (cloud_in[i].z))
255  continue;
256  tf.se3 (cloud_in[i].data, cloud_out[i].data);
257  }
258  }
259 }
260 
261 
262 template <typename PointT, typename Scalar> void
264  const Indices &indices,
265  pcl::PointCloud<PointT> &cloud_out,
266  const Eigen::Matrix<Scalar, 4, 4> &transform,
267  bool copy_all_fields)
268 {
269  std::size_t npts = indices.size ();
270  // In order to transform the data, we need to remove NaNs
271  cloud_out.is_dense = cloud_in.is_dense;
272  cloud_out.header = cloud_in.header;
273  cloud_out.width = static_cast<int> (npts);
274  cloud_out.height = 1;
275  cloud_out.resize (npts);
276  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
277  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
278 
279  pcl::detail::Transformer<Scalar> tf (transform);
280  if (cloud_in.is_dense)
281  {
282  // If the dataset is dense, simply transform it!
283  for (std::size_t i = 0; i < npts; ++i)
284  {
285  // Copy fields first, then transform xyz data
286  if (copy_all_fields)
287  cloud_out[i] = cloud_in[indices[i]];
288  tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
289  }
290  }
291  else
292  {
293  // Dataset might contain NaNs and Infs, so check for them first,
294  // otherwise we get errors during the multiplication (?)
295  for (std::size_t i = 0; i < npts; ++i)
296  {
297  if (copy_all_fields)
298  cloud_out[i] = cloud_in[indices[i]];
299  if (!std::isfinite (cloud_in[indices[i]].x) ||
300  !std::isfinite (cloud_in[indices[i]].y) ||
301  !std::isfinite (cloud_in[indices[i]].z))
302  continue;
303  tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
304  }
305  }
306 }
307 
308 
309 inline void
311  pcl::PointCloud<pcl::PointXY> &cloud_out,
312  const Eigen::Affine2f &transform,
313  bool copy_all_fields)
314  {
315  if (&cloud_in != &cloud_out)
316  {
317  cloud_out.header = cloud_in.header;
318  cloud_out.is_dense = cloud_in.is_dense;
319  cloud_out.reserve (cloud_in.size ());
320  if (copy_all_fields)
321  cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
322  else
323  cloud_out.resize (cloud_in.width, cloud_in.height);
324  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
325  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
326  }
327  if(cloud_in.is_dense)
328  {
329  for (std::size_t i = 0; i < cloud_out.size (); ++i)
330  {
331  cloud_out[i].getVector2fMap () = transform * cloud_in[i].getVector2fMap();
332  }
333  }
334  else
335  {
336  for (std::size_t i = 0; i < cloud_out.size (); ++i)
337  {
338  if (!std::isfinite(cloud_in[i].x) || !std::isfinite(cloud_in[i].y))
339  {
340  continue;
341  }
342  cloud_out[i].getVector2fMap () = transform * cloud_in[i].getVector2fMap();
343  }
344  }
345  }
346 
347 
348 template <typename PointT, typename Scalar> void
350  pcl::PointCloud<PointT> &cloud_out,
351  const Eigen::Matrix<Scalar, 4, 4> &transform,
352  bool copy_all_fields)
353 {
354  if (&cloud_in != &cloud_out)
355  {
356  // Note: could be replaced by cloud_out = cloud_in
357  cloud_out.header = cloud_in.header;
358  cloud_out.is_dense = cloud_in.is_dense;
359  cloud_out.reserve (cloud_in.size ());
360  if (copy_all_fields)
361  cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
362  else
363  cloud_out.resize (cloud_in.width, cloud_in.height);
364  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
365  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
366  }
367 
368  pcl::detail::Transformer<Scalar> tf (transform);
369  // If the data is dense, we don't need to check for NaN
370  if (cloud_in.is_dense)
371  {
372  for (std::size_t i = 0; i < cloud_out.size (); ++i)
373  {
374  tf.se3 (cloud_in[i].data, cloud_out[i].data);
375  tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
376  }
377  }
378  // Dataset might contain NaNs and Infs, so check for them first.
379  else
380  {
381  for (std::size_t i = 0; i < cloud_out.size (); ++i)
382  {
383  if (!std::isfinite (cloud_in[i].x) ||
384  !std::isfinite (cloud_in[i].y) ||
385  !std::isfinite (cloud_in[i].z))
386  continue;
387  tf.se3 (cloud_in[i].data, cloud_out[i].data);
388  tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n);
389  }
390  }
391 }
392 
393 
394 template <typename PointT, typename Scalar> void
396  const Indices &indices,
397  pcl::PointCloud<PointT> &cloud_out,
398  const Eigen::Matrix<Scalar, 4, 4> &transform,
399  bool copy_all_fields)
400 {
401  std::size_t npts = indices.size ();
402  // In order to transform the data, we need to remove NaNs
403  cloud_out.is_dense = cloud_in.is_dense;
404  cloud_out.header = cloud_in.header;
405  cloud_out.width = static_cast<int> (npts);
406  cloud_out.height = 1;
407  cloud_out.resize (npts);
408  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
409  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
410 
411  pcl::detail::Transformer<Scalar> tf (transform);
412  // If the data is dense, we don't need to check for NaN
413  if (cloud_in.is_dense)
414  {
415  for (std::size_t i = 0; i < cloud_out.size (); ++i)
416  {
417  // Copy fields first, then transform
418  if (copy_all_fields)
419  cloud_out[i] = cloud_in[indices[i]];
420  tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
421  tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
422  }
423  }
424  // Dataset might contain NaNs and Infs, so check for them first.
425  else
426  {
427  for (std::size_t i = 0; i < cloud_out.size (); ++i)
428  {
429  // Copy fields first, then transform
430  if (copy_all_fields)
431  cloud_out[i] = cloud_in[indices[i]];
432 
433  if (!std::isfinite (cloud_in[indices[i]].x) ||
434  !std::isfinite (cloud_in[indices[i]].y) ||
435  !std::isfinite (cloud_in[indices[i]].z))
436  continue;
437 
438  tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data);
439  tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n);
440  }
441  }
442 }
443 
444 
445 template <typename PointT, typename Scalar> inline void
447  pcl::PointCloud<PointT> &cloud_out,
448  const Eigen::Matrix<Scalar, 3, 1> &offset,
449  const Eigen::Quaternion<Scalar> &rotation,
450  bool copy_all_fields)
451 {
452  Eigen::Translation<Scalar, 3> translation (offset);
453  // Assemble an Eigen Transform
454  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
455  transformPointCloud (cloud_in, cloud_out, t, copy_all_fields);
456 }
457 
458 
459 template <typename PointT, typename Scalar> inline void
461  pcl::PointCloud<PointT> &cloud_out,
462  const Eigen::Matrix<Scalar, 3, 1> &offset,
463  const Eigen::Quaternion<Scalar> &rotation,
464  bool copy_all_fields)
465 {
466  Eigen::Translation<Scalar, 3> translation (offset);
467  // Assemble an Eigen Transform
468  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
469  transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields);
470 }
471 
472 
473 template <typename PointT, typename Scalar> inline PointT
474 transformPoint (const PointT &point, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
475 {
476  PointT ret = point;
477  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
478  tf.se3 (point.data, ret.data);
479  return (ret);
480 }
481 
482 
483 template <typename PointT, typename Scalar> inline PointT
484 transformPointWithNormal (const PointT &point, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
485 {
486  PointT ret = point;
487  pcl::detail::Transformer<Scalar> tf (transform.matrix ());
488  tf.se3 (point.data, ret.data);
489  tf.so3 (point.data_n, ret.data_n);
490  return (ret);
491 }
492 
493 
494 template <typename PointT, typename Scalar> double
496  Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
497 {
498  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
499  Eigen::Matrix<Scalar, 4, 1> centroid;
500 
501  pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
502 
503  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
504  Eigen::Matrix<Scalar, 3, 1> eigen_vals;
505  pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
506 
507  double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
508  double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
509 
510  transform.translation () = centroid.template head<3> ();
511  transform.linear () = eigen_vects;
512 
513  return (std::min (rel1, rel2));
514 }
515 
516 } // namespace pcl
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
PointT * data() noexcept
Definition: point_cloud.h:447
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
iterator end() noexcept
Definition: point_cloud.h:430
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
void reserve(std::size_t n)
Definition: point_cloud.h:445
iterator begin() noexcept
Definition: point_cloud.h:429
void assign(index_t count, const PointT &value)
Replaces the points with count copies of value
Definition: point_cloud.h:548
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Transform a point cloud and rotate its normals using an Eigen transform.
Definition: transforms.hpp:349
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.
Definition: transforms.hpp:484
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:509
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
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...
Definition: eigen.hpp:295
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.
Definition: eigen.h:389
double getPrincipalTransformation(const pcl::PointCloud< PointT > &cloud, Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Calculates the principal (PCA-based) alignment of the point cloud.
Definition: transforms.hpp:495
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.
A helper struct to apply an SO3 or SE3 transform to a 3D point.
Definition: transforms.hpp:68
void se3(const float *src, float *tgt) const
Apply SE3 transform.
Definition: transforms.hpp:91
void so3(const float *src, float *tgt) const
Apply SO3 transform (top-left corner of the transform matrix).
Definition: transforms.hpp:79
Transformer(const Eigen::Matrix< Scalar, 4, 4 > &transform)
Construct a transformer object.
Definition: transforms.hpp:74
const Eigen::Matrix< Scalar, 4, 4 > & tf
Definition: transforms.hpp:69