Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
transformation_estimation_point_to_plane_lls.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  */
38 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
39 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
40 
42 template <typename PointSource, typename PointTarget> inline void
43 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
44 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
45  const pcl::PointCloud<PointTarget> &cloud_tgt,
46  Eigen::Matrix4f &transformation_matrix)
47 {
48  size_t nr_points = cloud_src.points.size ();
49  if (cloud_tgt.points.size () != nr_points)
50  {
51  PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ());
52  return;
53  }
54 
55  // Approximate as a linear least squares problem
56  Eigen::MatrixXf A (nr_points, 6);
57  Eigen::MatrixXf b (nr_points, 1);
58  for (size_t i = 0; i < nr_points; ++i)
59  {
60  const float & sx = cloud_src.points[i].x;
61  const float & sy = cloud_src.points[i].y;
62  const float & sz = cloud_src.points[i].z;
63  const float & dx = cloud_tgt.points[i].x;
64  const float & dy = cloud_tgt.points[i].y;
65  const float & dz = cloud_tgt.points[i].z;
66  const float & nx = cloud_tgt.points[i].normal[0];
67  const float & ny = cloud_tgt.points[i].normal[1];
68  const float & nz = cloud_tgt.points[i].normal[2];
69  A (i, 0) = nz*sy - ny*sz;
70  A (i, 1) = nx*sz - nz*sx;
71  A (i, 2) = ny*sx - nx*sy;
72  A (i, 3) = nx;
73  A (i, 4) = ny;
74  A (i, 5) = nz;
75  b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
76  }
77 
78  // Solve A*x = b
79  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
80 
81  // Construct the transformation matrix from x
82  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
83 
84 }
85 
87 template <typename PointSource, typename PointTarget> void
88 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
89 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
90  const std::vector<int> &indices_src,
91  const pcl::PointCloud<PointTarget> &cloud_tgt,
92  Eigen::Matrix4f &transformation_matrix)
93 {
94  size_t nr_points = indices_src.size ();
95  if (cloud_tgt.points.size () != nr_points)
96  {
97  PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
98  return;
99  }
100 
101  // Approximate as a linear least squares problem
102  Eigen::MatrixXf A (nr_points, 6);
103  Eigen::MatrixXf b (nr_points, 1);
104  for (size_t i = 0; i < nr_points; ++i)
105  {
106  const float & sx = cloud_src.points[indices_src[i]].x;
107  const float & sy = cloud_src.points[indices_src[i]].y;
108  const float & sz = cloud_src.points[indices_src[i]].z;
109  const float & dx = cloud_tgt.points[i].x;
110  const float & dy = cloud_tgt.points[i].y;
111  const float & dz = cloud_tgt.points[i].z;
112  const float & nx = cloud_tgt.points[i].normal[0];
113  const float & ny = cloud_tgt.points[i].normal[1];
114  const float & nz = cloud_tgt.points[i].normal[2];
115  A (i, 0) = nz*sy - ny*sz;
116  A (i, 1) = nx*sz - nz*sx;
117  A (i, 2) = ny*sx - nx*sy;
118  A (i, 3) = nx;
119  A (i, 4) = ny;
120  A (i, 5) = nz;
121  b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
122  }
123 
124  // Solve A*x = b
125  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
126 
127  // Construct the transformation matrix from x
128  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
129 }
130 
131 
133 template <typename PointSource, typename PointTarget> inline void
134 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
135 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
136  const std::vector<int> &indices_src,
137  const pcl::PointCloud<PointTarget> &cloud_tgt,
138  const std::vector<int> &indices_tgt,
139  Eigen::Matrix4f &transformation_matrix)
140 {
141  size_t nr_points = indices_src.size ();
142  if (indices_tgt.size () != nr_points)
143  {
144  PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
145  return;
146  }
147 
148  // Approximate as a linear least squares problem
149  Eigen::MatrixXf A (nr_points, 6);
150  Eigen::MatrixXf b (nr_points, 1);
151  for (size_t i = 0; i < nr_points; ++i)
152  {
153  const float & sx = cloud_src.points[indices_src[i]].x;
154  const float & sy = cloud_src.points[indices_src[i]].y;
155  const float & sz = cloud_src.points[indices_src[i]].z;
156  const float & dx = cloud_tgt.points[indices_tgt[i]].x;
157  const float & dy = cloud_tgt.points[indices_tgt[i]].y;
158  const float & dz = cloud_tgt.points[indices_tgt[i]].z;
159  const float & nx = cloud_tgt.points[indices_tgt[i]].normal[0];
160  const float & ny = cloud_tgt.points[indices_tgt[i]].normal[1];
161  const float & nz = cloud_tgt.points[indices_tgt[i]].normal[2];
162  A (i, 0) = nz*sy - ny*sz;
163  A (i, 1) = nx*sz - nz*sx;
164  A (i, 2) = ny*sx - nx*sy;
165  A (i, 3) = nx;
166  A (i, 4) = ny;
167  A (i, 5) = nz;
168  b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
169  }
170 
171  // Solve A*x = b
172  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
173 
174  // Construct the transformation matrix from x
175  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
176 }
177 
179 template <typename PointSource, typename PointTarget> inline void
180 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
181 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
182  const pcl::PointCloud<PointTarget> &cloud_tgt,
183  const pcl::Correspondences &correspondences,
184  Eigen::Matrix4f &transformation_matrix)
185 {
186  size_t nr_points = correspondences.size ();
187 
188  // Approximate as a linear least squares problem
189  Eigen::MatrixXf A (nr_points, 6);
190  Eigen::MatrixXf b (nr_points, 1);
191  for (size_t i = 0; i < nr_points; ++i)
192  {
193  const int & src_idx = correspondences[i].index_query;
194  const int & tgt_idx = correspondences[i].index_match;
195  const float & sx = cloud_src.points[src_idx].x;
196  const float & sy = cloud_src.points[src_idx].y;
197  const float & sz = cloud_src.points[src_idx].z;
198  const float & dx = cloud_tgt.points[tgt_idx].x;
199  const float & dy = cloud_tgt.points[tgt_idx].y;
200  const float & dz = cloud_tgt.points[tgt_idx].z;
201  const float & nx = cloud_tgt.points[tgt_idx].normal[0];
202  const float & ny = cloud_tgt.points[tgt_idx].normal[1];
203  const float & nz = cloud_tgt.points[tgt_idx].normal[2];
204  A (i, 0) = nz*sy - ny*sz;
205  A (i, 1) = nx*sz - nz*sx;
206  A (i, 2) = ny*sx - nx*sy;
207  A (i, 3) = nx;
208  A (i, 4) = ny;
209  A (i, 5) = nz;
210  b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
211  }
212 
213  // Solve A*x = b
214  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
215 
216  // Construct the transformation matrix from x
217  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
218 }
219 
220 template <typename PointSource, typename PointTarget> inline void
221 pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
222 constructTransformationMatrix (const float & alpha, const float & beta, const float & gamma,
223  const float & tx, const float & ty, const float & tz,
224  Eigen::Matrix4f &transformation_matrix)
225 {
226 
227  // Construct the transformation matrix from rotation and translation
228  transformation_matrix = Eigen::Matrix4f::Zero ();
229  transformation_matrix (0, 0) = cosf (gamma) * cosf (beta);
230  transformation_matrix (0, 1) = -sinf (gamma) * cosf (alpha) + cosf (gamma) * sinf (beta) * sinf (alpha);
231  transformation_matrix (0, 2) = sinf (gamma) * sinf (alpha) + cosf (gamma) * sinf (beta) * cosf (alpha);
232  transformation_matrix (1, 0) = sinf (gamma) * cosf (beta);
233  transformation_matrix (1, 1) = cosf (gamma) * cosf (alpha) + sinf (gamma) * sinf (beta) * sinf (alpha);
234  transformation_matrix (1, 2) = -cosf (gamma) * sinf (alpha) + sinf (gamma) * sinf (beta) * cosf (alpha);
235  transformation_matrix (2, 0) = -sinf (beta);
236  transformation_matrix (2, 1) = cosf (beta) * sinf (alpha);
237  transformation_matrix (2, 2) = cosf (beta) * cosf (alpha);
238 
239  transformation_matrix (0, 3) = tx;
240  transformation_matrix (1, 3) = ty;
241  transformation_matrix (2, 3) = tz;
242  transformation_matrix (3, 3) = 1;
243 }
244 
245 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
Generated on Sat Aug 24 2013 17:28:36 for Point Cloud Library (PCL) by   doxygen 1.8.4