Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/correspondence_rejection_median_distance.h
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  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_
00041 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_
00042 
00043 #include <pcl/registration/correspondence_rejection.h>
00044 #include <pcl/point_cloud.h>
00045 
00046 namespace pcl
00047 {
00048   namespace registration
00049   {
00050     /** \brief CorrespondenceRejectorMedianDistance implements a simple correspondence
00051       * rejection method based on thresholding based on the median distance between the
00052       * correspondences.
00053       *
00054       * \note If \ref setInputCloud and \ref setInputTarget are given, then the
00055       * distances between correspondences will be estimated using the given XYZ
00056       * data, and not read from the set of input correspondences.
00057       *
00058       * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
00059       * \ingroup registration
00060       */
00061     class PCL_EXPORTS CorrespondenceRejectorMedianDistance: public CorrespondenceRejector
00062     {
00063       using CorrespondenceRejector::input_correspondences_;
00064       using CorrespondenceRejector::rejection_name_;
00065       using CorrespondenceRejector::getClassName;
00066 
00067       public:
00068         typedef boost::shared_ptr<CorrespondenceRejectorMedianDistance> Ptr;
00069         typedef boost::shared_ptr<const CorrespondenceRejectorMedianDistance> ConstPtr;
00070 
00071         /** \brief Empty constructor. */
00072         CorrespondenceRejectorMedianDistance () 
00073           : median_distance_ (0)
00074           , factor_ (1.0)
00075           , data_container_ ()
00076         {
00077           rejection_name_ = "CorrespondenceRejectorMedianDistance";
00078         }
00079 
00080         /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
00081           * \param[in] original_correspondences the set of initial correspondences given
00082           * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
00083           */
00084         void 
00085         getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
00086                                      pcl::Correspondences& remaining_correspondences);
00087 
00088         /** \brief Get the median distance used for thresholding in correspondence rejection. */
00089         inline double
00090         getMedianDistance () const { return (median_distance_); };
00091 
00092         /** \brief Provide a source point cloud dataset (must contain XYZ
00093           * data!), used to compute the correspondence distance.  
00094           * \param[in] cloud a cloud containing XYZ data
00095           */
00096         template <typename PointT> inline void 
00097         setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
00098         {
00099           if (!data_container_)
00100             data_container_.reset (new DataContainer<PointT>);
00101           boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
00102         }
00103 
00104         /** \brief Provide a source point cloud dataset (must contain XYZ
00105           * data!), used to compute the correspondence distance.  
00106           * \param[in] cloud a cloud containing XYZ data
00107           */
00108         template <typename PointT> inline void 
00109         setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
00110         {
00111           PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
00112           if (!data_container_)
00113             data_container_.reset (new DataContainer<PointT>);
00114           boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
00115         }
00116 
00117         /** \brief Provide a target point cloud dataset (must contain XYZ
00118           * data!), used to compute the correspondence distance.  
00119           * \param[in] target a cloud containing XYZ data
00120           */
00121         template <typename PointT> inline void 
00122         setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target)
00123         {
00124           if (!data_container_)
00125             data_container_.reset (new DataContainer<PointT>);
00126           boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
00127         }
00128 
00129         /** \brief Provide a pointer to the search object used to find correspondences in
00130           * the target cloud.
00131           * \param[in] tree a pointer to the spatial search object.
00132           * \param[in] force_no_recompute If set to true, this tree will NEVER be 
00133           * recomputed, regardless of calls to setInputTarget. Only use if you are 
00134           * confident that the tree will be set correctly.
00135           */
00136         template <typename PointT> inline void
00137         setSearchMethodTarget (const boost::shared_ptr<pcl::search::KdTree<PointT> > &tree, 
00138                                bool force_no_recompute = false) 
00139         { 
00140           boost::static_pointer_cast< DataContainer<PointT> > 
00141             (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
00142         }
00143 
00144         /** \brief Set the factor for correspondence rejection. Points with distance greater than median times factor
00145          *  will be rejected
00146          *  \param[in] factor value
00147          */
00148         inline void
00149         setMedianFactor (double factor) { factor_ = factor; };
00150 
00151         /** \brief Get the factor used for thresholding in correspondence rejection. */
00152         inline double
00153         getMedianFactor () const { return factor_; };
00154 
00155       protected:
00156 
00157         /** \brief Apply the rejection algorithm.
00158           * \param[out] correspondences the set of resultant correspondences.
00159           */
00160         inline void 
00161         applyRejection (pcl::Correspondences &correspondences)
00162         {
00163           getRemainingCorrespondences (*input_correspondences_, correspondences);
00164         }
00165 
00166         /** \brief The median distance threshold between two correspondent points in source <-> target.
00167           */
00168         double median_distance_;
00169 
00170         /** \brief The factor for correspondence rejection. Points with distance greater than median times factor
00171          *  will be rejected
00172          */
00173         double factor_;
00174 
00175         typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
00176 
00177         /** \brief A pointer to the DataContainer object containing the input and target point clouds */
00178         DataContainerPtr data_container_;
00179     };
00180   }
00181 }
00182 
00183 #include <pcl/registration/impl/correspondence_rejection_median_distance.hpp>
00184 
00185 #endif    // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_