Point Cloud Library (PCL)  1.7.1
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 {
46  if (&cloud_in != &cloud_out)
47  {
48  // Note: could be replaced by cloud_out = cloud_in
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_out.points.size ());
54  cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
55  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
56  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
57  }
58 
59  if (cloud_in.is_dense)
60  {
61  // If the dataset is dense, simply transform it!
62  for (size_t i = 0; i < cloud_out.points.size (); ++i)
63  {
64  //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
65  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
66  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));
67  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));
68  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));
69  }
70  }
71  else
72  {
73  // Dataset might contain NaNs and Infs, so check for them first,
74  // otherwise we get errors during the multiplication (?)
75  for (size_t i = 0; i < cloud_out.points.size (); ++i)
76  {
77  if (!pcl_isfinite (cloud_in.points[i].x) ||
78  !pcl_isfinite (cloud_in.points[i].y) ||
79  !pcl_isfinite (cloud_in.points[i].z))
80  continue;
81  //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
82  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
83  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));
84  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));
85  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));
86  }
87  }
88 }
89 
90 ///////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointT, typename Scalar> void
93  const std::vector<int> &indices,
94  pcl::PointCloud<PointT> &cloud_out,
95  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
96 {
97  size_t npts = indices.size ();
98  // In order to transform the data, we need to remove NaNs
99  cloud_out.is_dense = cloud_in.is_dense;
100  cloud_out.header = cloud_in.header;
101  cloud_out.width = static_cast<int> (npts);
102  cloud_out.height = 1;
103  cloud_out.points.resize (npts);
104  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
105  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
106 
107  if (cloud_in.is_dense)
108  {
109  // If the dataset is dense, simply transform it!
110  for (size_t i = 0; i < npts; ++i)
111  {
112  // Copy fields first, then transform xyz data
113  //cloud_out.points[i] = cloud_in.points[indices[i]];
114  //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
115  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
116  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));
117  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));
118  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));
119  }
120  }
121  else
122  {
123  // Dataset might contain NaNs and Infs, so check for them first,
124  // otherwise we get errors during the multiplication (?)
125  for (size_t i = 0; i < npts; ++i)
126  {
127  if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
128  !pcl_isfinite (cloud_in.points[indices[i]].y) ||
129  !pcl_isfinite (cloud_in.points[indices[i]].z))
130  continue;
131  //cloud_out.points[i] = cloud_in.points[indices[i]];
132  //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
133  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
134  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));
135  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));
136  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));
137  }
138  }
139 }
140 
141 ///////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointT, typename Scalar> void
144  pcl::PointCloud<PointT> &cloud_out,
145  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
146 {
147  if (&cloud_in != &cloud_out)
148  {
149  // Note: could be replaced by cloud_out = cloud_in
150  cloud_out.header = cloud_in.header;
151  cloud_out.width = cloud_in.width;
152  cloud_out.height = cloud_in.height;
153  cloud_out.is_dense = cloud_in.is_dense;
154  cloud_out.points.reserve (cloud_out.points.size ());
155  cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
156  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
157  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
158  }
159 
160  // If the data is dense, we don't need to check for NaN
161  if (cloud_in.is_dense)
162  {
163  for (size_t i = 0; i < cloud_out.points.size (); ++i)
164  {
165  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
166  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
167  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));
168  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));
169  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));
170 
171  // Rotate normals (WARNING: transform.rotation () uses SVD internally!)
172  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
173  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
174  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));
175  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));
176  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));
177  }
178  }
179  // Dataset might contain NaNs and Infs, so check for them first.
180  else
181  {
182  for (size_t i = 0; i < cloud_out.points.size (); ++i)
183  {
184  if (!pcl_isfinite (cloud_in.points[i].x) ||
185  !pcl_isfinite (cloud_in.points[i].y) ||
186  !pcl_isfinite (cloud_in.points[i].z))
187  continue;
188 
189  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
190  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
191  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));
192  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));
193  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));
194 
195  // Rotate normals
196  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
197  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
198  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));
199  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));
200  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));
201  }
202  }
203 }
204 
205 ///////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointT, typename Scalar> void
208  const std::vector<int> &indices,
209  pcl::PointCloud<PointT> &cloud_out,
210  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
211 {
212  size_t npts = indices.size ();
213  // In order to transform the data, we need to remove NaNs
214  cloud_out.is_dense = cloud_in.is_dense;
215  cloud_out.header = cloud_in.header;
216  cloud_out.width = static_cast<int> (npts);
217  cloud_out.height = 1;
218  cloud_out.points.resize (npts);
219  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
220  cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
221 
222  // If the data is dense, we don't need to check for NaN
223  if (cloud_in.is_dense)
224  {
225  for (size_t i = 0; i < cloud_out.points.size (); ++i)
226  {
227  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
228  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
229  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));
230  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));
231  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));
232 
233  // Rotate normals
234  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
235  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
236  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));
237  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));
238  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));
239  }
240  }
241  // Dataset might contain NaNs and Infs, so check for them first.
242  else
243  {
244  for (size_t i = 0; i < cloud_out.points.size (); ++i)
245  {
246  if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
247  !pcl_isfinite (cloud_in.points[indices[i]].y) ||
248  !pcl_isfinite (cloud_in.points[indices[i]].z))
249  continue;
250 
251  //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
252  Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
253  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));
254  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));
255  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));
256 
257  // Rotate normals
258  //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
259  Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
260  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));
261  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));
262  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));
263  }
264  }
265 }
266 
267 ///////////////////////////////////////////////////////////////////////////////////////////
268 template <typename PointT, typename Scalar> inline void
270  pcl::PointCloud<PointT> &cloud_out,
271  const Eigen::Matrix<Scalar, 3, 1> &offset,
272  const Eigen::Quaternion<Scalar> &rotation)
273 {
274  Eigen::Translation<Scalar, 3> translation (offset);
275  // Assemble an Eigen Transform
276  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
277  transformPointCloud (cloud_in, cloud_out, t);
278 }
279 
280 ///////////////////////////////////////////////////////////////////////////////////////////
281 template <typename PointT, typename Scalar> inline void
283  pcl::PointCloud<PointT> &cloud_out,
284  const Eigen::Matrix<Scalar, 3, 1> &offset,
285  const Eigen::Quaternion<Scalar> &rotation)
286 {
287  Eigen::Translation<Scalar, 3> translation (offset);
288  // Assemble an Eigen Transform
289  Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
290  transformPointCloudWithNormals (cloud_in, cloud_out, t);
291 }
292 
293 ///////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointT, typename Scalar> inline PointT
296  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
297 {
298  PointT ret = point;
299  //ret.getVector3fMap () = transform * point.getVector3fMap ();
300  ret.x = static_cast<float> (transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3));
301  ret.y = static_cast<float> (transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3));
302  ret.z = static_cast<float> (transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3));
303 
304  return (ret);
305 }
306 
307 ///////////////////////////////////////////////////////////////////////////////////////////
308 template <typename PointT, typename Scalar> double
310  Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
311 {
312  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
313  Eigen::Matrix<Scalar, 4, 1> centroid;
314 
315  pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
316 
317  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
318  Eigen::Matrix<Scalar, 3, 1> eigen_vals;
319  pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
320 
321  double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
322  double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
323 
324  transform.translation () = centroid.head (3);
325  transform.linear () = eigen_vects;
326 
327  return (std::min (rel1, rel2));
328 }
329