Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
41namespace pcl {
42
43namespace registration {
44
45template <typename PointSource, typename PointTarget, typename Scalar>
46inline 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
66template <typename PointSource, typename PointTarget, typename Scalar>
67void
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
87template <typename PointSource, typename PointTarget, typename Scalar>
88inline 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
109template <typename PointSource, typename PointTarget, typename Scalar>
110void
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
122template <typename PointSource, typename PointTarget, typename Scalar>
123inline 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
155template <typename PointSource, typename PointTarget, typename Scalar>
156void
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::size_t size() const
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