Point Cloud Library (PCL)  1.14.1-dev
transforms.h
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/point_cloud.h>
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/eigen.h>
45 #include <pcl/PointIndices.h>
46 
47 namespace pcl
48 {
49  /** \brief Apply an affine transform defined by an Eigen Transform
50  * \param[in] cloud_in the input point cloud
51  * \param[out] cloud_out the resultant output point cloud
52  * \param[in] transform an affine transformation (typically a rigid transformation)
53  * \param[in] copy_all_fields flag that controls whether the contents of the fields
54  * (other than x, y, z) should be copied into the new transformed cloud
55  * \note Can be used with cloud_in equal to cloud_out
56  * \ingroup common
57  */
58  template <typename PointT, typename Scalar> void
60  pcl::PointCloud<PointT> &cloud_out,
61  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
62  bool copy_all_fields = true)
63  {
64  return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
65  }
66 
67  template <typename PointT> void
69  pcl::PointCloud<PointT> &cloud_out,
70  const Eigen::Affine3f &transform,
71  bool copy_all_fields = true)
72  {
73  return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
74  }
75 
76  /** \brief Apply an affine transform defined by an Eigen Transform
77  * \param[in] cloud_in the input point cloud
78  * \param[in] indices the set of point indices to use from the input point cloud
79  * \param[out] cloud_out the resultant output point cloud
80  * \param[in] transform an affine transformation (typically a rigid transformation)
81  * \param[in] copy_all_fields flag that controls whether the contents of the fields
82  * (other than x, y, z) should be copied into the new transformed cloud
83  * \ingroup common
84  */
85  template <typename PointT, typename Scalar> void
87  const Indices &indices,
88  pcl::PointCloud<PointT> &cloud_out,
89  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
90  bool copy_all_fields = true)
91  {
92  return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
93  }
94 
95  template <typename PointT> void
97  const Indices &indices,
98  pcl::PointCloud<PointT> &cloud_out,
99  const Eigen::Affine3f &transform,
100  bool copy_all_fields = true)
101  {
102  return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
103  }
104 
105  /** \brief Apply an affine transform defined by an Eigen Transform
106  * \param[in] cloud_in the input point cloud
107  * \param[in] indices the set of point indices to use from the input point cloud
108  * \param[out] cloud_out the resultant output point cloud
109  * \param[in] transform an affine transformation (typically a rigid transformation)
110  * \param[in] copy_all_fields flag that controls whether the contents of the fields
111  * (other than x, y, z) should be copied into the new transformed cloud
112  * \ingroup common
113  */
114  template <typename PointT, typename Scalar> void
116  const pcl::PointIndices &indices,
117  pcl::PointCloud<PointT> &cloud_out,
118  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
119  bool copy_all_fields = true)
120  {
121  return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
122  }
123 
124  template <typename PointT> void
126  const pcl::PointIndices &indices,
127  pcl::PointCloud<PointT> &cloud_out,
128  const Eigen::Affine3f &transform,
129  bool copy_all_fields = true)
130  {
131  return (transformPointCloud<PointT, float> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
132  }
133 
134  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
135  * \param[in] cloud_in the input point cloud
136  * \param[out] cloud_out the resultant output point cloud
137  * \param[in] transform an affine transformation (typically a rigid transformation)
138  * \param[in] copy_all_fields flag that controls whether the contents of the fields
139  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
140  * transformed cloud
141  * \note Can be used with cloud_in equal to cloud_out
142  */
143  template <typename PointT, typename Scalar> void
145  pcl::PointCloud<PointT> &cloud_out,
146  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
147  bool copy_all_fields = true)
148  {
149  return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
150  }
151 
152  template <typename PointT> void
154  pcl::PointCloud<PointT> &cloud_out,
155  const Eigen::Affine3f &transform,
156  bool copy_all_fields = true)
157  {
158  return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform.matrix (), copy_all_fields));
159  }
160 
161  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
162  * \param[in] cloud_in the input point cloud
163  * \param[in] indices the set of point indices to use from the input point cloud
164  * \param[out] cloud_out the resultant output point cloud
165  * \param[in] transform an affine transformation (typically a rigid transformation)
166  * \param[in] copy_all_fields flag that controls whether the contents of the fields
167  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
168  * transformed cloud
169  */
170  template <typename PointT, typename Scalar> void
172  const Indices &indices,
173  pcl::PointCloud<PointT> &cloud_out,
174  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
175  bool copy_all_fields = true)
176  {
177  return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
178  }
179 
180  template <typename PointT> void
182  const Indices &indices,
183  pcl::PointCloud<PointT> &cloud_out,
184  const Eigen::Affine3f &transform,
185  bool copy_all_fields = true)
186  {
187  return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform.matrix (), copy_all_fields));
188  }
189 
190  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
191  * \param[in] cloud_in the input point cloud
192  * \param[in] indices the set of point indices to use from the input point cloud
193  * \param[out] cloud_out the resultant output point cloud
194  * \param[in] transform an affine transformation (typically a rigid transformation)
195  * \param[in] copy_all_fields flag that controls whether the contents of the fields
196  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
197  * transformed cloud
198  */
199  template <typename PointT, typename Scalar> void
201  const pcl::PointIndices &indices,
202  pcl::PointCloud<PointT> &cloud_out,
203  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
204  bool copy_all_fields = true)
205  {
206  return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
207  }
208 
209 
210  template <typename PointT> void
212  const pcl::PointIndices &indices,
213  pcl::PointCloud<PointT> &cloud_out,
214  const Eigen::Affine3f &transform,
215  bool copy_all_fields = true)
216  {
217  return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices.indices, cloud_out, transform.matrix (), copy_all_fields));
218  }
219 
220  /** \brief Apply a rigid transform defined by a 4x4 matrix
221  * \param[in] cloud_in the input point cloud
222  * \param[out] cloud_out the resultant output point cloud
223  * \param[in] transform a rigid transformation
224  * \param[in] copy_all_fields flag that controls whether the contents of the fields
225  * (other than x, y, z) should be copied into the new transformed cloud
226  * \note Can be used with cloud_in equal to cloud_out
227  * \ingroup common
228  */
229  template <typename PointT, typename Scalar> void
231  pcl::PointCloud<PointT> &cloud_out,
232  const Eigen::Matrix<Scalar, 4, 4> &transform,
233  bool copy_all_fields = true);
234 
235  template <typename PointT> void
237  pcl::PointCloud<PointT> &cloud_out,
238  const Eigen::Matrix4f &transform,
239  bool copy_all_fields = true)
240  {
241  return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
242  }
243 
244  /** \brief Apply a rigid transform defined by a 4x4 matrix
245  * \param[in] cloud_in the input point cloud
246  * \param[in] indices the set of point indices to use from the input point cloud
247  * \param[out] cloud_out the resultant output point cloud
248  * \param[in] transform a rigid transformation
249  * \param[in] copy_all_fields flag that controls whether the contents of the fields
250  * (other than x, y, z) should be copied into the new transformed cloud
251  * \ingroup common
252  */
253  template <typename PointT, typename Scalar> void
255  const Indices &indices,
256  pcl::PointCloud<PointT> &cloud_out,
257  const Eigen::Matrix<Scalar, 4, 4> &transform,
258  bool copy_all_fields = true);
259 
260  template <typename PointT> void
262  const Indices &indices,
263  pcl::PointCloud<PointT> &cloud_out,
264  const Eigen::Matrix4f &transform,
265  bool copy_all_fields = true)
266  {
267  return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
268  }
269 
270  /** \brief Apply a rigid transform defined by a 4x4 matrix
271  * \param[in] cloud_in the input point cloud
272  * \param[in] indices the set of point indices to use from the input point cloud
273  * \param[out] cloud_out the resultant output point cloud
274  * \param[in] transform a rigid transformation
275  * \param[in] copy_all_fields flag that controls whether the contents of the fields
276  * (other than x, y, z) should be copied into the new transformed cloud
277  * \ingroup common
278  */
279  template <typename PointT, typename Scalar> void
281  const pcl::PointIndices &indices,
282  pcl::PointCloud<PointT> &cloud_out,
283  const Eigen::Matrix<Scalar, 4, 4> &transform,
284  bool copy_all_fields = true)
285  {
286  return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
287  }
288 
289  template <typename PointT> void
291  const pcl::PointIndices &indices,
292  pcl::PointCloud<PointT> &cloud_out,
293  const Eigen::Matrix4f &transform,
294  bool copy_all_fields = true)
295  {
296  return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
297  }
298 
299  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
300  * \param[in] cloud_in the input point cloud
301  * \param[out] cloud_out the resultant output point cloud
302  * \param[in] transform an affine transformation (typically a rigid transformation)
303  * \param[in] copy_all_fields flag that controls whether the contents of the fields
304  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
305  * transformed cloud
306  * \note Can be used with cloud_in equal to cloud_out
307  * \ingroup common
308  */
309  template <typename PointT, typename Scalar> void
311  pcl::PointCloud<PointT> &cloud_out,
312  const Eigen::Matrix<Scalar, 4, 4> &transform,
313  bool copy_all_fields = true);
314 
315 
316  template <typename PointT> void
318  pcl::PointCloud<PointT> &cloud_out,
319  const Eigen::Matrix4f &transform,
320  bool copy_all_fields = true)
321  {
322  return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
323  }
324 
325  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
326  * \param[in] cloud_in the input point cloud
327  * \param[in] indices the set of point indices to use from the input point cloud
328  * \param[out] cloud_out the resultant output point cloud
329  * \param[in] transform an affine transformation (typically a rigid transformation)
330  * \param[in] copy_all_fields flag that controls whether the contents of the fields
331  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
332  * transformed cloud
333  * \note Can be used with cloud_in equal to cloud_out
334  * \ingroup common
335  */
336  template <typename PointT, typename Scalar> void
338  const Indices &indices,
339  pcl::PointCloud<PointT> &cloud_out,
340  const Eigen::Matrix<Scalar, 4, 4> &transform,
341  bool copy_all_fields = true);
342 
343 
344  template <typename PointT> void
346  const Indices &indices,
347  pcl::PointCloud<PointT> &cloud_out,
348  const Eigen::Matrix4f &transform,
349  bool copy_all_fields = true)
350  {
351  return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
352  }
353 
354  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
355  * \param[in] cloud_in the input point cloud
356  * \param[in] indices the set of point indices to use from the input point cloud
357  * \param[out] cloud_out the resultant output point cloud
358  * \param[in] transform an affine transformation (typically a rigid transformation)
359  * \param[in] copy_all_fields flag that controls whether the contents of the fields
360  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
361  * transformed cloud
362  * \note Can be used with cloud_in equal to cloud_out
363  * \ingroup common
364  */
365  template <typename PointT, typename Scalar> void
367  const pcl::PointIndices &indices,
368  pcl::PointCloud<PointT> &cloud_out,
369  const Eigen::Matrix<Scalar, 4, 4> &transform,
370  bool copy_all_fields = true)
371  {
372  return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
373  }
374 
375 
376  template <typename PointT> void
378  const pcl::PointIndices &indices,
379  pcl::PointCloud<PointT> &cloud_out,
380  const Eigen::Matrix4f &transform,
381  bool copy_all_fields = true)
382  {
383  return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
384  }
385 
386  /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
387  * \param[in] cloud_in the input point cloud
388  * \param[out] cloud_out the resultant output point cloud
389  * \param[in] offset the translation component of the rigid transformation
390  * \param[in] rotation the rotation component of the rigid transformation
391  * \param[in] copy_all_fields flag that controls whether the contents of the fields
392  * (other than x, y, z) should be copied into the new transformed cloud
393  * \ingroup common
394  */
395  template <typename PointT, typename Scalar> inline void
397  pcl::PointCloud<PointT> &cloud_out,
398  const Eigen::Matrix<Scalar, 3, 1> &offset,
399  const Eigen::Quaternion<Scalar> &rotation,
400  bool copy_all_fields = true);
401 
402  template <typename PointT> inline void
404  pcl::PointCloud<PointT> &cloud_out,
405  const Eigen::Vector3f &offset,
406  const Eigen::Quaternionf &rotation,
407  bool copy_all_fields = true)
408  {
409  return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation, copy_all_fields));
410  }
411 
412  /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
413  * \param[in] cloud_in the input point cloud
414  * \param[out] cloud_out the resultant output point cloud
415  * \param[in] offset the translation component of the rigid transformation
416  * \param[in] rotation the rotation component of the rigid transformation
417  * \param[in] copy_all_fields flag that controls whether the contents of the fields
418  * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
419  * transformed cloud
420  * \ingroup common
421  */
422  template <typename PointT, typename Scalar> inline void
424  pcl::PointCloud<PointT> &cloud_out,
425  const Eigen::Matrix<Scalar, 3, 1> &offset,
426  const Eigen::Quaternion<Scalar> &rotation,
427  bool copy_all_fields = true);
428 
429  template <typename PointT> void
431  pcl::PointCloud<PointT> &cloud_out,
432  const Eigen::Vector3f &offset,
433  const Eigen::Quaternionf &rotation,
434  bool copy_all_fields = true)
435  {
436  return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation, copy_all_fields));
437  }
438 
439  /** \brief Apply an affine transform on a pointcloud having points of type PointXY
440  * \param[in] cloud_in the input point cloud
441  * \param[out] cloud_out the resultant output point cloud
442  * \param[in] transform an affine transformation
443  * \param[in] copy_all_fields flag that controls whether the contents of the fields
444  * (other than x, y, z) should be copied into the new transformed cloud
445  * \ingroup common
446  */
447  void
449  pcl::PointCloud<pcl::PointXY>& cloud_out,
450  const Eigen::Affine2f& transform,
451  bool copy_all_fields = true);
452 
453  /** \brief Transform a point with members x,y,z
454  * \param[in] point the point to transform
455  * \param[out] transform the transformation to apply
456  * \return the transformed point
457  * \ingroup common
458  */
459  template <typename PointT, typename Scalar> inline PointT
460  transformPoint (const PointT &point,
461  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
462 
463  template <typename PointT> inline PointT
464  transformPoint (const PointT &point,
465  const Eigen::Affine3f &transform)
466  {
467  return (transformPoint<PointT, float> (point, transform));
468  }
469 
470  /** \brief Transform a point with members x,y,z,normal_x,normal_y,normal_z
471  * \param[in] point the point to transform
472  * \param[out] transform the transformation to apply
473  * \return the transformed point
474  * \ingroup common
475  */
476  template <typename PointT, typename Scalar> inline PointT
477  transformPointWithNormal (const PointT &point,
478  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
479 
480  template <typename PointT> inline PointT
482  const Eigen::Affine3f &transform)
483  {
484  return (transformPointWithNormal<PointT, float> (point, transform));
485  }
486 
487  /** \brief Calculates the principal (PCA-based) alignment of the point cloud
488  * \param[in] cloud the input point cloud
489  * \param[out] transform the resultant transform
490  * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1.
491  * \note If the return value is close to one then the transformation might be not unique -> two principal directions have
492  * almost same variance (extend)
493  */
494  template <typename PointT, typename Scalar> inline double
496  Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
497 
498  template <typename PointT> inline double
500  Eigen::Affine3f &transform)
501  {
502  return (getPrincipalTransformation<PointT, float> (cloud, transform));
503  }
504 }
505 
506 #include <pcl/common/impl/transforms.hpp>
Define methods for centroid estimation and covariance matrix calculus.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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
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 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.