Point Cloud Library (PCL)  1.11.1
transformation_estimation_point_to_plane_lls.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  * $Id$
38  *
39  */
40 
41 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
42 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
43 
44 #include <pcl/cloud_iterator.h>
45 
46 
47 namespace pcl
48 {
49 
50 namespace registration
51 {
52 
53 template <typename PointSource, typename PointTarget, typename Scalar> inline void
56  const pcl::PointCloud<PointTarget> &cloud_tgt,
57  Matrix4 &transformation_matrix) const
58 {
59  const auto nr_points = cloud_src.size ();
60  if (cloud_tgt.size () != nr_points)
61  {
62  PCL_ERROR(
63  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
64  "Number or points in source (%zu) differs than target (%zu)!\n",
65  static_cast<std::size_t>(nr_points),
66  static_cast<std::size_t>(cloud_tgt.size()));
67  return;
68  }
69 
70  ConstCloudIterator<PointSource> source_it (cloud_src);
71  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
72  estimateRigidTransformation (source_it, target_it, transformation_matrix);
73 }
74 
75 
76 template <typename PointSource, typename PointTarget, typename Scalar> void
79  const std::vector<int> &indices_src,
80  const pcl::PointCloud<PointTarget> &cloud_tgt,
81  Matrix4 &transformation_matrix) const
82 {
83  const auto nr_points = indices_src.size ();
84  if (cloud_tgt.size () != nr_points)
85  {
86  PCL_ERROR(
87  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
88  "Number or points in source (%zu) differs than target (%zu)!\n",
89  indices_src.size(),
90  static_cast<std::size_t>(cloud_tgt.size()));
91  return;
92  }
93 
94  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
95  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
96  estimateRigidTransformation (source_it, target_it, transformation_matrix);
97 }
98 
99 
100 template <typename PointSource, typename PointTarget, typename Scalar> inline void
103  const std::vector<int> &indices_src,
104  const pcl::PointCloud<PointTarget> &cloud_tgt,
105  const std::vector<int> &indices_tgt,
106  Matrix4 &transformation_matrix) const
107 {
108  const auto nr_points = indices_src.size ();
109  if (indices_tgt.size () != nr_points)
110  {
111  PCL_ERROR(
112  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
113  "Number or points in source (%zu) differs than target (%zu)!\n",
114  indices_src.size(),
115  indices_tgt.size());
116  return;
117  }
118 
119  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
120  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
121  estimateRigidTransformation (source_it, target_it, transformation_matrix);
122 }
123 
124 
125 template <typename PointSource, typename PointTarget, typename Scalar> inline void
128  const pcl::PointCloud<PointTarget> &cloud_tgt,
129  const pcl::Correspondences &correspondences,
130  Matrix4 &transformation_matrix) const
131 {
132  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
133  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
134  estimateRigidTransformation (source_it, target_it, transformation_matrix);
135 }
136 
137 
138 template <typename PointSource, typename PointTarget, typename Scalar> inline void
140 constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
141  const double & tx, const double & ty, const double & tz,
142  Matrix4 &transformation_matrix) const
143 {
144  // Construct the transformation matrix from rotation and translation
145  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
146  transformation_matrix (0, 0) = static_cast<Scalar> ( std::cos (gamma) * std::cos (beta));
147  transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha));
148  transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha));
149  transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * std::cos (beta));
150  transformation_matrix (1, 1) = static_cast<Scalar> ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
151  transformation_matrix (1, 2) = static_cast<Scalar> (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha));
152  transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
153  transformation_matrix (2, 1) = static_cast<Scalar> ( std::cos (beta) * sin (alpha));
154  transformation_matrix (2, 2) = static_cast<Scalar> ( std::cos (beta) * std::cos (alpha));
155 
156  transformation_matrix (0, 3) = static_cast<Scalar> (tx);
157  transformation_matrix (1, 3) = static_cast<Scalar> (ty);
158  transformation_matrix (2, 3) = static_cast<Scalar> (tz);
159  transformation_matrix (3, 3) = static_cast<Scalar> (1);
160 }
161 
162 
163 template <typename PointSource, typename PointTarget, typename Scalar> inline void
165 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
166 {
167  using Vector6d = Eigen::Matrix<double, 6, 1>;
168  using Matrix6d = Eigen::Matrix<double, 6, 6>;
169 
170  Matrix6d ATA;
171  Vector6d ATb;
172  ATA.setZero ();
173  ATb.setZero ();
174 
175  // Approximate as a linear least squares problem
176  while (source_it.isValid () && target_it.isValid ())
177  {
178  if (!std::isfinite (source_it->x) ||
179  !std::isfinite (source_it->y) ||
180  !std::isfinite (source_it->z) ||
181  !std::isfinite (target_it->x) ||
182  !std::isfinite (target_it->y) ||
183  !std::isfinite (target_it->z) ||
184  !std::isfinite (target_it->normal_x) ||
185  !std::isfinite (target_it->normal_y) ||
186  !std::isfinite (target_it->normal_z))
187  {
188  ++target_it;
189  ++source_it;
190  continue;
191  }
192 
193  const float & sx = source_it->x;
194  const float & sy = source_it->y;
195  const float & sz = source_it->z;
196  const float & dx = target_it->x;
197  const float & dy = target_it->y;
198  const float & dz = target_it->z;
199  const float & nx = target_it->normal[0];
200  const float & ny = target_it->normal[1];
201  const float & nz = target_it->normal[2];
202 
203  double a = nz*sy - ny*sz;
204  double b = nx*sz - nz*sx;
205  double c = ny*sx - nx*sy;
206 
207  // 0 1 2 3 4 5
208  // 6 7 8 9 10 11
209  // 12 13 14 15 16 17
210  // 18 19 20 21 22 23
211  // 24 25 26 27 28 29
212  // 30 31 32 33 34 35
213 
214  ATA.coeffRef (0) += a * a;
215  ATA.coeffRef (1) += a * b;
216  ATA.coeffRef (2) += a * c;
217  ATA.coeffRef (3) += a * nx;
218  ATA.coeffRef (4) += a * ny;
219  ATA.coeffRef (5) += a * nz;
220  ATA.coeffRef (7) += b * b;
221  ATA.coeffRef (8) += b * c;
222  ATA.coeffRef (9) += b * nx;
223  ATA.coeffRef (10) += b * ny;
224  ATA.coeffRef (11) += b * nz;
225  ATA.coeffRef (14) += c * c;
226  ATA.coeffRef (15) += c * nx;
227  ATA.coeffRef (16) += c * ny;
228  ATA.coeffRef (17) += c * nz;
229  ATA.coeffRef (21) += nx * nx;
230  ATA.coeffRef (22) += nx * ny;
231  ATA.coeffRef (23) += nx * nz;
232  ATA.coeffRef (28) += ny * ny;
233  ATA.coeffRef (29) += ny * nz;
234  ATA.coeffRef (35) += nz * nz;
235 
236  double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
237  ATb.coeffRef (0) += a * d;
238  ATb.coeffRef (1) += b * d;
239  ATb.coeffRef (2) += c * d;
240  ATb.coeffRef (3) += nx * d;
241  ATb.coeffRef (4) += ny * d;
242  ATb.coeffRef (5) += nz * d;
243 
244  ++target_it;
245  ++source_it;
246  }
247 
248  ATA.coeffRef (6) = ATA.coeff (1);
249  ATA.coeffRef (12) = ATA.coeff (2);
250  ATA.coeffRef (13) = ATA.coeff (8);
251  ATA.coeffRef (18) = ATA.coeff (3);
252  ATA.coeffRef (19) = ATA.coeff (9);
253  ATA.coeffRef (20) = ATA.coeff (15);
254  ATA.coeffRef (24) = ATA.coeff (4);
255  ATA.coeffRef (25) = ATA.coeff (10);
256  ATA.coeffRef (26) = ATA.coeff (16);
257  ATA.coeffRef (27) = ATA.coeff (22);
258  ATA.coeffRef (30) = ATA.coeff (5);
259  ATA.coeffRef (31) = ATA.coeff (11);
260  ATA.coeffRef (32) = ATA.coeff (17);
261  ATA.coeffRef (33) = ATA.coeff (23);
262  ATA.coeffRef (34) = ATA.coeff (29);
263 
264  // Solve A*x = b
265  Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
266 
267  // Construct the transformation matrix from x
268  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
269 }
270 
271 } // namespace registration
272 } // namespace pcl
273 
274 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
275 
Iterator class for point clouds with or without given indices.
std::size_t size() const
Definition: point_cloud.h:459
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void constructTransformationMatrix(const double &alpha, const double &beta, const double &gamma, const double &tx, const double &ty, const double &tz, Matrix4 &transformation_matrix) const
Construct a 4 by 4 transformation matrix from the provided rotation and translation.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const override
Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences