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