Point Cloud Library (PCL)  1.3.1
correspondence_rejection_distance.h
Go to the documentation of this file.
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  *
00035  */
00036 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_DISTANCE_H_
00037 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_DISTANCE_H_
00038 
00039 #include <pcl/registration/correspondence_rejection.h>
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/kdtree/kdtree_flann.h>
00042 
00043 namespace pcl
00044 {
00045   namespace registration
00046   {
00059     class CorrespondenceRejectorDistance: public CorrespondenceRejector
00060     {
00061       using CorrespondenceRejector::input_correspondences_;
00062       using CorrespondenceRejector::rejection_name_;
00063       using CorrespondenceRejector::getClassName;
00064 
00065       public:
00066 
00068         CorrespondenceRejectorDistance () : max_distance_(std::numeric_limits<float>::max ()),
00069                                             data_container_ ()
00070         {
00071           rejection_name_ = "CorrespondenceRejectorDistance";
00072         }
00073 
00078         inline void 
00079         getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
00080                                      pcl::Correspondences& remaining_correspondences);
00081 
00087         virtual inline void 
00088         setMaximumDistance (float distance) { max_distance_ = distance * distance; };
00089 
00091         inline float 
00092         getMaximumDistance () { return std::sqrt (max_distance_); };
00093 
00098         template <typename PointT> inline void 
00099         setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
00100         {
00101           if (!data_container_)
00102             data_container_.reset (new DataContainer<PointT>);
00103           boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputCloud (cloud);
00104         }
00105 
00110         template <typename PointT> inline void 
00111         setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
00112         {
00113           if (!data_container_)
00114             data_container_.reset (new DataContainer<PointT>);
00115           boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
00116         }
00117 
00118       protected:
00119 
00120         void 
00121         applyRejection (pcl::Correspondences &correspondences);
00122 
00126         float max_distance_;
00127 
00128         class DataContainerInterface
00129         {
00130           public:
00131             virtual double getCorrespondenceScore (int index) = 0;
00132             virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0;
00133         };
00134 
00135         template <typename PointT>
00136         class DataContainer : public DataContainerInterface
00137         {
00138           typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
00139           typedef typename pcl::KdTree<PointT>::Ptr KdTreePtr;
00140           
00141           public:
00142 
00143             DataContainer () : input_ (), target_ ()
00144             {
00145               tree_.reset (new pcl::KdTreeFLANN<PointT>);
00146             }
00147 
00148             inline void 
00149             setInputCloud (const PointCloudConstPtr &cloud)
00150             {
00151               input_ = cloud;
00152             }
00153 
00154             inline void 
00155             setInputTarget (const PointCloudConstPtr &target)
00156             {
00157               target_ = target;
00158               tree_->setInputCloud (target_);
00159             }
00160 
00161             inline double 
00162             getCorrespondenceScore (int index)
00163             {
00164               std::vector<int> indices (1);
00165               std::vector<float> distances (1);
00166               if (tree_->nearestKSearch (input_->points[index], 1, indices, distances))
00167               {
00168                 return (distances[0]);
00169               }
00170               else
00171                 return (std::numeric_limits<double>::max ());
00172             }
00173 
00174             inline double 
00175             getCorrespondenceScore (const pcl::Correspondence &corr)
00176             {
00177               // Get the source and the target feature from the list
00178               const PointT &src = input_->points[corr.index_query];
00179               const PointT &tgt = target_->points[corr.index_match];
00180 
00181               return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
00182             }
00183 
00184           private:
00185             PointCloudConstPtr input_, target_;
00186             KdTreePtr tree_;
00187         };
00188 
00189         typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
00190 
00191         DataContainerPtr data_container_;
00192     };
00193 
00194   }
00195 }
00196 
00197 #include "pcl/registration/impl/correspondence_rejection_distance.hpp"
00198 
00199 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_DISTANCE_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines