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