registration.h
Go to the documentation of this file.
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 6133 2012-07-04 18:41:13Z 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/pcl_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 () : reg_name_ (),
00091                         tree_ (new pcl::KdTreeFLANN<PointTarget>),
00092                         nr_iterations_(0),
00093                         max_iterations_(10),
00094                         ransac_iterations_ (0),
00095                         target_ (),
00096                         final_transformation_ (Eigen::Matrix4f::Identity ()),
00097                         transformation_ (Eigen::Matrix4f::Identity ()),
00098                         previous_transformation_ (Eigen::Matrix4f::Identity ()),
00099                         transformation_epsilon_ (0.0), 
00100                         euclidean_fitness_epsilon_ (-std::numeric_limits<double>::max ()),
00101                         corr_dist_threshold_ (std::sqrt (std::numeric_limits<double>::max ())),
00102                         inlier_threshold_ (0.05),
00103                         converged_ (false), 
00104                         min_number_correspondences_ (3), 
00105                         correspondence_distances_ (),
00106                         transformation_estimation_ (),
00107                         update_visualizer_ (NULL),
00108                         point_representation_ ()
00109       {
00110       }
00111 
00113       virtual ~Registration () {}
00114 
00118       void
00119       setTransformationEstimation (const TransformationEstimationPtr &te) { transformation_estimation_ = te; }
00120 
00124       virtual inline void 
00125       setInputTarget (const PointCloudTargetConstPtr &cloud);
00126 
00128       inline PointCloudTargetConstPtr const 
00129       getInputTarget () { return (target_ ); }
00130 
00132       inline Eigen::Matrix4f 
00133       getFinalTransformation () { return (final_transformation_); }
00134 
00136       inline Eigen::Matrix4f 
00137       getLastIncrementalTransformation () { return (transformation_); }
00138 
00142       inline void 
00143       setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; }
00144 
00146       inline int 
00147       getMaximumIterations () { return (max_iterations_); }
00148 
00152       inline void 
00153       setRANSACIterations (int ransac_iterations) { ransac_iterations_ = ransac_iterations; }
00154 
00156       inline double 
00157       getRANSACIterations () { return (ransac_iterations_); }
00158 
00166       inline void 
00167       setRANSACOutlierRejectionThreshold (double inlier_threshold) { inlier_threshold_ = inlier_threshold; }
00168 
00170       inline double 
00171       getRANSACOutlierRejectionThreshold () { return (inlier_threshold_); }
00172 
00178       inline void 
00179       setMaxCorrespondenceDistance (double distance_threshold) { corr_dist_threshold_ = distance_threshold; }
00180 
00184       inline double 
00185       getMaxCorrespondenceDistance () { return (corr_dist_threshold_); }
00186 
00193       inline void 
00194       setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; }
00195 
00199       inline double 
00200       getTransformationEpsilon () { return (transformation_epsilon_); }
00201 
00210       inline void 
00211       setEuclideanFitnessEpsilon (double epsilon) { euclidean_fitness_epsilon_ = epsilon; }
00212 
00216       inline double 
00217       getEuclideanFitnessEpsilon () { return (euclidean_fitness_epsilon_); }
00218 
00222       inline void
00223       setPointRepresentation (const PointRepresentationConstPtr &point_representation)
00224       {
00225         point_representation_ = point_representation;
00226       }
00227 
00232       template<typename FunctionSignature> inline bool
00233       registerVisualizationCallback (boost::function<FunctionSignature> &visualizerCallback)
00234       {
00235         if (visualizerCallback != NULL)
00236         {
00237           update_visualizer_ = visualizerCallback;
00238           return (true);
00239         }
00240         else
00241           return (false);
00242       }
00243 
00248       inline double 
00249       getFitnessScore (double max_range = std::numeric_limits<double>::max ());
00250 
00256       inline double 
00257       getFitnessScore (const std::vector<float> &distances_a, const std::vector<float> &distances_b);
00258 
00260       inline bool 
00261       hasConverged () { return (converged_); }
00262 
00267       inline void 
00268       align (PointCloudSource &output);
00269 
00275       inline void 
00276       align (PointCloudSource &output, const Eigen::Matrix4f& guess);
00277 
00279       inline const std::string&
00280       getClassName () const { return (reg_name_); }
00281 
00282     protected:
00284       std::string reg_name_;
00285 
00287       KdTreePtr tree_;
00288 
00290       int nr_iterations_;
00291 
00295       int max_iterations_;
00296 
00298       int ransac_iterations_;
00299 
00301       PointCloudTargetConstPtr target_;
00302 
00304       Eigen::Matrix4f final_transformation_;
00305 
00307       Eigen::Matrix4f transformation_;
00308 
00310       Eigen::Matrix4f previous_transformation_;
00311 
00315       double transformation_epsilon_;
00316 
00321       double euclidean_fitness_epsilon_;
00322 
00326       double corr_dist_threshold_;
00327 
00332       double inlier_threshold_;
00333 
00335       bool converged_;
00336 
00340       int min_number_correspondences_;
00341 
00345       std::vector<float> correspondence_distances_;                                                              
00346 
00348       TransformationEstimationPtr transformation_estimation_;
00349 
00353       boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
00354                            const std::vector<int> &indices_src,
00355                            const pcl::PointCloud<PointTarget> &cloud_tgt,
00356                            const std::vector<int> &indices_tgt)> update_visualizer_;
00357 
00364       inline bool
00365       searchForNeighbors (const PointCloudSource &cloud, int index, 
00366                           std::vector<int> &indices, std::vector<float> &distances)
00367       {
00368         int k = tree_->nearestKSearch (cloud, index, 1, indices, distances);
00369         if (k == 0)
00370           return (false);
00371         return (true);
00372       }
00373 
00374     private:
00375  
00377       virtual void 
00378       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) = 0;
00379 
00381       PointRepresentationConstPtr point_representation_;
00382     public:
00383       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00384    };
00385 }
00386 
00387 #include <pcl/registration/impl/registration.hpp>
00388 
00389 #endif  //#ifndef PCL_REGISTRATION_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:38