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