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