Point Cloud Library (PCL)  1.3.1
correspondence_rejection_features.hpp
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_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
00037 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_
00038 
00040 void
00041 pcl::registration::CorrespondenceRejectorFeatures::applyRejection (
00042     pcl::Correspondences &correspondences)
00043 {
00044   unsigned int number_valid_correspondences = 0;
00045   correspondences.resize (input_correspondences_->size ());
00046   // For each set of features, go over each correspondence from input_correspondences_
00047   for (size_t i = 0; i < input_correspondences_->size (); ++i)
00048   {
00049     // Go over the map of features
00050     for (FeaturesMap::const_iterator it = features_map_.begin (); it != features_map_.end (); ++it)
00051     {
00052       // Check if the score in feature space is above the given threshold
00053       // (assume that the number of feature correspondences is the same as the number of point correspondences)
00054       if (!it->second->isCorrespondenceValid (i))
00055         break;
00056 
00057       correspondences[number_valid_correspondences] = (*input_correspondences_)[i];
00058       ++number_valid_correspondences;
00059     }
00060   }
00061   correspondences.resize (number_valid_correspondences);
00062 }
00063 
00065 void
00066 pcl::registration::CorrespondenceRejectorFeatures::getRemainingCorrespondences (
00067     const pcl::Correspondences& original_correspondences, 
00068     pcl::Correspondences& remaining_correspondences)
00069 {
00070   unsigned int number_valid_correspondences = 0;
00071   remaining_correspondences.resize (original_correspondences.size ());
00072   // For each set of features, go over each correspondence from input_correspondences_
00073   for (size_t i = 0; i < input_correspondences_->size (); ++i)
00074   {
00075     // Go over the map of features
00076     for (FeaturesMap::const_iterator it = features_map_.begin (); it != features_map_.end (); ++it)
00077     {
00078       // Check if the score in feature space is above the given threshold
00079       // (assume that the number of feature correspondenecs is the same as the number of point correspondences)
00080       if (!it->second->isCorrespondenceValid (i))
00081         break;
00082 
00083       remaining_correspondences[number_valid_correspondences] = original_correspondences[i];
00084       ++number_valid_correspondences;
00085     }
00086   }
00087   remaining_correspondences.resize (number_valid_correspondences);
00088 }
00089 
00091 template <typename FeatureT> inline void 
00092 pcl::registration::CorrespondenceRejectorFeatures::setSourceFeature (
00093     const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, const std::string &key)
00094 {
00095   if (features_map_.count (key) == 0)
00096     features_map_[key].reset (new FeatureContainer<FeatureT>);
00097   boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setSourceFeature (source_feature);
00098 }
00099 
00101 template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00102 pcl::registration::CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key)
00103 {
00104   if (features_map_.count (key) == 0)
00105     return (boost::shared_ptr<pcl::PointCloud<const FeatureT> > ());
00106   else
00107     return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getSourceFeature ());
00108 }
00109 
00111 template <typename FeatureT> inline void 
00112 pcl::registration::CorrespondenceRejectorFeatures::setTargetFeature (
00113     const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, const std::string &key)
00114 {
00115   if (features_map_.count (key) == 0)
00116     features_map_[key].reset (new FeatureContainer<FeatureT>);
00117   boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setTargetFeature (target_feature);
00118 }
00119 
00121 template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
00122 pcl::registration::CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key)
00123 {
00124   typedef pcl::PointCloud<FeatureT> FeatureCloud;
00125   typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr;
00126 
00127   if (features_map_.count (key) == 0)
00128     return (boost::shared_ptr<const pcl::PointCloud<FeatureT> > ());
00129   else
00130     return (boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->getTargetFeature ());
00131 }
00132 
00134 template <typename FeatureT> inline void 
00135 pcl::registration::CorrespondenceRejectorFeatures::setDistanceThreshold (
00136     double thresh, const std::string &key)
00137 {
00138   if (features_map_.count (key) == 0)
00139     features_map_[key].reset (new FeatureContainer<FeatureT>);
00140   boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setDistanceThreshold (thresh);
00141 }
00142 
00144 inline bool
00145 pcl::registration::CorrespondenceRejectorFeatures::hasValidFeatures ()
00146 {
00147   if (features_map_.empty ())
00148     return (false);
00149   typename FeaturesMap::const_iterator feature_itr;
00150   for (feature_itr = features_map_.begin (); feature_itr != features_map_.end (); ++feature_itr)
00151     if (!feature_itr->second->isValid ())
00152       return (false);
00153   return (true);
00154 }
00155 
00157 template <typename FeatureT> inline void 
00158 pcl::registration::CorrespondenceRejectorFeatures::setFeatureRepresentation (
00159   const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
00160   const std::string &key)
00161 {
00162   if (features_map_.count (key) == 0)
00163     features_map_[key].reset (new FeatureContainer<FeatureT>);
00164   boost::static_pointer_cast<FeatureContainer<FeatureT> > (features_map_[key])->setFeatureRepresentation (fr);
00165 }
00166 
00167 
00168 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines