Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ 00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ 00042 00043 #include <pcl/common/eigen.h> 00044 00045 /////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00047 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( 00048 const pcl::PointCloud<PointSource> &cloud_src, 00049 const pcl::PointCloud<PointTarget> &cloud_tgt, 00050 Matrix4 &transformation_matrix) const 00051 { 00052 size_t nr_points = cloud_src.points.size (); 00053 if (cloud_tgt.points.size () != nr_points) 00054 { 00055 PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ()); 00056 return; 00057 } 00058 00059 ConstCloudIterator<PointSource> source_it (cloud_src); 00060 ConstCloudIterator<PointTarget> target_it (cloud_tgt); 00061 estimateRigidTransformation (source_it, target_it, transformation_matrix); 00062 } 00063 00064 /////////////////////////////////////////////////////////////////////////////////////////// 00065 template <typename PointSource, typename PointTarget, typename Scalar> void 00066 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( 00067 const pcl::PointCloud<PointSource> &cloud_src, 00068 const std::vector<int> &indices_src, 00069 const pcl::PointCloud<PointTarget> &cloud_tgt, 00070 Matrix4 &transformation_matrix) const 00071 { 00072 if (indices_src.size () != cloud_tgt.points.size ()) 00073 { 00074 PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); 00075 return; 00076 } 00077 00078 ConstCloudIterator<PointSource> source_it (cloud_src, indices_src); 00079 ConstCloudIterator<PointTarget> target_it (cloud_tgt); 00080 estimateRigidTransformation (source_it, target_it, transformation_matrix); 00081 } 00082 00083 /////////////////////////////////////////////////////////////////////////////////////////// 00084 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00085 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( 00086 const pcl::PointCloud<PointSource> &cloud_src, 00087 const std::vector<int> &indices_src, 00088 const pcl::PointCloud<PointTarget> &cloud_tgt, 00089 const std::vector<int> &indices_tgt, 00090 Matrix4 &transformation_matrix) const 00091 { 00092 if (indices_src.size () != indices_tgt.size ()) 00093 { 00094 PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); 00095 return; 00096 } 00097 00098 ConstCloudIterator<PointSource> source_it (cloud_src, indices_src); 00099 ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt); 00100 estimateRigidTransformation (source_it, target_it, transformation_matrix); 00101 } 00102 00103 /////////////////////////////////////////////////////////////////////////////////////////// 00104 template <typename PointSource, typename PointTarget, typename Scalar> void 00105 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( 00106 const pcl::PointCloud<PointSource> &cloud_src, 00107 const pcl::PointCloud<PointTarget> &cloud_tgt, 00108 const pcl::Correspondences &correspondences, 00109 Matrix4 &transformation_matrix) const 00110 { 00111 ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true); 00112 ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false); 00113 estimateRigidTransformation (source_it, target_it, transformation_matrix); 00114 } 00115 00116 /////////////////////////////////////////////////////////////////////////////////////////// 00117 template <typename PointSource, typename PointTarget, typename Scalar> inline void 00118 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( 00119 ConstCloudIterator<PointSource>& source_it, 00120 ConstCloudIterator<PointTarget>& target_it, 00121 Matrix4 &transformation_matrix) const 00122 { 00123 // Convert to Eigen format 00124 const int npts = static_cast <int> (source_it.size ()); 00125 00126 Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts); 00127 Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts); 00128 00129 for (int i = 0; i < npts; ++i) 00130 { 00131 cloud_src (0, i) = source_it->x; 00132 cloud_src (1, i) = source_it->y; 00133 cloud_src (2, i) = source_it->z; 00134 ++source_it; 00135 00136 cloud_tgt (0, i) = target_it->x; 00137 cloud_tgt (1, i) = target_it->y; 00138 cloud_tgt (2, i) = target_it->z; 00139 ++target_it; 00140 } 00141 00142 if (use_umeyama_) 00143 { 00144 // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) 00145 transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); 00146 } 00147 else 00148 { 00149 source_it.reset (); target_it.reset (); 00150 // <cloud_src,cloud_src> is the source dataset 00151 transformation_matrix.setIdentity (); 00152 00153 Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; 00154 // Estimate the centroids of source, target 00155 compute3DCentroid (source_it, centroid_src); 00156 compute3DCentroid (target_it, centroid_tgt); 00157 source_it.reset (); target_it.reset (); 00158 00159 // Subtract the centroids from source, target 00160 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean; 00161 demeanPointCloud (source_it, centroid_src, cloud_src_demean); 00162 demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); 00163 00164 getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); 00165 } 00166 } 00167 00168 /////////////////////////////////////////////////////////////////////////////////////////// 00169 template <typename PointSource, typename PointTarget, typename Scalar> void 00170 pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation ( 00171 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean, 00172 const Eigen::Matrix<Scalar, 4, 1> ¢roid_src, 00173 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean, 00174 const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt, 00175 Matrix4 &transformation_matrix) const 00176 { 00177 transformation_matrix.setIdentity (); 00178 00179 // Assemble the correlation matrix H = source * target' 00180 Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); 00181 00182 // Compute the Singular Value Decomposition 00183 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); 00184 Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU (); 00185 Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV (); 00186 00187 // Compute R = V * U' 00188 if (u.determinant () * v.determinant () < 0) 00189 { 00190 for (int x = 0; x < 3; ++x) 00191 v (x, 2) *= -1; 00192 } 00193 00194 Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose (); 00195 00196 // Return the correct transformation 00197 transformation_matrix.topLeftCorner (3, 3) = R; 00198 const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3)); 00199 transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc; 00200 } 00201 00202 //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>; 00203 00204 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */