Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/correspondence_rejection_features.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_FEATURES_H_
00041 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_
00042 
00043 #include <pcl/registration/correspondence_rejection.h>
00044 #include <pcl/point_cloud.h>
00045 #include <pcl/point_representation.h>
00046 #include <pcl/registration/boost.h>
00047 
00048 namespace pcl
00049 {
00050   namespace registration
00051   {
00052     /** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature
00053       * descriptors. Given an input feature space, the method checks if each feature in the source cloud has a
00054       * correspondence in the target cloud, either by checking the first K (given) point correspondences, or 
00055       * by defining a tolerance threshold via a radius in feature space.
00056       * \todo explain this better.
00057       * \author Radu B. Rusu
00058       * \ingroup registration
00059       */
00060     class PCL_EXPORTS CorrespondenceRejectorFeatures: public CorrespondenceRejector
00061     {
00062       using CorrespondenceRejector::input_correspondences_;
00063       using CorrespondenceRejector::rejection_name_;
00064       using CorrespondenceRejector::getClassName;
00065 
00066       public:
00067         typedef boost::shared_ptr<CorrespondenceRejectorFeatures> Ptr;
00068         typedef boost::shared_ptr<const CorrespondenceRejectorFeatures> ConstPtr;
00069 
00070         /** \brief Empty constructor. */
00071         CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ()), features_map_ ()
00072         {
00073           rejection_name_ = "CorrespondenceRejectorFeatures";
00074         }
00075 
00076         /** \brief Empty destructor. */
00077         virtual ~CorrespondenceRejectorFeatures () {}
00078 
00079         /** \brief Get a list of valid correspondences after rejection from the original set of correspondences
00080           * \param[in] original_correspondences the set of initial correspondences given
00081           * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
00082           */
00083         void 
00084         getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
00085                                      pcl::Correspondences& remaining_correspondences);
00086 
00087         /** \brief Provide a pointer to a cloud of feature descriptors associated with the source point cloud
00088           * \param[in] source_feature a cloud of feature descriptors associated with the source point cloud
00089           * \param[in] key a string that uniquely identifies the feature
00090           */
00091         template <typename FeatureT> inline void 
00092         setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, 
00093                           const std::string &key);
00094 
00095         /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
00096           * \param[in] key a string that uniquely identifies the feature (must match the key provided by setSourceFeature)
00097           */
00098         template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00099         getSourceFeature (const std::string &key);
00100 
00101         /** \brief Provide a pointer to a cloud of feature descriptors associated with the target point cloud
00102           * \param[in] target_feature a cloud of feature descriptors associated with the target point cloud
00103           * \param[in] key a string that uniquely identifies the feature
00104           */
00105         template <typename FeatureT> inline void 
00106         setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, 
00107                           const std::string &key);
00108 
00109         /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
00110           * \param[in] key a string that uniquely identifies the feature (must match the key provided by setTargetFeature)
00111           */
00112         template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00113         getTargetFeature (const std::string &key);
00114 
00115         /** \brief Set a hard distance threshold in the feature \a FeatureT space, between source and target
00116           * features. Any feature correspondence that is above this threshold will be considered bad and will be
00117           * filtered out.
00118           * \param[in] thresh the distance threshold
00119           * \param[in] key a string that uniquely identifies the feature
00120           */
00121         template <typename FeatureT> inline void 
00122         setDistanceThreshold (double thresh, const std::string &key);
00123 
00124         /** \brief Test that all features are valid (i.e., does each key have a valid source cloud, target cloud, 
00125           * and search method)
00126           */
00127         inline bool 
00128         hasValidFeatures ();
00129 
00130         /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
00131           * \param[in] key a string that uniquely identifies the feature
00132           * \param[in] fr the point feature representation to be used 
00133           */
00134         template <typename FeatureT> inline void
00135         setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
00136                                   const std::string &key);
00137 
00138       protected:
00139 
00140         /** \brief Apply the rejection algorithm.
00141           * \param[out] correspondences the set of resultant correspondences.
00142           */
00143         inline void 
00144         applyRejection (pcl::Correspondences &correspondences)
00145         {
00146           getRemainingCorrespondences (*input_correspondences_, correspondences);
00147         }
00148 
00149         /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
00150           * distance is larger than this threshold, the points will not be ignored in the alignment process.
00151           */
00152         float max_distance_;
00153 
00154         class FeatureContainerInterface
00155         {
00156           public:
00157             /** \brief Empty destructor */
00158             virtual ~FeatureContainerInterface () {}
00159             virtual bool isValid () = 0;
00160             virtual double getCorrespondenceScore (int index) = 0;
00161             virtual bool isCorrespondenceValid (int index) = 0;
00162         };
00163 
00164         typedef boost::unordered_map<std::string, boost::shared_ptr<FeatureContainerInterface> > FeaturesMap;
00165 
00166         /** \brief An STL map containing features to use when performing the correspondence search.*/
00167         FeaturesMap features_map_;
00168 
00169         /** \brief An inner class containing pointers to the source and target feature clouds 
00170           * and the parameters needed to perform the correspondence search.  This class extends 
00171           * FeatureContainerInterface, which contains abstract methods for any methods that do not depend on the 
00172           * FeatureT --- these methods can thus be called from a pointer to FeatureContainerInterface without 
00173           * casting to the derived class.
00174           */
00175         template <typename FeatureT>
00176         class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
00177         {
00178           public:
00179             typedef typename pcl::PointCloud<FeatureT>::ConstPtr FeatureCloudConstPtr;
00180             typedef boost::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &, 
00181                                           std::vector<float> &)> SearchMethod;
00182             
00183             typedef typename pcl::PointRepresentation<FeatureT>::ConstPtr PointRepresentationConstPtr;
00184 
00185             FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
00186             {
00187             }
00188       
00189             /** \brief Empty destructor */
00190             virtual ~FeatureContainer () {}
00191 
00192             inline void 
00193             setSourceFeature (const FeatureCloudConstPtr &source_features)
00194             {
00195               source_features_ = source_features;
00196             }
00197             
00198             inline FeatureCloudConstPtr 
00199             getSourceFeature ()
00200             {
00201               return (source_features_);
00202             }
00203             
00204             inline void 
00205             setTargetFeature (const FeatureCloudConstPtr &target_features)
00206             {
00207               target_features_ = target_features;
00208             }
00209             
00210             inline FeatureCloudConstPtr 
00211             getTargetFeature ()
00212             {
00213               return (target_features_);
00214             }
00215             
00216             inline void 
00217             setDistanceThreshold (double thresh)
00218             {
00219               thresh_ = thresh;
00220             }
00221 
00222             virtual inline bool 
00223             isValid ()
00224             {
00225               if (!source_features_ || !target_features_)
00226                 return (false);
00227               else
00228                 return (source_features_->points.size () > 0 && 
00229                         target_features_->points.size () > 0);
00230             }
00231 
00232             /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
00233               * \param[in] fr the point feature representation to be used
00234               */
00235             inline void
00236             setFeatureRepresentation (const PointRepresentationConstPtr &fr)
00237             {
00238               feature_representation_ = fr;
00239             }
00240 
00241             /** \brief Obtain a score between a pair of correspondences.
00242               * \param[in] the index to check in the list of correspondences
00243               * \return score the resultant computed score
00244               */
00245             virtual inline double
00246             getCorrespondenceScore (int index)
00247             {
00248               // If no feature representation was given, reset to the default implementation for FeatureT
00249               if (!feature_representation_)
00250                 feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);
00251 
00252               // Get the source and the target feature from the list
00253               const FeatureT &feat_src = source_features_->points[index];
00254               const FeatureT &feat_tgt = target_features_->points[index];
00255 
00256               // Check if the representations are valid
00257               if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
00258               {
00259                 PCL_ERROR ("[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
00260                 return (std::numeric_limits<double>::max ());
00261               }
00262 
00263               // Set the internal feature point representation of choice
00264               Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
00265               feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
00266               Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
00267               feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);
00268 
00269               // Compute the L2 norm
00270               return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
00271             }
00272 
00273             /** \brief Check whether the correspondence pair at the given index is valid
00274               * by computing the score and testing it against the user given threshold 
00275               * \param[in] the index to check in the list of correspondences
00276               * \return true if the correspondence is good, false otherwise
00277               */
00278             virtual inline bool
00279             isCorrespondenceValid (int index)
00280             {
00281               if (getCorrespondenceScore (index) < thresh_ * thresh_)
00282                 return (true);
00283               else
00284                 return (false);
00285             }
00286              
00287           private:
00288             FeatureCloudConstPtr source_features_, target_features_;
00289             SearchMethod search_method_;
00290 
00291             /** \brief The L2 squared Euclidean threshold. */
00292             double thresh_;
00293 
00294             /** \brief The internal point feature representation used. */
00295             PointRepresentationConstPtr feature_representation_;
00296         };
00297     };
00298   }
00299 }
00300 
00301 #include <pcl/registration/impl/correspondence_rejection_features.hpp>
00302 
00303 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_ */