Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
45namespace pcl {
46
47namespace registration {
48
49template <typename PointSource, typename PointTarget, typename Scalar>
50inline void
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
79template <typename PointSource, typename PointTarget, typename Scalar>
80void
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
110template <typename PointSource, typename PointTarget, typename Scalar>
111inline void
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
142template <typename PointSource, typename PointTarget, typename Scalar>
143inline void
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
159template <typename PointSource, typename PointTarget, typename Scalar>
160inline void
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
192template <typename PointSource, typename PointTarget, typename Scalar>
193inline void
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 */
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
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
Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133