Point Cloud Library (PCL) 1.13.0
transformation_estimation_symmetric_point_to_plane_lls.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2019-, 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 */
37
38#pragma once
39
40#include <pcl/cloud_iterator.h>
41
42namespace pcl {
43
44namespace registration {
45
46template <typename PointSource, typename PointTarget, typename Scalar>
47inline void
50 const pcl::PointCloud<PointTarget>& cloud_tgt,
51 Matrix4& transformation_matrix) const
52{
53 const auto nr_points = cloud_src.size();
54 if (cloud_tgt.size() != nr_points) {
55 PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
56 "estimateRigidTransformation] Number or points in source (%zu) differs "
57 "from target (%zu)!\n",
58 static_cast<std::size_t>(nr_points),
59 static_cast<std::size_t>(cloud_tgt.size()));
60 return;
61 }
62
63 ConstCloudIterator<PointSource> source_it(cloud_src);
64 ConstCloudIterator<PointTarget> target_it(cloud_tgt);
65 estimateRigidTransformation(source_it, target_it, transformation_matrix);
66}
67
68template <typename PointSource, typename PointTarget, typename Scalar>
69void
72 const pcl::Indices& indices_src,
73 const pcl::PointCloud<PointTarget>& cloud_tgt,
74 Matrix4& transformation_matrix) const
75{
76 const auto nr_points = indices_src.size();
77 if (cloud_tgt.size() != nr_points) {
78 PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
79 "estimateRigidTransformation] Number or points in source (%zu) differs "
80 "than target (%zu)!\n",
81 indices_src.size(),
82 static_cast<std::size_t>(cloud_tgt.size()));
83 return;
84 }
85
86 ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
87 ConstCloudIterator<PointTarget> target_it(cloud_tgt);
88 estimateRigidTransformation(source_it, target_it, transformation_matrix);
89}
90
91template <typename PointSource, typename PointTarget, typename Scalar>
92inline void
95 const pcl::Indices& indices_src,
96 const pcl::PointCloud<PointTarget>& cloud_tgt,
97 const pcl::Indices& indices_tgt,
98 Matrix4& transformation_matrix) const
99{
100 const auto nr_points = indices_src.size();
101 if (indices_tgt.size() != nr_points) {
102 PCL_ERROR("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::"
103 "estimateRigidTransformation] Number or points in source (%zu) differs "
104 "than target (%zu)!\n",
105 indices_src.size(),
106 indices_tgt.size());
107 return;
108 }
109
110 ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
111 ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
112 estimateRigidTransformation(source_it, target_it, transformation_matrix);
113}
114
115template <typename PointSource, typename PointTarget, typename Scalar>
116inline void
119 const pcl::PointCloud<PointTarget>& cloud_tgt,
120 const pcl::Correspondences& correspondences,
121 Matrix4& transformation_matrix) const
122{
123 ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
124 ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
125 estimateRigidTransformation(source_it, target_it, transformation_matrix);
126}
127
128template <typename PointSource, typename PointTarget, typename Scalar>
129inline void
132 Matrix4& transformation_matrix) const
133{
134 // Construct the transformation matrix from rotation and translation
135 const Eigen::AngleAxis<Scalar> rotation_z(parameters(2),
136 Eigen::Matrix<Scalar, 3, 1>::UnitZ());
137 const Eigen::AngleAxis<Scalar> rotation_y(parameters(1),
138 Eigen::Matrix<Scalar, 3, 1>::UnitY());
139 const Eigen::AngleAxis<Scalar> rotation_x(parameters(0),
140 Eigen::Matrix<Scalar, 3, 1>::UnitX());
141 const Eigen::Translation<Scalar, 3> translation(
142 parameters(3), parameters(4), parameters(5));
143 const Eigen::Transform<Scalar, 3, Eigen::Affine> transform =
144 rotation_z * rotation_y * rotation_x * translation * rotation_z * rotation_y *
145 rotation_x;
146 transformation_matrix = transform.matrix();
147}
148
149template <typename PointSource, typename PointTarget, typename Scalar>
150inline void
154 Matrix4& transformation_matrix) const
155{
156 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
157 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
158
159 Matrix6 ATA;
160 Vector6 ATb;
161 ATA.setZero();
162 ATb.setZero();
163 auto M = ATA.template selfadjointView<Eigen::Upper>();
164
165 // Approximate as a linear least squares problem
166 source_it.reset();
167 target_it.reset();
168 for (; source_it.isValid() && target_it.isValid(); ++source_it, ++target_it) {
169 const Vector3 p(source_it->x, source_it->y, source_it->z);
170 const Vector3 q(target_it->x, target_it->y, target_it->z);
171 const Vector3 n1(source_it->getNormalVector3fMap().template cast<Scalar>());
172 const Vector3 n2(target_it->getNormalVector3fMap().template cast<Scalar>());
173 Vector3 n;
174 if (enforce_same_direction_normals_) {
175 if (n1.dot(n2) >= 0.)
176 n = n1 + n2;
177 else
178 n = n1 - n2;
179 }
180 else {
181 n = n1 + n2;
182 }
183
184 if (!p.array().isFinite().all() || !q.array().isFinite().all() ||
185 !n.array().isFinite().all()) {
186 continue;
187 }
188
189 Vector6 v;
190 v << (p + q).cross(n), n;
191 M.rankUpdate(v);
192
193 ATb += v * (q - p).dot(n);
194 }
195
196 // Solve A*x = b
197 const Vector6 x = M.ldlt().solve(ATb);
198
199 // Construct the transformation matrix from x
200 constructTransformationMatrix(x, transformation_matrix);
201}
202
203template <typename PointSource, typename PointTarget, typename Scalar>
204inline void
206 setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
207{
208 enforce_same_direction_normals_ = enforce_same_direction_normals;
209}
210
211template <typename PointSource, typename PointTarget, typename Scalar>
212inline bool
215{
216 return enforce_same_direction_normals_;
217}
218
219} // namespace registration
220} // namespace pcl
Iterator class for point clouds with or without given indices.
std::size_t size() const
Definition: point_cloud.h:443
bool getEnforceSameDirectionNormals()
Obtain whether source or target normals are negated on a per-point basis such that they point in the ...
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.
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
Set whether or not to negate source or target normals on a per-point basis such that they point in th...
void constructTransformationMatrix(const Vector6 &parameters, Matrix4 &transformation_matrix) const
Construct a 4 by 4 transformation matrix from the provided rotation and translation.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133