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: registration.h 3041 2011-11-01 04:44:41Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_REGISTRATION_H_ 00041 #define PCL_REGISTRATION_H_ 00042 00043 #include <boost/function.hpp> 00044 #include <boost/bind.hpp> 00045 00046 // PCL includes 00047 #include <pcl/pcl_base.h> 00048 #include <pcl/common/transforms.h> 00049 #include <pcl/win32_macros.h> 00050 #include <pcl/kdtree/kdtree_flann.h> 00051 #include "pcl/registration/transformation_estimation.h" 00052 00053 namespace pcl 00054 { 00060 template <typename PointSource, typename PointTarget> 00061 class Registration : public PCLBase<PointSource> 00062 { 00063 public: 00064 using PCLBase<PointSource>::initCompute; 00065 using PCLBase<PointSource>::deinitCompute; 00066 using PCLBase<PointSource>::input_; 00067 using PCLBase<PointSource>::indices_; 00068 00069 typedef boost::shared_ptr< Registration<PointSource, PointTarget> > Ptr; 00070 typedef boost::shared_ptr< const Registration<PointSource, PointTarget> > ConstPtr; 00071 00072 typedef typename pcl::KdTree<PointTarget> KdTree; 00073 typedef typename pcl::KdTree<PointTarget>::Ptr KdTreePtr; 00074 00075 typedef pcl::PointCloud<PointSource> PointCloudSource; 00076 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00077 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00078 00079 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00080 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00081 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00082 00083 typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr; 00084 00085 typedef typename pcl::registration::TransformationEstimation<PointSource, PointTarget> TransformationEstimation; 00086 typedef typename TransformationEstimation::Ptr TransformationEstimationPtr; 00087 typedef typename TransformationEstimation::ConstPtr TransformationEstimationConstPtr; 00088 00090 Registration () : nr_iterations_(0), 00091 max_iterations_(10), 00092 target_ (), 00093 final_transformation_ (Eigen::Matrix4f::Identity ()), 00094 transformation_ (Eigen::Matrix4f::Identity ()), 00095 previous_transformation_ (Eigen::Matrix4f::Identity ()), 00096 transformation_epsilon_ (0.0), 00097 euclidean_fitness_epsilon_ (-std::numeric_limits<double>::max ()), 00098 corr_dist_threshold_ (std::sqrt (std::numeric_limits<double>::max ())), 00099 inlier_threshold_ (0.05), 00100 converged_ (false), min_number_correspondences_ (3), 00101 transformation_estimation_ (), 00102 point_representation_ () 00103 { 00104 tree_.reset (new pcl::KdTreeFLANN<PointTarget>); // ANN tree for nearest neighbor search 00105 update_visualizer_ = NULL; 00106 } 00107 00109 virtual ~Registration () {} 00110 00111 void 00112 setTransformationEstimation (const TransformationEstimationPtr &te) { transformation_estimation_ = te; } 00113 00117 virtual inline void 00118 setInputTarget (const PointCloudTargetConstPtr &cloud); 00119 00121 inline PointCloudTargetConstPtr const 00122 getInputTarget () { return (target_ ); } 00123 00125 inline Eigen::Matrix4f 00126 getFinalTransformation () { return (final_transformation_); } 00127 00129 inline Eigen::Matrix4f 00130 getLastIncrementalTransformation () { return (transformation_); } 00131 00135 inline void 00136 setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; } 00137 00139 inline int 00140 getMaximumIterations () { return (max_iterations_); } 00141 00149 inline void 00150 setRANSACOutlierRejectionThreshold (double inlier_threshold) { inlier_threshold_ = inlier_threshold; } 00151 00153 inline double 00154 getRANSACOutlierRejectionThreshold () { return (inlier_threshold_); } 00155 00161 inline void 00162 setMaxCorrespondenceDistance (double distance_threshold) { corr_dist_threshold_ = distance_threshold; } 00163 00167 inline double 00168 getMaxCorrespondenceDistance () { return (corr_dist_threshold_); } 00169 00176 inline void 00177 setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; } 00178 00182 inline double 00183 getTransformationEpsilon () { return (transformation_epsilon_); } 00184 00193 inline void 00194 setEuclideanFitnessEpsilon (double epsilon) { euclidean_fitness_epsilon_ = epsilon; } 00195 00199 inline double 00200 getEuclideanFitnessEpsilon () { return (euclidean_fitness_epsilon_); } 00201 00205 inline void 00206 setPointRepresentation (const PointRepresentationConstPtr &point_representation) 00207 { 00208 point_representation_ = point_representation; 00209 } 00210 00215 template<typename FunctionSignature> inline bool 00216 registerVisualizationCallback (boost::function<FunctionSignature> &visualizerCallback) 00217 { 00218 if (visualizerCallback != NULL) 00219 { 00220 update_visualizer_ = visualizerCallback; 00221 return (true); 00222 } 00223 else 00224 return (false); 00225 } 00226 00231 inline double 00232 getFitnessScore (double max_range = std::numeric_limits<double>::max ()); 00233 00239 inline double 00240 getFitnessScore (const std::vector<float> &distances_a, const std::vector<float> &distances_b); 00241 00243 inline bool 00244 hasConverged () { return (converged_); } 00245 00250 inline void 00251 align (PointCloudSource &output); 00252 00258 inline void 00259 align (PointCloudSource &output, const Eigen::Matrix4f& guess); 00260 00262 inline const std::string& 00263 getClassName () const { return (reg_name_); } 00264 00265 protected: 00267 std::string reg_name_; 00268 00270 KdTreePtr tree_; 00271 00273 int nr_iterations_; 00274 00276 int max_iterations_; 00277 00279 PointCloudTargetConstPtr target_; 00280 00282 Eigen::Matrix4f final_transformation_; 00283 00285 Eigen::Matrix4f transformation_; 00286 00288 Eigen::Matrix4f previous_transformation_; 00289 00293 double transformation_epsilon_; 00294 00299 double euclidean_fitness_epsilon_; 00300 00304 double corr_dist_threshold_; 00305 00310 double inlier_threshold_; 00311 00313 bool converged_; 00314 00318 int min_number_correspondences_; 00319 00323 std::vector<float> correspondence_distances_; 00324 00326 TransformationEstimationPtr transformation_estimation_; 00327 00331 boost::function<void(const pcl::PointCloud<PointSource> &cloud_src, 00332 const std::vector<int> &indices_src, 00333 const pcl::PointCloud<PointTarget> &cloud_tgt, 00334 const std::vector<int> &indices_tgt)> update_visualizer_; 00335 00342 inline bool 00343 searchForNeighbors (const PointCloudSource &cloud, int index, 00344 std::vector<int> &indices, std::vector<float> &distances) 00345 { 00346 int k = tree_->nearestKSearch (cloud, index, 1, indices, distances); 00347 if (k == 0) 00348 return (false); 00349 return (true); 00350 } 00351 00352 private: 00353 00355 virtual void 00356 computeTransformation (PointCloudSource &output) = 0; 00357 00359 virtual void 00360 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) {} 00361 00363 PointRepresentationConstPtr point_representation_; 00364 }; 00365 } 00366 00367 #include "pcl/registration/impl/registration.hpp" 00368 00369 #endif //#ifndef PCL_REGISTRATION_H_