Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: gicp.h 3042 2011-11-01 04:44:47Z svn $ 00035 * 00036 */ 00037 00038 #ifndef PCL_GICP_H_ 00039 #define PCL_GICP_H_ 00040 00041 // PCL includes 00042 #include "pcl/registration/icp.h" 00043 00044 #include <cminpack.h> 00045 00046 namespace pcl 00047 { 00058 template <typename PointSource, typename PointTarget> 00059 class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget> 00060 { 00061 using IterativeClosestPoint<PointSource, PointTarget>::reg_name_; 00062 using IterativeClosestPoint<PointSource, PointTarget>::getClassName; 00063 using IterativeClosestPoint<PointSource, PointTarget>::indices_; 00064 using IterativeClosestPoint<PointSource, PointTarget>::target_; 00065 using IterativeClosestPoint<PointSource, PointTarget>::input_; 00066 using IterativeClosestPoint<PointSource, PointTarget>::tree_; 00067 using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_; 00068 using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_; 00069 using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_; 00070 using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_; 00071 using IterativeClosestPoint<PointSource, PointTarget>::transformation_; 00072 using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_; 00073 using IterativeClosestPoint<PointSource, PointTarget>::converged_; 00074 using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_; 00075 using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_; 00076 using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_; 00077 using IterativeClosestPoint<PointSource, PointTarget>::rigid_transformation_estimation_; 00078 using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_; 00079 00080 typedef pcl::PointCloud<PointSource> PointCloudSource; 00081 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00082 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00083 00084 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00085 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00086 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00087 00088 typedef PointIndices::Ptr PointIndicesPtr; 00089 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00090 00091 typedef typename pcl::KdTree<PointSource> InputKdTree; 00092 typedef typename pcl::KdTree<PointSource>::Ptr InputKdTreePtr; 00093 00094 public: 00096 GeneralizedIterativeClosestPoint () : 00097 k_correspondences_(20), 00098 gicp_epsilon_(0.0004), rotation_epsilon_(2e-3), 00099 input_covariances_(0), target_covariances_(0), mahalanobis_(0) 00100 { 00101 min_number_correspondences_ = 4; 00102 reg_name_ = "GeneralizedIterativeClosestPoint"; 00103 max_iterations_ = 200; 00104 transformation_epsilon_ = 5e-4; 00105 corr_dist_threshold_ = 5. * 5.; 00106 rigid_transformation_estimation_ = 00107 boost::bind (&GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationLM, 00108 this, _1, _2, _3, _4, _5); 00109 input_tree_.reset (new pcl::KdTreeFLANN<PointSource>); 00110 } 00111 00115 inline void 00116 setInputCloud (const PointCloudSourceConstPtr &cloud) 00117 { 00118 if (cloud->points.empty ()) 00119 { 00120 PCL_ERROR ("[pcl::%s::setInputInput] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00121 return; 00122 } 00123 PointCloudSource input = *cloud; 00124 // Set all the point.data[3] values to 1 to aid the rigid transformation 00125 for (size_t i = 0; i < input.size (); ++i) 00126 input[i].data[3] = 1.0; 00127 00128 input_ = input.makeShared (); 00129 input_tree_->setInputCloud (input_); 00130 input_covariances_.reserve (input_->size ()); 00131 } 00132 00136 inline void 00137 setInputTarget (const PointCloudTargetConstPtr &target) 00138 { 00139 pcl::Registration<PointSource, PointTarget>::setInputTarget(target); 00140 target_covariances_.reserve (target_->size ()); 00141 } 00142 00149 void 00150 estimateRigidTransformationLM (const PointCloudSource &cloud_src, 00151 const PointCloudTarget &cloud_tgt, 00152 Eigen::Matrix4f &transformation_matrix); 00153 00162 void 00163 estimateRigidTransformationLM (const PointCloudSource &cloud_src, 00164 const std::vector<int> &indices_src, 00165 const PointCloudTarget &cloud_tgt, 00166 const std::vector<int> &indices_tgt, 00167 Eigen::Matrix4f &transformation_matrix); 00168 00170 inline const Eigen::Matrix3d& mahalanobis(size_t index) const 00171 { 00172 assert(index < mahalanobis_.size()); 00173 return mahalanobis_[index]; 00174 } 00175 00183 void 00184 computeRDerivative(const double x[], const Eigen::Matrix3d &R, double g[]); 00185 00191 inline void 00192 setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } 00193 00197 inline double 00198 getRotationEpsilon () { return (rotation_epsilon_); } 00199 00206 void 00207 setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 00208 00212 void 00213 getCorrespondenceRandomness () { return (k_correspondences_); } 00214 00215 private: 00216 00220 int k_correspondences_; 00225 double gicp_epsilon_; 00230 double rotation_epsilon_; 00232 Eigen::Matrix4f base_transformation_; 00243 static int 00244 functionToOptimize (void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag); 00245 00256 static int 00257 functionToOptimizeIndices (void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag); 00258 00260 boost::mutex tmp_mutex_; 00261 00263 const PointCloudSource *tmp_src_; 00264 00266 const PointCloudTarget *tmp_tgt_; 00267 00269 const std::vector<int> *tmp_idx_src_; 00270 00272 const std::vector<int> *tmp_idx_tgt_; 00273 00275 InputKdTreePtr input_tree_; 00276 00278 std::vector<Eigen::Matrix3d> input_covariances_; 00279 00281 std::vector<Eigen::Matrix3d> target_covariances_; 00282 00284 std::vector<Eigen::Matrix3d> mahalanobis_; 00285 00292 template<typename PointT> 00293 void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 00294 const typename pcl::KdTree<PointT>::Ptr tree, 00295 std::vector<Eigen::Matrix3d>& cloud_covariances); 00296 00301 inline double 00302 matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) 00303 { 00304 double r = 0.; 00305 size_t n = mat1.rows(); 00306 // tr(mat1^t.mat2) 00307 for(size_t i = 0; i < n; i++) 00308 for(size_t j = 0; j < n; j++) 00309 r += mat1 (j, i) * mat2 (i,j); 00310 return r; 00311 } 00312 00316 void 00317 computeTransformation (PointCloudSource &output) 00318 { 00319 computeTransformation (output, Eigen::Matrix4f::Identity()); 00320 } 00321 00326 void 00327 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess); 00328 00334 inline bool 00335 searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance) 00336 { 00337 int k = tree_->nearestKSearch (query, 1, index, distance); 00338 if (k == 0) 00339 return (false); 00340 return (true); 00341 } 00342 00346 inline void 00347 heteregenious_product(const Eigen::Matrix4f& mat1, const Eigen::Matrix4f& mat2, Eigen::Matrix4d& result) 00348 { 00349 result.setZero(); 00350 for(size_t i = 0; i < 4; i++) 00351 for(size_t j = 0; j < 4; j++) 00352 for(size_t k = 0; k < 4; k++) 00353 result(i,j)+= double(mat1(i,k)) * double(mat2(k,j)); 00354 } 00355 00356 void apply_state(Eigen::Matrix4f &t, const double x[]); 00357 void get_translation(const Eigen::Matrix4f &t, double &x, double &y, double &z); 00358 void get_rotation(const Eigen::Matrix4f &t, double &rx, double &ry, double &rz); 00359 }; 00360 } 00361 00362 #include "pcl/registration/impl/gicp.hpp" 00363 00364 #endif //#ifndef PCL_GICP_H_