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) 2009-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * Copyright (c) Alexandru-Eugen Ichim 00008 * 00009 * All rights reserved. 00010 * 00011 * Redistribution and use in source and binary forms, with or without 00012 * modification, are permitted provided that the following conditions 00013 * are met: 00014 * 00015 * * Redistributions of source code must retain the above copyright 00016 * notice, this list of conditions and the following disclaimer. 00017 * * Redistributions in binary form must reproduce the above 00018 * copyright notice, this list of conditions and the following 00019 * disclaimer in the documentation and/or other materials provided 00020 * with the distribution. 00021 * * Neither the name of the copyright holder(s) nor the names of its 00022 * contributors may be used to endorse or promote products derived 00023 * from this software without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00026 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00027 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00028 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00029 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00030 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00031 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00032 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00033 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00034 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00035 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00036 * POSSIBILITY OF SUCH DAMAGE. 00037 */ 00038 00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ 00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ 00041 00042 #include <pcl/registration/warp_point_rigid.h> 00043 #include <pcl/registration/warp_point_rigid_6d.h> 00044 #include <pcl/registration/distances.h> 00045 #include <unsupported/Eigen/NonLinearOptimization> 00046 00047 00048 ////////////////////////////////////////////////////////////////////////////////////////////// 00049 template <typename PointSource, typename PointTarget, typename MatScalar> 00050 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::TransformationEstimationPointToPlaneWeighted () 00051 : tmp_src_ () 00052 , tmp_tgt_ () 00053 , tmp_idx_src_ () 00054 , tmp_idx_tgt_ () 00055 , warp_point_ (new WarpPointRigid6D<PointSource, PointTarget, MatScalar>) 00056 , correspondence_weights_ () 00057 , use_correspondence_weights_ (true) 00058 { 00059 }; 00060 00061 ////////////////////////////////////////////////////////////////////////////////////////////// 00062 template <typename PointSource, typename PointTarget, typename MatScalar> void 00063 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00064 const pcl::PointCloud<PointSource> &cloud_src, 00065 const pcl::PointCloud<PointTarget> &cloud_tgt, 00066 Matrix4 &transformation_matrix) const 00067 { 00068 00069 // <cloud_src,cloud_src> is the source dataset 00070 if (cloud_src.points.size () != cloud_tgt.points.size ()) 00071 { 00072 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); 00073 PCL_ERROR ("Number or points in source (%zu) differs than target (%zu)!\n", 00074 cloud_src.points.size (), cloud_tgt.points.size ()); 00075 return; 00076 } 00077 if (cloud_src.points.size () < 4) // need at least 4 samples 00078 { 00079 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); 00080 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!\n", 00081 cloud_src.points.size ()); 00082 return; 00083 } 00084 00085 if (correspondence_weights_.size () != cloud_src.size ()) 00086 { 00087 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); 00088 PCL_ERROR ("Number of weights (%zu) differs than number of points (%zu)!\n", 00089 correspondence_weights_.size (), cloud_src.points.size ()); 00090 return; 00091 } 00092 00093 int n_unknowns = warp_point_->getDimension (); 00094 VectorX x (n_unknowns); 00095 x.setZero (); 00096 00097 // Set temporary pointers 00098 tmp_src_ = &cloud_src; 00099 tmp_tgt_ = &cloud_tgt; 00100 00101 OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this); 00102 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); 00103 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff); 00104 int info = lm.minimize (x); 00105 00106 // Compute the norm of the residuals 00107 PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]"); 00108 PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); 00109 PCL_DEBUG ("Final solution: [%f", x[0]); 00110 for (int i = 1; i < n_unknowns; ++i) 00111 PCL_DEBUG (" %f", x[i]); 00112 PCL_DEBUG ("]\n"); 00113 00114 // Return the correct transformation 00115 warp_point_->setParam (x); 00116 transformation_matrix = warp_point_->getTransform (); 00117 00118 tmp_src_ = NULL; 00119 tmp_tgt_ = NULL; 00120 } 00121 00122 ////////////////////////////////////////////////////////////////////////////////////////////// 00123 template <typename PointSource, typename PointTarget, typename MatScalar> void 00124 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00125 const pcl::PointCloud<PointSource> &cloud_src, 00126 const std::vector<int> &indices_src, 00127 const pcl::PointCloud<PointTarget> &cloud_tgt, 00128 Matrix4 &transformation_matrix) const 00129 { 00130 if (indices_src.size () != cloud_tgt.points.size ()) 00131 { 00132 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); 00133 return; 00134 } 00135 00136 if (correspondence_weights_.size () != indices_src.size ()) 00137 { 00138 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); 00139 PCL_ERROR ("Number of weights (%zu) differs than number of points (%zu)!\n", 00140 correspondence_weights_.size (), indices_src.size ()); 00141 return; 00142 } 00143 00144 00145 // <cloud_src,cloud_src> is the source dataset 00146 transformation_matrix.setIdentity (); 00147 00148 const int nr_correspondences = static_cast<const int> (cloud_tgt.points.size ()); 00149 std::vector<int> indices_tgt; 00150 indices_tgt.resize(nr_correspondences); 00151 for (int i = 0; i < nr_correspondences; ++i) 00152 indices_tgt[i] = i; 00153 00154 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 00155 } 00156 00157 ////////////////////////////////////////////////////////////////////////////////////////////// 00158 template <typename PointSource, typename PointTarget, typename MatScalar> inline void 00159 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00160 const pcl::PointCloud<PointSource> &cloud_src, 00161 const std::vector<int> &indices_src, 00162 const pcl::PointCloud<PointTarget> &cloud_tgt, 00163 const std::vector<int> &indices_tgt, 00164 Matrix4 &transformation_matrix) const 00165 { 00166 if (indices_src.size () != indices_tgt.size ()) 00167 { 00168 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); 00169 return; 00170 } 00171 00172 if (indices_src.size () < 4) // need at least 4 samples 00173 { 00174 PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] "); 00175 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!", 00176 indices_src.size ()); 00177 return; 00178 } 00179 00180 if (correspondence_weights_.size () != indices_src.size ()) 00181 { 00182 PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); 00183 PCL_ERROR ("Number of weights (%zu) differs than number of points (%zu)!\n", 00184 correspondence_weights_.size (), indices_src.size ()); 00185 return; 00186 } 00187 00188 00189 int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space 00190 VectorX x (n_unknowns); 00191 x.setConstant (n_unknowns, 0); 00192 00193 // Set temporary pointers 00194 tmp_src_ = &cloud_src; 00195 tmp_tgt_ = &cloud_tgt; 00196 tmp_idx_src_ = &indices_src; 00197 tmp_idx_tgt_ = &indices_tgt; 00198 00199 OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this); 00200 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor); 00201 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff); 00202 int info = lm.minimize (x); 00203 00204 // Compute the norm of the residuals 00205 PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); 00206 PCL_DEBUG ("Final solution: [%f", x[0]); 00207 for (int i = 1; i < n_unknowns; ++i) 00208 PCL_DEBUG (" %f", x[i]); 00209 PCL_DEBUG ("]\n"); 00210 00211 // Return the correct transformation 00212 warp_point_->setParam (x); 00213 transformation_matrix = warp_point_->getTransform (); 00214 00215 tmp_src_ = NULL; 00216 tmp_tgt_ = NULL; 00217 tmp_idx_src_ = tmp_idx_tgt_ = NULL; 00218 } 00219 00220 ////////////////////////////////////////////////////////////////////////////////////////////// 00221 template <typename PointSource, typename PointTarget, typename MatScalar> inline void 00222 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00223 const pcl::PointCloud<PointSource> &cloud_src, 00224 const pcl::PointCloud<PointTarget> &cloud_tgt, 00225 const pcl::Correspondences &correspondences, 00226 Matrix4 &transformation_matrix) const 00227 { 00228 const int nr_correspondences = static_cast<const int> (correspondences.size ()); 00229 std::vector<int> indices_src (nr_correspondences); 00230 std::vector<int> indices_tgt (nr_correspondences); 00231 for (int i = 0; i < nr_correspondences; ++i) 00232 { 00233 indices_src[i] = correspondences[i].index_query; 00234 indices_tgt[i] = correspondences[i].index_match; 00235 } 00236 00237 if (use_correspondence_weights_) 00238 { 00239 correspondence_weights_.resize (nr_correspondences); 00240 for (size_t i = 0; i < nr_correspondences; ++i) 00241 correspondence_weights_[i] = correspondences[i].weight; 00242 } 00243 00244 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 00245 } 00246 00247 ////////////////////////////////////////////////////////////////////////////////////////////// 00248 template <typename PointSource, typename PointTarget, typename MatScalar> int 00249 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::OptimizationFunctor::operator () ( 00250 const VectorX &x, VectorX &fvec) const 00251 { 00252 const PointCloud<PointSource> & src_points = *estimator_->tmp_src_; 00253 const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_; 00254 00255 // Initialize the warp function with the given parameters 00256 estimator_->warp_point_->setParam (x); 00257 00258 // Transform each source point and compute its distance to the corresponding target point 00259 for (int i = 0; i < values (); ++i) 00260 { 00261 const PointSource & p_src = src_points.points[i]; 00262 const PointTarget & p_tgt = tgt_points.points[i]; 00263 00264 // Transform the source point based on the current warp parameters 00265 Vector4 p_src_warped; 00266 estimator_->warp_point_->warpPoint (p_src, p_src_warped); 00267 00268 // Estimate the distance (cost function) 00269 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt); 00270 } 00271 return (0); 00272 } 00273 00274 ////////////////////////////////////////////////////////////////////////////////////////////// 00275 template <typename PointSource, typename PointTarget, typename MatScalar> int 00276 pcl::registration::TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar>::OptimizationFunctorWithIndices::operator() ( 00277 const VectorX &x, VectorX &fvec) const 00278 { 00279 const PointCloud<PointSource> & src_points = *estimator_->tmp_src_; 00280 const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_; 00281 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_; 00282 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_; 00283 00284 // Initialize the warp function with the given parameters 00285 estimator_->warp_point_->setParam (x); 00286 00287 // Transform each source point and compute its distance to the corresponding target point 00288 for (int i = 0; i < values (); ++i) 00289 { 00290 const PointSource & p_src = src_points.points[src_indices[i]]; 00291 const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]]; 00292 00293 // Transform the source point based on the current warp parameters 00294 Vector4 p_src_warped; 00295 estimator_->warp_point_->warpPoint (p_src, p_src_warped); 00296 00297 // Estimate the distance (cost function) 00298 fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt); 00299 } 00300 return (0); 00301 } 00302 00303 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ */