Point Cloud Library (PCL)  1.8.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 ///////////////////////////////////////////////////////////////////////////////////////////
41 template <typename PointT, typename Scalar> void
43  pcl::PointCloud<PointT> &cloud_out,
44  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
45  bool copy_all_fields)
46 {
47  if (&cloud_in != &cloud_out)
48  {
49  cloud_out.header = cloud_in.header;
50  cloud_out.is_dense = cloud_in.is_dense;
51  cloud_out.width = cloud_in.width;
52  cloud_out.height = cloud_in.height;
53  cloud_out.points.reserve (cloud_in.points.size ());
54  if (copy_all_fields)
55  cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
56  else
57  cloud_out.points.resize (cloud_in.points.size ());
58  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
59  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
60  }
61 
62  if (cloud_in.is_dense)
63  {
64  // If the dataset is dense, simply transform it!
65  for (size_t i = 0; i < cloud_out.points.size (); ++i)
66  {
67  //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
68  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
69  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
70  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
71  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
72  }
73  }
74  else
75  {
76  // Dataset might contain NaNs and Infs, so check for them first,
77  // otherwise we get errors during the multiplication (?)
78  for (size_t i = 0; i < cloud_out.points.size (); ++i)
79  {
80  if (!pcl_isfinite (cloud_in.points[i].x) ||
81  !pcl_isfinite (cloud_in.points[i].y) ||
82  !pcl_isfinite (cloud_in.points[i].z))
83  continue;
84  //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
85  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
86  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
87  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
88  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
89  }
90  }
91 }
92 
93 ///////////////////////////////////////////////////////////////////////////////////////////
94 template <typename PointT, typename Scalar> void
96  const std::vector<int> &indices,
97  pcl::PointCloud<PointT> &cloud_out,
98  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
99  bool copy_all_fields)
100 {
101  size_t npts = indices.size ();
102  // In order to transform the data, we need to remove NaNs
103  cloud_out.is_dense = cloud_in.is_dense;
104  cloud_out.header = cloud_in.header;
105  cloud_out.width = static_cast<int> (npts);
106  cloud_out.height = 1;
107  cloud_out.points.resize (npts);
108  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
109  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
110 
111  if (cloud_in.is_dense)
112  {
113  // If the dataset is dense, simply transform it!
114  for (size_t i = 0; i < npts; ++i)
115  {
116  // Copy fields first, then transform xyz data
117  if (copy_all_fields)
118  cloud_out.points[i] = cloud_in.points[indices[i]];
119  //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
120  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
121  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
122  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
123  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
124  }
125  }
126  else
127  {
128  // Dataset might contain NaNs and Infs, so check for them first,
129  // otherwise we get errors during the multiplication (?)
130  for (size_t i = 0; i < npts; ++i)
131  {
132  if (copy_all_fields)
133  cloud_out.points[i] = cloud_in.points[indices[i]];
134  if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
135  !pcl_isfinite (cloud_in.points[indices[i]].y) ||
136  !pcl_isfinite (cloud_in.points[indices[i]].z))
137  continue;
138  //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
139  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
140  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
141  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
142  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
143  }
144  }
145 }
146 
147 ///////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointT, typename Scalar> void
150  pcl::PointCloud<PointT> &cloud_out,
151  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
152  bool copy_all_fields)
153 {
154  if (&cloud_in != &cloud_out)
155  {
156  // Note: could be replaced by cloud_out = cloud_in
157  cloud_out.header = cloud_in.header;
158  cloud_out.width = cloud_in.width;
159  cloud_out.height = cloud_in.height;
160  cloud_out.is_dense = cloud_in.is_dense;
161  cloud_out.points.reserve (cloud_out.points.size ());
162  if (copy_all_fields)
163  cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
164  else
165  cloud_out.points.resize (cloud_in.points.size ());
166  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
167  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
168  }
169 
170  // If the data is dense, we don't need to check for NaN
171  if (cloud_in.is_dense)
172  {
173  for (size_t i = 0; i < cloud_out.points.size (); ++i)
174  {
175  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
176  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
177  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
178  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
179  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
180 
181  // Rotate normals (WARNING: transform.rotation () uses SVD internally!)
182  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
183  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
184  cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
185  cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
186  cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
187  }
188  }
189  // Dataset might contain NaNs and Infs, so check for them first.
190  else
191  {
192  for (size_t i = 0; i < cloud_out.points.size (); ++i)
193  {
194  if (!pcl_isfinite (cloud_in.points[i].x) ||
195  !pcl_isfinite (cloud_in.points[i].y) ||
196  !pcl_isfinite (cloud_in.points[i].z))
197  continue;
198 
199  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
200  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
201  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
202  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
203  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
204 
205  // Rotate normals
206  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
207  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
208  cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
209  cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
210  cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
211  }
212  }
213 }
214 
215 ///////////////////////////////////////////////////////////////////////////////////////////
216 template <typename PointT, typename Scalar> void
218  const std::vector<int> &indices,
219  pcl::PointCloud<PointT> &cloud_out,
220  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
221  bool copy_all_fields)
222 {
223  size_t npts = indices.size ();
224  // In order to transform the data, we need to remove NaNs
225  cloud_out.is_dense = cloud_in.is_dense;
226  cloud_out.header = cloud_in.header;
227  cloud_out.width = static_cast<int> (npts);
228  cloud_out.height = 1;
229  cloud_out.points.resize (npts);
230  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
231  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
232 
233  // If the data is dense, we don't need to check for NaN
234  if (cloud_in.is_dense)
235  {
236  for (size_t i = 0; i < cloud_out.points.size (); ++i)
237  {
238  // Copy fields first, then transform
239  if (copy_all_fields)
240  cloud_out.points[i] = cloud_in.points[indices[i]];
241  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
242  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
243  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
244  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
245  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
246 
247  // Rotate normals
248  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
249  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
250  cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
251  cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
252  cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
253  }
254  }
255  // Dataset might contain NaNs and Infs, so check for them first.
256  else
257  {
258  for (size_t i = 0; i < cloud_out.points.size (); ++i)
259  {
260  // Copy fields first, then transform
261  if (copy_all_fields)
262  cloud_out.points[i] = cloud_in.points[indices[i]];
263 
264  if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
265  !pcl_isfinite (cloud_in.points[indices[i]].y) ||
266  !pcl_isfinite (cloud_in.points[indices[i]].z))
267  continue;
268 
269  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
270  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
271  cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
272  cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
273  cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
274 
275  // Rotate normals
276  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
277  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
278  cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
279  cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
280  cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
281  }
282  }
283 }
284 
285 ///////////////////////////////////////////////////////////////////////////////////////////
286 template <typename PointT, typename Scalar> inline void
288  pcl::PointCloud<PointT> &cloud_out,
289  const Eigen::Matrix<Scalar, 3, 1> &offset,
290  const Eigen::Quaternion<Scalar> &rotation,
291  bool copy_all_fields)
292 {
293  Eigen::Translation<Scalar, 3> translation (offset);
294  // Assemble an Eigen Transform
295  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
296  transformPointCloud (cloud_in, cloud_out, t, copy_all_fields);
297 }
298 
299 ///////////////////////////////////////////////////////////////////////////////////////////
300 template <typename PointT, typename Scalar> inline void
302  pcl::PointCloud<PointT> &cloud_out,
303  const Eigen::Matrix<Scalar, 3, 1> &offset,
304  const Eigen::Quaternion<Scalar> &rotation,
305  bool copy_all_fields)
306 {
307  Eigen::Translation<Scalar, 3> translation (offset);
308  // Assemble an Eigen Transform
309  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
310  transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields);
311 }
312 
313 ///////////////////////////////////////////////////////////////////////////////////////////
314 template <typename PointT, typename Scalar> inline PointT
316  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
317 {
318  PointT ret = point;
319  ret.getVector3fMap () = transform * point.getVector3fMap ();
320 
321  return (ret);
322 }
323 
324 ///////////////////////////////////////////////////////////////////////////////////////////
325 template <typename PointT, typename Scalar> inline PointT
327  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
328 {
329  PointT ret = point;
330  ret.getVector3fMap () = transform * point.getVector3fMap ();
331  ret.getNormalVector3fMap () = transform.rotation () * point.getNormalVector3fMap ();
332 
333  return (ret);
334 }
335 
336 ///////////////////////////////////////////////////////////////////////////////////////////
337 template <typename PointT, typename Scalar> double
339  Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
340 {
341  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
342  Eigen::Matrix<Scalar, 4, 1> centroid;
343 
344  pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
345 
346  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
347  Eigen::Matrix<Scalar, 3, 1> eigen_vals;
348  pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
349 
350  double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
351  double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
352 
353  transform.translation () = centroid.head (3);
354  transform.linear () = eigen_vects;
355 
356  return (std::min (rel1, rel2));
357 }
358 
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:423
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:338
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
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 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:489
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.
Definition: transforms.hpp:149
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
Definition: transforms.hpp:42
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:251
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:326
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields=true)
Transform a point cloud and rotate its normals using an Eigen transform.
Definition: transforms.h:307
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415