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