Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
46namespace pcl {
47
48namespace registration {
49
50template <typename PointSource, typename PointTarget, typename Scalar>
51inline void
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
72template <typename PointSource, typename PointTarget, typename Scalar>
73void
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
95template <typename PointSource, typename PointTarget, typename Scalar>
96inline void
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
119template <typename PointSource, typename PointTarget, typename Scalar>
120inline void
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
132template <typename PointSource, typename PointTarget, typename Scalar>
133inline void
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
165template <typename PointSource, typename PointTarget, typename Scalar>
166inline void
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
271} // namespace registration
272} // namespace pcl
273
274#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t size() const
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
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.
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133