Point Cloud Library (PCL)  1.14.1-dev
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 
42 namespace pcl {
43 
44 namespace registration {
45 
46 template <typename PointSource, typename PointTarget, typename Scalar>
47 inline 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 
68 template <typename PointSource, typename PointTarget, typename Scalar>
69 void
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 
91 template <typename PointSource, typename PointTarget, typename Scalar>
92 inline 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 
115 template <typename PointSource, typename PointTarget, typename Scalar>
116 inline 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 
128 template <typename PointSource, typename PointTarget, typename Scalar>
129 inline void
131  constructTransformationMatrix(const Vector6& parameters,
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 
149 template <typename PointSource, typename PointTarget, typename Scalar>
150 inline 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 
203 template <typename PointSource, typename PointTarget, typename Scalar>
204 inline void
206  setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
207 {
208  enforce_same_direction_normals_ = enforce_same_direction_normals;
209 }
210 
211 template <typename PointSource, typename PointTarget, typename Scalar>
212 inline 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