Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
transformation_estimation_svd.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, 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 Willow Garage, Inc. 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_SVD_HPP_
39 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
40 
42 template <typename PointSource, typename PointTarget> inline void
43 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
44  const pcl::PointCloud<PointSource> &cloud_src,
45  const pcl::PointCloud<PointTarget> &cloud_tgt,
46  Eigen::Matrix4f &transformation_matrix)
47 {
48  // <cloud_src,cloud_src> is the source dataset
49  transformation_matrix.setIdentity ();
50 
51  Eigen::Vector4f centroid_src, centroid_tgt;
52  // Estimate the centroids of source, target
53  compute3DCentroid (cloud_src, centroid_src);
54  compute3DCentroid (cloud_tgt, centroid_tgt);
55 
56  // Subtract the centroids from source, target
57  Eigen::MatrixXf cloud_src_demean;
58  demeanPointCloud (cloud_src, centroid_src, cloud_src_demean);
59 
60  Eigen::MatrixXf cloud_tgt_demean;
61  demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
62 
63  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
64 }
65 
67 template <typename PointSource, typename PointTarget> void
68 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
69  const pcl::PointCloud<PointSource> &cloud_src,
70  const std::vector<int> &indices_src,
71  const pcl::PointCloud<PointTarget> &cloud_tgt,
72  Eigen::Matrix4f &transformation_matrix)
73 {
74  if (indices_src.size () != cloud_tgt.points.size ())
75  {
76  PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
77  return;
78  }
79 
80  // <cloud_src,cloud_src> is the source dataset
81  transformation_matrix.setIdentity ();
82 
83  Eigen::Vector4f centroid_src, centroid_tgt;
84  // Estimate the centroids of source, target
85  compute3DCentroid (cloud_src, indices_src, centroid_src);
86  compute3DCentroid (cloud_tgt, centroid_tgt);
87 
88  // Subtract the centroids from source, target
89  Eigen::MatrixXf cloud_src_demean;
90  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
91 
92  Eigen::MatrixXf cloud_tgt_demean;
93  demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
94 
95  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
96 }
97 
98 
100 template <typename PointSource, typename PointTarget> inline void
101 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
102  const pcl::PointCloud<PointSource> &cloud_src,
103  const std::vector<int> &indices_src,
104  const pcl::PointCloud<PointTarget> &cloud_tgt,
105  const std::vector<int> &indices_tgt,
106  Eigen::Matrix4f &transformation_matrix)
107 {
108  if (indices_src.size () != indices_tgt.size ())
109  {
110  PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
111  return;
112  }
113 
114  // <cloud_src,cloud_src> is the source dataset
115  transformation_matrix.setIdentity ();
116 
117  Eigen::Vector4f centroid_src, centroid_tgt;
118  // Estimate the centroids of source, target
119  compute3DCentroid (cloud_src, indices_src, centroid_src);
120  compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
121 
122  // Subtract the centroids from source, target
123  Eigen::MatrixXf cloud_src_demean;
124  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
125 
126  Eigen::MatrixXf cloud_tgt_demean;
127  demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
128 
129  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
130 }
131 
133 template <typename PointSource, typename PointTarget> void
134 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::estimateRigidTransformation (
135  const pcl::PointCloud<PointSource> &cloud_src,
136  const pcl::PointCloud<PointTarget> &cloud_tgt,
137  const pcl::Correspondences &correspondences,
138  Eigen::Matrix4f &transformation_matrix)
139 {
140  std::vector<int> indices_src, indices_tgt;
141  pcl::registration::getQueryIndices (correspondences, indices_src);
142  pcl::registration::getMatchIndices (correspondences, indices_tgt);
143 
144  // <cloud_src,cloud_src> is the source dataset
145  Eigen::Vector4f centroid_src, centroid_tgt;
146  // Estimate the centroids of source, target
147  compute3DCentroid (cloud_src, indices_src, centroid_src);
148  compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
149 
150  // Subtract the centroids from source, target
151  Eigen::MatrixXf cloud_src_demean;
152  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
153 
154  Eigen::MatrixXf cloud_tgt_demean;
155  demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
156 
157  getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
158 }
159 
161 template <typename PointSource, typename PointTarget> void
162 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
163  const Eigen::MatrixXf &cloud_src_demean,
164  const Eigen::Vector4f &centroid_src,
165  const Eigen::MatrixXf &cloud_tgt_demean,
166  const Eigen::Vector4f &centroid_tgt,
167  Eigen::Matrix4f &transformation_matrix)
168 {
169  transformation_matrix.setIdentity ();
170 
171  // Assemble the correlation matrix H = source * target'
172  Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
173 
174  // Compute the Singular Value Decomposition
175  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
176  Eigen::Matrix3f u = svd.matrixU ();
177  Eigen::Matrix3f v = svd.matrixV ();
178 
179  // Compute R = V * U'
180  if (u.determinant () * v.determinant () < 0)
181  {
182  for (int x = 0; x < 3; ++x)
183  v (x, 2) *= -1;
184  }
185 
186  Eigen::Matrix3f R = v * u.transpose ();
187 
188  // Return the correct transformation
189  transformation_matrix.topLeftCorner<3, 3> () = R;
190  const Eigen::Vector3f Rc (R * centroid_src.head<3> ());
191  transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
192 }
193 
194 //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>;
195 
196 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */
Generated on Sat Aug 24 2013 17:28:36 for Point Cloud Library (PCL) by   doxygen 1.8.4