Point Cloud Library (PCL)
1.7.0
|
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_ */