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-2011, 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_LM_HPP_ 00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ 00042 00043 #include <pcl/registration/warp_point_rigid.h> 00044 #include <pcl/registration/warp_point_rigid_6d.h> 00045 #include <pcl/registration/distances.h> 00046 #include <unsupported/Eigen/NonLinearOptimization> 00047 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////// 00050 template <typename PointSource, typename PointTarget, typename MatScalar> 00051 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::TransformationEstimationLM () 00052 : tmp_src_ () 00053 , tmp_tgt_ () 00054 , tmp_idx_src_ () 00055 , tmp_idx_tgt_ () 00056 , warp_point_ (new WarpPointRigid6D<PointSource, PointTarget, MatScalar>) 00057 { 00058 }; 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////////// 00061 template <typename PointSource, typename PointTarget, typename MatScalar> void 00062 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00063 const pcl::PointCloud<PointSource> &cloud_src, 00064 const pcl::PointCloud<PointTarget> &cloud_tgt, 00065 Matrix4 &transformation_matrix) const 00066 { 00067 00068 // <cloud_src,cloud_src> is the source dataset 00069 if (cloud_src.points.size () != cloud_tgt.points.size ()) 00070 { 00071 PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "); 00072 PCL_ERROR ("Number or points in source (%zu) differs than target (%zu)!\n", 00073 cloud_src.points.size (), cloud_tgt.points.size ()); 00074 return; 00075 } 00076 if (cloud_src.points.size () < 4) // need at least 4 samples 00077 { 00078 PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "); 00079 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!\n", 00080 cloud_src.points.size ()); 00081 return; 00082 } 00083 00084 int n_unknowns = warp_point_->getDimension (); 00085 VectorX x (n_unknowns); 00086 x.setZero (); 00087 00088 // Set temporary pointers 00089 tmp_src_ = &cloud_src; 00090 tmp_tgt_ = &cloud_tgt; 00091 00092 OptimizationFunctor functor (static_cast<int> (cloud_src.points.size ()), this); 00093 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); 00094 //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff); 00095 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, MatScalar> lm (num_diff); 00096 int info = lm.minimize (x); 00097 00098 // Compute the norm of the residuals 00099 PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]"); 00100 PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); 00101 PCL_DEBUG ("Final solution: [%f", x[0]); 00102 for (int i = 1; i < n_unknowns; ++i) 00103 PCL_DEBUG (" %f", x[i]); 00104 PCL_DEBUG ("]\n"); 00105 00106 // Return the correct transformation 00107 warp_point_->setParam (x); 00108 transformation_matrix = warp_point_->getTransform (); 00109 00110 tmp_src_ = NULL; 00111 tmp_tgt_ = NULL; 00112 } 00113 00114 ////////////////////////////////////////////////////////////////////////////////////////////// 00115 template <typename PointSource, typename PointTarget, typename MatScalar> void 00116 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00117 const pcl::PointCloud<PointSource> &cloud_src, 00118 const std::vector<int> &indices_src, 00119 const pcl::PointCloud<PointTarget> &cloud_tgt, 00120 Matrix4 &transformation_matrix) const 00121 { 00122 if (indices_src.size () != cloud_tgt.points.size ()) 00123 { 00124 PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ()); 00125 return; 00126 } 00127 00128 // <cloud_src,cloud_src> is the source dataset 00129 transformation_matrix.setIdentity (); 00130 00131 const int nr_correspondences = static_cast<const int> (cloud_tgt.points.size ()); 00132 std::vector<int> indices_tgt; 00133 indices_tgt.resize(nr_correspondences); 00134 for (int i = 0; i < nr_correspondences; ++i) 00135 indices_tgt[i] = i; 00136 00137 estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 00138 } 00139 00140 ////////////////////////////////////////////////////////////////////////////////////////////// 00141 template <typename PointSource, typename PointTarget, typename MatScalar> inline void 00142 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00143 const pcl::PointCloud<PointSource> &cloud_src, 00144 const std::vector<int> &indices_src, 00145 const pcl::PointCloud<PointTarget> &cloud_tgt, 00146 const std::vector<int> &indices_tgt, 00147 Matrix4 &transformation_matrix) const 00148 { 00149 if (indices_src.size () != indices_tgt.size ()) 00150 { 00151 PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ()); 00152 return; 00153 } 00154 00155 if (indices_src.size () < 4) // need at least 4 samples 00156 { 00157 PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] "); 00158 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!", 00159 indices_src.size ()); 00160 return; 00161 } 00162 00163 int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space 00164 VectorX x (n_unknowns); 00165 x.setConstant (n_unknowns, 0); 00166 00167 // Set temporary pointers 00168 tmp_src_ = &cloud_src; 00169 tmp_tgt_ = &cloud_tgt; 00170 tmp_idx_src_ = &indices_src; 00171 tmp_idx_tgt_ = &indices_tgt; 00172 00173 OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this); 00174 Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor); 00175 //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff); 00176 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices>, MatScalar> lm (num_diff); 00177 int info = lm.minimize (x); 00178 00179 // Compute the norm of the residuals 00180 PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); 00181 PCL_DEBUG ("Final solution: [%f", x[0]); 00182 for (int i = 1; i < n_unknowns; ++i) 00183 PCL_DEBUG (" %f", x[i]); 00184 PCL_DEBUG ("]\n"); 00185 00186 // Return the correct transformation 00187 warp_point_->setParam (x); 00188 transformation_matrix = warp_point_->getTransform (); 00189 00190 tmp_src_ = NULL; 00191 tmp_tgt_ = NULL; 00192 tmp_idx_src_ = tmp_idx_tgt_ = NULL; 00193 } 00194 00195 ////////////////////////////////////////////////////////////////////////////////////////////// 00196 template <typename PointSource, typename PointTarget, typename MatScalar> inline void 00197 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( 00198 const pcl::PointCloud<PointSource> &cloud_src, 00199 const pcl::PointCloud<PointTarget> &cloud_tgt, 00200 const pcl::Correspondences &correspondences, 00201 Matrix4 &transformation_matrix) const 00202 { 00203 const int nr_correspondences = static_cast<const int> (correspondences.size ()); 00204 std::vector<int> indices_src (nr_correspondences); 00205 std::vector<int> indices_tgt (nr_correspondences); 00206 for (int i = 0; i < nr_correspondences; ++i) 00207 { 00208 indices_src[i] = correspondences[i].index_query; 00209 indices_tgt[i] = correspondences[i].index_match; 00210 } 00211 00212 estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); 00213 } 00214 00215 ////////////////////////////////////////////////////////////////////////////////////////////// 00216 template <typename PointSource, typename PointTarget, typename MatScalar> int 00217 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::OptimizationFunctor::operator () ( 00218 const VectorX &x, VectorX &fvec) const 00219 { 00220 const PointCloud<PointSource> & src_points = *estimator_->tmp_src_; 00221 const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_; 00222 00223 // Initialize the warp function with the given parameters 00224 estimator_->warp_point_->setParam (x); 00225 00226 // Transform each source point and compute its distance to the corresponding target point 00227 for (int i = 0; i < values (); ++i) 00228 { 00229 const PointSource & p_src = src_points.points[i]; 00230 const PointTarget & p_tgt = tgt_points.points[i]; 00231 00232 // Transform the source point based on the current warp parameters 00233 Vector4 p_src_warped; 00234 estimator_->warp_point_->warpPoint (p_src, p_src_warped); 00235 00236 // Estimate the distance (cost function) 00237 fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt); 00238 } 00239 return (0); 00240 } 00241 00242 ////////////////////////////////////////////////////////////////////////////////////////////// 00243 template <typename PointSource, typename PointTarget, typename MatScalar> int 00244 pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::OptimizationFunctorWithIndices::operator() ( 00245 const VectorX &x, VectorX &fvec) const 00246 { 00247 const PointCloud<PointSource> & src_points = *estimator_->tmp_src_; 00248 const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_; 00249 const std::vector<int> & src_indices = *estimator_->tmp_idx_src_; 00250 const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_; 00251 00252 // Initialize the warp function with the given parameters 00253 estimator_->warp_point_->setParam (x); 00254 00255 // Transform each source point and compute its distance to the corresponding target point 00256 for (int i = 0; i < values (); ++i) 00257 { 00258 const PointSource & p_src = src_points.points[src_indices[i]]; 00259 const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]]; 00260 00261 // Transform the source point based on the current warp parameters 00262 Vector4 p_src_warped; 00263 estimator_->warp_point_->warpPoint (p_src, p_src_warped); 00264 00265 // Estimate the distance (cost function) 00266 fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt); 00267 } 00268 return (0); 00269 } 00270 00271 //#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationLM<T,U>; 00272 00273 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */