Point Cloud Library (PCL)  1.3.1
correspondence_rejection_features.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_FEATURES_H_
00037 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_
00038 
00039 #include <boost/function.hpp>
00040 #include <boost/unordered_map.hpp>
00041 #include <pcl/registration/correspondence_rejection.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_representation.h>
00044 
00045 namespace pcl
00046 {
00047   namespace registration
00048   {
00058     class CorrespondenceRejectorFeatures: public CorrespondenceRejector
00059     {
00060       using CorrespondenceRejector::input_correspondences_;
00061       using CorrespondenceRejector::rejection_name_;
00062       using CorrespondenceRejector::getClassName;
00063 
00064       public:
00066         CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ())
00067         {
00068           rejection_name_ = "CorrespondenceRejectorFeatures";
00069         }
00070 
00075         void 
00076         getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
00077                                      pcl::Correspondences& remaining_correspondences);
00078 
00083         template <typename FeatureT> inline void 
00084         setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, 
00085                           const std::string &key);
00086 
00090         template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00091         getSourceFeature (const std::string &key);
00092 
00097         template <typename FeatureT> inline void 
00098         setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, 
00099                           const std::string &key);
00100 
00104         template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00105         getTargetFeature (const std::string &key);
00106 
00113         template <typename FeatureT> inline void 
00114         setDistanceThreshold (double thresh, const std::string &key);
00115 
00119         inline bool 
00120         hasValidFeatures ();
00121 
00126         template <typename FeatureT> inline void
00127         setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
00128                                   const std::string &key);
00129 
00130      protected:
00131 
00132         void 
00133         applyRejection (pcl::Correspondences &correspondences);
00134 
00135         float max_distance_;
00136 
00137         class FeatureContainerInterface
00138         {
00139           public:
00140             virtual bool isValid () = 0;
00141             virtual double getCorrespondenceScore (int index) = 0;
00142             virtual bool isCorrespondenceValid (int index) = 0;
00143         };
00144 
00145         typedef boost::unordered_map<std::string, boost::shared_ptr<FeatureContainerInterface> > FeaturesMap;
00146 
00148         FeaturesMap features_map_;
00149 
00156         template <typename FeatureT>
00157         class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
00158         {
00159           public:
00160             typedef typename pcl::PointCloud<FeatureT>::ConstPtr FeatureCloudConstPtr;
00161             typedef boost::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &, 
00162                                           std::vector<float> &)> SearchMethod;
00163             
00164             typedef typename pcl::PointRepresentation<FeatureT>::ConstPtr PointRepresentationConstPtr;
00165 
00166             FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
00167             {
00168             }
00169 
00170             inline void 
00171             setSourceFeature (const FeatureCloudConstPtr &source_features)
00172             {
00173               source_features_ = source_features;
00174             }
00175             
00176             inline FeatureCloudConstPtr 
00177             getSourceFeature ()
00178             {
00179               return (source_features_);
00180             }
00181             
00182             inline void 
00183             setTargetFeature (const FeatureCloudConstPtr &target_features)
00184             {
00185               target_features_ = target_features;
00186             }
00187             
00188             inline FeatureCloudConstPtr 
00189             getTargetFeature ()
00190             {
00191               return (target_features_);
00192             }
00193             
00194             inline void 
00195             setDistanceThreshold (double thresh)
00196             {
00197               thresh_ = thresh;
00198             }
00199 
00200             virtual inline bool 
00201             isValid ()
00202             {
00203               if (!source_features_ || !target_features_)
00204                 return (false);
00205               else
00206                 return (source_features_->points.size () > 0 && 
00207                         target_features_->points.size () > 0);
00208             }
00209 
00213             inline void
00214             setFeatureRepresentation (const PointRepresentationConstPtr &fr)
00215             {
00216               feature_representation_ = fr;
00217             }
00218 
00223             virtual inline double
00224             getCorrespondenceScore (int index)
00225             {
00226               // If no feature representation was given, reset to the default implementation for FeatureT
00227               if (!feature_representation_)
00228                 feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);
00229 
00230               // Get the source and the target feature from the list
00231               const FeatureT &feat_src = source_features_->points[index];
00232               const FeatureT &feat_tgt = target_features_->points[index];
00233 
00234               // Check if the representations are valid
00235               if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
00236               {
00237                 PCL_ERROR ("[pcl::registration::CorrespondenceRejectorFeatures::getCorrespondenceScore] Invalid feature representation given!\n");
00238                 return (std::numeric_limits<double>::max ());
00239               }
00240 
00241               // Set the internal feature point representation of choice
00242               Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
00243               feature_representation_->vectorize ((FeatureT)feat_src, feat_src_ptr);
00244               Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
00245               feature_representation_->vectorize ((FeatureT)feat_tgt, feat_tgt_ptr);
00246 
00247               // Compute the L2 norm
00248               return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
00249             }
00250 
00256             virtual inline bool
00257             isCorrespondenceValid (int index)
00258             {
00259               if (getCorrespondenceScore (index) < thresh_ * thresh_)
00260                 return (true);
00261               else
00262                 return (false);
00263             }
00264              
00265           private:
00266             FeatureCloudConstPtr source_features_, target_features_;
00267             SearchMethod search_method_;
00268 
00270             double thresh_;
00271 
00273             PointRepresentationConstPtr feature_representation_;
00274         };
00275     };
00276   }
00277 }
00278 
00279 #include "pcl/registration/impl/correspondence_rejection_features.hpp"
00280 
00281 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines