Point Cloud Library (PCL)  1.14.1-dev
transformation_estimation_2D.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  */
37 
38 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
39 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
40 
41 namespace pcl {
42 
43 namespace registration {
44 
45 template <typename PointSource, typename PointTarget, typename Scalar>
46 inline void
49  const pcl::PointCloud<PointTarget>& cloud_tgt,
50  Matrix4& transformation_matrix) const
51 {
52  const auto nr_points = cloud_src.size();
53  if (cloud_tgt.size() != nr_points) {
54  PCL_ERROR("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
55  "or points in source (%zu) differs than target (%zu)!\n",
56  static_cast<std::size_t>(nr_points),
57  static_cast<std::size_t>(cloud_tgt.size()));
58  return;
59  }
60 
61  ConstCloudIterator<PointSource> source_it(cloud_src);
62  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
63  estimateRigidTransformation(source_it, target_it, transformation_matrix);
64 }
65 
66 template <typename PointSource, typename PointTarget, typename Scalar>
67 void
70  const pcl::Indices& indices_src,
71  const pcl::PointCloud<PointTarget>& cloud_tgt,
72  Matrix4& transformation_matrix) const
73 {
74  if (indices_src.size() != cloud_tgt.size()) {
75  PCL_ERROR("[pcl::Transformation2D::estimateRigidTransformation] Number or points "
76  "in source (%zu) differs than target (%zu)!\n",
77  indices_src.size(),
78  static_cast<std::size_t>(cloud_tgt.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 template <typename PointSource, typename PointTarget, typename Scalar>
88 inline void
91  const pcl::Indices& indices_src,
92  const pcl::PointCloud<PointTarget>& cloud_tgt,
93  const pcl::Indices& indices_tgt,
94  Matrix4& transformation_matrix) const
95 {
96  if (indices_src.size() != indices_tgt.size()) {
97  PCL_ERROR("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number "
98  "or points in source (%lu) differs than target (%lu)!\n",
99  indices_src.size(),
100  indices_tgt.size());
101  return;
102  }
103 
104  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
105  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
106  estimateRigidTransformation(source_it, target_it, transformation_matrix);
107 }
108 
109 template <typename PointSource, typename PointTarget, typename Scalar>
110 void
113  const pcl::PointCloud<PointTarget>& cloud_tgt,
114  const pcl::Correspondences& correspondences,
115  Matrix4& transformation_matrix) const
116 {
117  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
118  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
119  estimateRigidTransformation(source_it, target_it, transformation_matrix);
120 }
121 
122 template <typename PointSource, typename PointTarget, typename Scalar>
123 inline void
127  Matrix4& transformation_matrix) const
128 {
129  source_it.reset();
130  target_it.reset();
131 
132  Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
133  // Estimate the centroids of source, target
134  compute3DCentroid(source_it, centroid_src);
135  compute3DCentroid(target_it, centroid_tgt);
136  source_it.reset();
137  target_it.reset();
138 
139  // ignore z component
140  centroid_src[2] = 0.0f;
141  centroid_tgt[2] = 0.0f;
142  // Subtract the centroids from source, target
143  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
144  cloud_tgt_demean;
145  demeanPointCloud(source_it, centroid_src, cloud_src_demean);
146  demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);
147 
148  getTransformationFromCorrelation(cloud_src_demean,
149  centroid_src,
150  cloud_tgt_demean,
151  centroid_tgt,
152  transformation_matrix);
153 }
154 
155 template <typename PointSource, typename PointTarget, typename Scalar>
156 void
159  const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
160  const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
161  const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
162  const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
163  Matrix4& transformation_matrix) const
164 {
165  transformation_matrix.setIdentity();
166 
167  // Assemble the correlation matrix H = source * target'
168  Eigen::Matrix<Scalar, 3, 3> H =
169  (cloud_src_demean * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>();
170 
171  float angle = std::atan2((H(0, 1) - H(1, 0)), (H(0, 0) + H(1, 1)));
172 
173  Eigen::Matrix<Scalar, 3, 3> R(Eigen::Matrix<Scalar, 3, 3>::Identity());
174  R(0, 0) = R(1, 1) = std::cos(angle);
175  R(0, 1) = -std::sin(angle);
176  R(1, 0) = std::sin(angle);
177 
178  // Return the correct transformation
179  transformation_matrix.template topLeftCorner<3, 3>().matrix() = R;
180  const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.template head<3>().matrix());
181  transformation_matrix.template block<3, 1>(0, 3).matrix() =
182  centroid_tgt.template head<3>() - Rc;
183 }
184 
185 } // namespace registration
186 } // namespace pcl
187 
188 #endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_
Iterator class for point clouds with or without given indices.
std::size_t size() const
Definition: point_cloud.h:443
void getTransformationFromCorrelation(const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_src_demean, const Eigen::Matrix< Scalar, 4, 1 > &centroid_src, const Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > &cloud_tgt_demean, const Eigen::Matrix< Scalar, 4, 1 > &centroid_tgt, Matrix4 &transformation_matrix) const
Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src.
void estimateRigidTransformation(const pcl::PointCloud< PointSource > &cloud_src, const pcl::PointCloud< PointTarget > &cloud_tgt, Matrix4 &transformation_matrix) const
Estimate a rigid transformation between a source and a target point cloud in 2D.
typename TransformationEstimation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition: centroid.hpp:933
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:57
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133