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