Point Cloud Library (PCL)  1.14.1-dev
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 namespace pcl {
47 
48 namespace registration {
49 
50 template <typename PointSource, typename PointTarget, typename Scalar>
51 inline void
52 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
53  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
54  const pcl::PointCloud<PointTarget>& cloud_tgt,
55  Matrix4& transformation_matrix) const
56 {
57  const auto nr_points = cloud_src.size();
58  if (cloud_tgt.size() != nr_points) {
59  PCL_ERROR(
60  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
61  "Number or points in source (%zu) differs than target (%zu)!\n",
62  static_cast<std::size_t>(nr_points),
63  static_cast<std::size_t>(cloud_tgt.size()));
64  return;
65  }
66 
67  ConstCloudIterator<PointSource> source_it(cloud_src);
68  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
69  estimateRigidTransformation(source_it, target_it, transformation_matrix);
70 }
71 
72 template <typename PointSource, typename PointTarget, typename Scalar>
73 void
74 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
75  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
76  const pcl::Indices& indices_src,
77  const pcl::PointCloud<PointTarget>& cloud_tgt,
78  Matrix4& transformation_matrix) const
79 {
80  const auto nr_points = indices_src.size();
81  if (cloud_tgt.size() != nr_points) {
82  PCL_ERROR(
83  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
84  "Number or points in source (%zu) differs than target (%zu)!\n",
85  indices_src.size(),
86  static_cast<std::size_t>(cloud_tgt.size()));
87  return;
88  }
89 
90  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
91  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
92  estimateRigidTransformation(source_it, target_it, transformation_matrix);
93 }
94 
95 template <typename PointSource, typename PointTarget, typename Scalar>
96 inline void
97 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
98  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
99  const pcl::Indices& indices_src,
100  const pcl::PointCloud<PointTarget>& cloud_tgt,
101  const pcl::Indices& indices_tgt,
102  Matrix4& transformation_matrix) const
103 {
104  const auto nr_points = indices_src.size();
105  if (indices_tgt.size() != nr_points) {
106  PCL_ERROR(
107  "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
108  "Number or points in source (%zu) differs than target (%zu)!\n",
109  indices_src.size(),
110  indices_tgt.size());
111  return;
112  }
113 
114  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
115  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
116  estimateRigidTransformation(source_it, target_it, transformation_matrix);
117 }
118 
119 template <typename PointSource, typename PointTarget, typename Scalar>
120 inline void
121 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
122  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
123  const pcl::PointCloud<PointTarget>& cloud_tgt,
124  const pcl::Correspondences& correspondences,
125  Matrix4& transformation_matrix) const
126 {
127  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
128  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
129  estimateRigidTransformation(source_it, target_it, transformation_matrix);
130 }
131 
132 template <typename PointSource, typename PointTarget, typename Scalar>
133 inline void
134 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
135  constructTransformationMatrix(const double& alpha,
136  const double& beta,
137  const double& gamma,
138  const double& tx,
139  const double& ty,
140  const double& tz,
141  Matrix4& transformation_matrix) const
142 {
143  // Construct the transformation matrix from rotation and translation
144  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero();
145  transformation_matrix(0, 0) = static_cast<Scalar>(std::cos(gamma) * std::cos(beta));
146  transformation_matrix(0, 1) = static_cast<Scalar>(
147  -sin(gamma) * std::cos(alpha) + std::cos(gamma) * sin(beta) * sin(alpha));
148  transformation_matrix(0, 2) = static_cast<Scalar>(
149  sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
150  transformation_matrix(1, 0) = static_cast<Scalar>(sin(gamma) * std::cos(beta));
151  transformation_matrix(1, 1) = static_cast<Scalar>(
152  std::cos(gamma) * std::cos(alpha) + sin(gamma) * sin(beta) * sin(alpha));
153  transformation_matrix(1, 2) = static_cast<Scalar>(
154  -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
155  transformation_matrix(2, 0) = static_cast<Scalar>(-sin(beta));
156  transformation_matrix(2, 1) = static_cast<Scalar>(std::cos(beta) * sin(alpha));
157  transformation_matrix(2, 2) = static_cast<Scalar>(std::cos(beta) * std::cos(alpha));
158 
159  transformation_matrix(0, 3) = static_cast<Scalar>(tx);
160  transformation_matrix(1, 3) = static_cast<Scalar>(ty);
161  transformation_matrix(2, 3) = static_cast<Scalar>(tz);
162  transformation_matrix(3, 3) = static_cast<Scalar>(1);
163 }
164 
165 template <typename PointSource, typename PointTarget, typename Scalar>
166 inline void
167 TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
168  estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
169  ConstCloudIterator<PointTarget>& target_it,
170  Matrix4& transformation_matrix) const
171 {
172  using Vector6d = Eigen::Matrix<double, 6, 1>;
173  using Matrix6d = Eigen::Matrix<double, 6, 6>;
174 
175  Matrix6d ATA;
176  Vector6d ATb;
177  ATA.setZero();
178  ATb.setZero();
179 
180  // Approximate as a linear least squares problem
181  while (source_it.isValid() && target_it.isValid()) {
182  if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
183  !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
184  !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
185  !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
186  !std::isfinite(target_it->normal_z)) {
187  ++target_it;
188  ++source_it;
189  continue;
190  }
191 
192  const float& sx = source_it->x;
193  const float& sy = source_it->y;
194  const float& sz = source_it->z;
195  const float& dx = target_it->x;
196  const float& dy = target_it->y;
197  const float& dz = target_it->z;
198  const float& nx = target_it->normal[0];
199  const float& ny = target_it->normal[1];
200  const float& nz = target_it->normal[2];
201 
202  double a = nz * sy - ny * sz;
203  double b = nx * sz - nz * sx;
204  double c = ny * sx - nx * sy;
205 
206  // 0 1 2 3 4 5
207  // 6 7 8 9 10 11
208  // 12 13 14 15 16 17
209  // 18 19 20 21 22 23
210  // 24 25 26 27 28 29
211  // 30 31 32 33 34 35
212 
213  ATA.coeffRef(0) += a * a;
214  ATA.coeffRef(1) += a * b;
215  ATA.coeffRef(2) += a * c;
216  ATA.coeffRef(3) += a * nx;
217  ATA.coeffRef(4) += a * ny;
218  ATA.coeffRef(5) += a * nz;
219  ATA.coeffRef(7) += b * b;
220  ATA.coeffRef(8) += b * c;
221  ATA.coeffRef(9) += b * nx;
222  ATA.coeffRef(10) += b * ny;
223  ATA.coeffRef(11) += b * nz;
224  ATA.coeffRef(14) += c * c;
225  ATA.coeffRef(15) += c * nx;
226  ATA.coeffRef(16) += c * ny;
227  ATA.coeffRef(17) += c * nz;
228  ATA.coeffRef(21) += nx * nx;
229  ATA.coeffRef(22) += nx * ny;
230  ATA.coeffRef(23) += nx * nz;
231  ATA.coeffRef(28) += ny * ny;
232  ATA.coeffRef(29) += ny * nz;
233  ATA.coeffRef(35) += nz * nz;
234 
235  double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
236  ATb.coeffRef(0) += a * d;
237  ATb.coeffRef(1) += b * d;
238  ATb.coeffRef(2) += c * d;
239  ATb.coeffRef(3) += nx * d;
240  ATb.coeffRef(4) += ny * d;
241  ATb.coeffRef(5) += nz * d;
242 
243  ++target_it;
244  ++source_it;
245  }
246 
247  ATA.coeffRef(6) = ATA.coeff(1);
248  ATA.coeffRef(12) = ATA.coeff(2);
249  ATA.coeffRef(13) = ATA.coeff(8);
250  ATA.coeffRef(18) = ATA.coeff(3);
251  ATA.coeffRef(19) = ATA.coeff(9);
252  ATA.coeffRef(20) = ATA.coeff(15);
253  ATA.coeffRef(24) = ATA.coeff(4);
254  ATA.coeffRef(25) = ATA.coeff(10);
255  ATA.coeffRef(26) = ATA.coeff(16);
256  ATA.coeffRef(27) = ATA.coeff(22);
257  ATA.coeffRef(30) = ATA.coeff(5);
258  ATA.coeffRef(31) = ATA.coeff(11);
259  ATA.coeffRef(32) = ATA.coeff(17);
260  ATA.coeffRef(33) = ATA.coeff(23);
261  ATA.coeffRef(34) = ATA.coeff(29);
262 
263  // Solve A*x = b
264  Vector6d x = static_cast<Vector6d>(ATA.inverse() * ATb);
265 
266  // Construct the transformation matrix from x
267  constructTransformationMatrix(
268  x(0), x(1), x(2), x(3), x(4), x(5), transformation_matrix);
269 
270  if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) {
271  size_t N = 0;
272  double loss = 0.0;
273  source_it.reset();
274  target_it.reset();
275  while (source_it.isValid() && target_it.isValid()) {
276  if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
277  !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
278  !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
279  !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
280  !std::isfinite(target_it->normal_z)) {
281  ++target_it;
282  ++source_it;
283  continue;
284  }
285  const float& sx = source_it->x;
286  const float& sy = source_it->y;
287  const float& sz = source_it->z;
288  const float& dx = target_it->x;
289  const float& dy = target_it->y;
290  const float& dz = target_it->z;
291  const float& nx = target_it->normal[0];
292  const float& ny = target_it->normal[1];
293  const float& nz = target_it->normal[2];
294  double a = nz * sy - ny * sz;
295  double b = nx * sz - nz * sx;
296  double c = ny * sx - nx * sy;
297  double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
298  Vector6d Arow;
299  Arow << a, b, c, nx, ny, nz;
300  loss += pow(Arow.transpose() * x - d, 2);
301  ++target_it;
302  ++source_it;
303  ++N;
304  }
305  loss /= N;
306  PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPlaneLLS::"
307  "estimateRigidTransformation] Loss :%.10e\n",
308  loss);
309  }
310 }
311 
312 } // namespace registration
313 } // namespace pcl
314 
315 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition: cloud_iterator.h:121
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::registration::TransformationEstimationPointToPlaneLLS::Matrix4
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: transformation_estimation_point_to_plane_lls.h:72
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:135
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:53
pcl::console::isVerbosityLevelEnabled
PCL_EXPORTS bool isVerbosityLevelEnabled(VERBOSITY_LEVEL severity)
is verbosity level enabled?
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:89
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133