Point Cloud Library (PCL)  1.11.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 
42 
43 namespace pcl
44 {
45 
46 namespace registration
47 {
48 
49 template <typename PointSource, typename PointTarget, typename Scalar> inline void
52  const pcl::PointCloud<PointTarget> &cloud_tgt,
53  Matrix4 &transformation_matrix) const
54 {
55  const auto nr_points = cloud_src.points.size ();
56  if (cloud_tgt.points.size () != nr_points)
57  {
58  PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.points.size ());
59  return;
60  }
61 
62  ConstCloudIterator<PointSource> source_it (cloud_src);
63  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
64  estimateRigidTransformation (source_it, target_it, transformation_matrix);
65 }
66 
67 
68 template <typename PointSource, typename PointTarget, typename Scalar> void
71  const std::vector<int> &indices_src,
72  const pcl::PointCloud<PointTarget> &cloud_tgt,
73  Matrix4 &transformation_matrix) const
74 {
75  const auto nr_points = indices_src.size ();
76  if (cloud_tgt.points.size () != nr_points)
77  {
78  PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ());
79  return;
80  }
81 
82  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
83  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
84  estimateRigidTransformation (source_it, target_it, transformation_matrix);
85 }
86 
87 
88 template <typename PointSource, typename PointTarget, typename Scalar> inline void
91  const std::vector<int> &indices_src,
92  const pcl::PointCloud<PointTarget> &cloud_tgt,
93  const std::vector<int> &indices_tgt,
94  Matrix4 &transformation_matrix) const
95 {
96  const auto nr_points = indices_src.size ();
97  if (indices_tgt.size () != nr_points)
98  {
99  PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
100  return;
101  }
102 
103  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
104  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
105  estimateRigidTransformation (source_it, target_it, transformation_matrix);
106 }
107 
108 
109 template <typename PointSource, typename PointTarget, typename Scalar> inline void
112  const pcl::PointCloud<PointTarget> &cloud_tgt,
113  const pcl::Correspondences &correspondences,
114  Matrix4 &transformation_matrix) const
115 {
116  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
117  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
118  estimateRigidTransformation (source_it, target_it, transformation_matrix);
119 }
120 
121 
122 template <typename PointSource, typename PointTarget, typename Scalar> inline void
124 constructTransformationMatrix (const Vector6 &parameters,
125  Matrix4 &transformation_matrix) const
126 {
127  // Construct the transformation matrix from rotation and translation
128  const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
129  const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
130  const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
131  const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
132  const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
133  translation *
134  rotation_z * rotation_y * rotation_x;
135  transformation_matrix = transform.matrix ();
136 }
137 
138 
139 template <typename PointSource, typename PointTarget, typename Scalar> inline void
141 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
142 {
143  using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
144  using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
145 
146  Matrix6 ATA;
147  Vector6 ATb;
148  ATA.setZero ();
149  ATb.setZero ();
150  auto M = ATA.template selfadjointView<Eigen::Upper> ();
151 
152  // Approximate as a linear least squares problem
153  source_it.reset ();
154  target_it.reset ();
155  for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it)
156  {
157  const Vector3 p (source_it->x, source_it->y, source_it->z);
158  const Vector3 q (target_it->x, target_it->y, target_it->z);
159  const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
160  const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
161  Vector3 n;
162  if (enforce_same_direction_normals_)
163  {
164  if (n1.dot (n2) >= 0.)
165  n = n1 + n2;
166  else
167  n = n1 - n2;
168  }
169  else
170  {
171  n = n1 + n2;
172  }
173 
174  if (!p.array().isFinite().all() ||
175  !q.array().isFinite().all() ||
176  !n.array().isFinite().all())
177  {
178  continue;
179  }
180 
181  Vector6 v;
182  v << (p + q).cross (n), n;
183  M.rankUpdate (v);
184 
185  ATb += v * (q - p).dot (n);
186  }
187 
188  // Solve A*x = b
189  const Vector6 x = M.ldlt ().solve (ATb);
190 
191  // Construct the transformation matrix from x
192  constructTransformationMatrix (x, transformation_matrix);
193 }
194 
195 
196 template <typename PointSource, typename PointTarget, typename Scalar> inline void
198 setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
199 {
200  enforce_same_direction_normals_ = enforce_same_direction_normals;
201 }
202 
203 
204 template <typename PointSource, typename PointTarget, typename Scalar> inline bool
207 {
208  return enforce_same_direction_normals_;
209 }
210 
211 } // namespace registration
212 } // namespace pcl
213 
Iterator class for point clouds with or without given indices.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &indices_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