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