43 #include <pcl/registration/correspondence_rejection.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_representation.h>
47 #include <unordered_map>
51 namespace registration
68 using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
69 using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
74 rejection_name_ =
"CorrespondenceRejectorFeatures";
92 template <
typename FeatureT>
inline void
94 const std::string &key);
100 getSourceFeature (
const std::string &key);
106 template <
typename FeatureT>
inline void
108 const std::string &key);
114 getTargetFeature (
const std::string &key);
122 template <
typename FeatureT>
inline void
123 setDistanceThreshold (
double thresh,
const std::string &key);
135 template <
typename FeatureT>
inline void
137 const std::string &key);
147 getRemainingCorrespondences (*input_correspondences_, correspondences);
164 using Ptr = shared_ptr<FeatureContainerInterface>;
167 using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
178 template <
typename FeatureT>
187 FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
197 source_features_ = source_features;
200 inline FeatureCloudConstPtr
203 return (source_features_);
209 target_features_ = target_features;
212 inline FeatureCloudConstPtr
215 return (target_features_);
227 if (!source_features_ || !target_features_)
229 return (source_features_->size () > 0 &&
230 target_features_->size () > 0);
239 feature_representation_ = fr;
250 if (!feature_representation_)
254 const FeatureT &feat_src = (*source_features_)[index];
255 const FeatureT &feat_tgt = (*target_features_)[index];
258 if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
260 PCL_ERROR (
"[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
261 return (std::numeric_limits<double>::max ());
265 Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
266 feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
267 Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
268 feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);
271 return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
282 return (getCorrespondenceScore (index) < thresh_ * thresh_);
286 FeatureCloudConstPtr source_features_, target_features_;
287 SearchMethod search_method_;
293 PointRepresentationConstPtr feature_representation_;
299 #include <pcl/registration/impl/correspondence_rejection_features.hpp>