43 #include <pcl/registration/correspondence_rejection.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_representation.h>
46 #include <pcl/registration/boost.h>
48 #include <unordered_map>
52 namespace registration
69 using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
70 using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
75 rejection_name_ =
"CorrespondenceRejectorFeatures";
93 template <
typename FeatureT>
inline void
95 const std::string &key);
101 getSourceFeature (
const std::string &key);
107 template <
typename FeatureT>
inline void
109 const std::string &key);
115 getTargetFeature (
const std::string &key);
123 template <
typename FeatureT>
inline void
124 setDistanceThreshold (
double thresh,
const std::string &key);
136 template <
typename FeatureT>
inline void
138 const std::string &key);
148 getRemainingCorrespondences (*input_correspondences_, correspondences);
165 using Ptr = shared_ptr<FeatureContainerInterface>;
168 using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
179 template <
typename FeatureT>
188 FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
198 source_features_ = source_features;
201 inline FeatureCloudConstPtr
204 return (source_features_);
210 target_features_ = target_features;
213 inline FeatureCloudConstPtr
216 return (target_features_);
228 if (!source_features_ || !target_features_)
230 return (source_features_->points.size () > 0 &&
231 target_features_->points.size () > 0);
240 feature_representation_ = fr;
251 if (!feature_representation_)
255 const FeatureT &feat_src = source_features_->points[index];
256 const FeatureT &feat_tgt = target_features_->points[index];
259 if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
261 PCL_ERROR (
"[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
262 return (std::numeric_limits<double>::max ());
266 Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
267 feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
268 Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
269 feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);
272 return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
283 return (getCorrespondenceScore (index) < thresh_ * thresh_);
287 FeatureCloudConstPtr source_features_, target_features_;
288 SearchMethod search_method_;
294 PointRepresentationConstPtr feature_representation_;
300 #include <pcl/registration/impl/correspondence_rejection_features.hpp>
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< const PointRepresentation< PointT > > ConstPtr
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
typename pcl::PointCloud< FeatureT >::ConstPtr FeatureCloudConstPtr
~FeatureContainer()
Empty destructor.
FeatureCloudConstPtr getTargetFeature()
FeatureCloudConstPtr getSourceFeature()
void setDistanceThreshold(double thresh)
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing features.
void setSourceFeature(const FeatureCloudConstPtr &source_features)
bool isCorrespondenceValid(int index) override
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
double getCorrespondenceScore(int index) override
Obtain a score between a pair of correspondences.
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
std::function< int(const pcl::PointCloud< FeatureT > &, int, std::vector< int > &, std::vector< float > &)> SearchMethod
void setTargetFeature(const FeatureCloudConstPtr &target_features)
shared_ptr< FeatureContainerInterface > Ptr
virtual ~FeatureContainerInterface()=default
Empty destructor.
virtual double getCorrespondenceScore(int index)=0
virtual bool isCorrespondenceValid(int index)=0
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
bool hasValidFeatures()
Test that all features are valid (i.e., does each key have a valid source cloud, target cloud,...
CorrespondenceRejectorFeatures()
Empty constructor.
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
~CorrespondenceRejectorFeatures()
Empty destructor.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
CorrespondenceRejector represents the base class for correspondence rejection methods
shared_ptr< const CorrespondenceRejector > ConstPtr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< CorrespondenceRejector > Ptr
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences