Point Cloud Library (PCL)  1.11.1
correspondence_rejection_features.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/registration/correspondence_rejection.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_representation.h>
46 
47 #include <unordered_map>
48 
49 namespace pcl
50 {
51  namespace registration
52  {
53  /** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature
54  * descriptors. Given an input feature space, the method checks if each feature in the source cloud has a
55  * correspondence in the target cloud, either by checking the first K (given) point correspondences, or
56  * by defining a tolerance threshold via a radius in feature space.
57  * \todo explain this better.
58  * \author Radu B. Rusu
59  * \ingroup registration
60  */
62  {
66 
67  public:
68  using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
69  using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
70 
71  /** \brief Empty constructor. */
72  CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ())
73  {
74  rejection_name_ = "CorrespondenceRejectorFeatures";
75  }
76 
77  /** \brief Empty destructor. */
79 
80  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences
81  * \param[in] original_correspondences the set of initial correspondences given
82  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
83  */
84  void
85  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
86  pcl::Correspondences& remaining_correspondences) override;
87 
88  /** \brief Provide a pointer to a cloud of feature descriptors associated with the source point cloud
89  * \param[in] source_feature a cloud of feature descriptors associated with the source point cloud
90  * \param[in] key a string that uniquely identifies the feature
91  */
92  template <typename FeatureT> inline void
93  setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature,
94  const std::string &key);
95 
96  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
97  * \param[in] key a string that uniquely identifies the feature (must match the key provided by setSourceFeature)
98  */
99  template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
100  getSourceFeature (const std::string &key);
101 
102  /** \brief Provide a pointer to a cloud of feature descriptors associated with the target point cloud
103  * \param[in] target_feature a cloud of feature descriptors associated with the target point cloud
104  * \param[in] key a string that uniquely identifies the feature
105  */
106  template <typename FeatureT> inline void
107  setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature,
108  const std::string &key);
109 
110  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
111  * \param[in] key a string that uniquely identifies the feature (must match the key provided by setTargetFeature)
112  */
113  template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
114  getTargetFeature (const std::string &key);
115 
116  /** \brief Set a hard distance threshold in the feature \a FeatureT space, between source and target
117  * features. Any feature correspondence that is above this threshold will be considered bad and will be
118  * filtered out.
119  * \param[in] thresh the distance threshold
120  * \param[in] key a string that uniquely identifies the feature
121  */
122  template <typename FeatureT> inline void
123  setDistanceThreshold (double thresh, const std::string &key);
124 
125  /** \brief Test that all features are valid (i.e., does each key have a valid source cloud, target cloud,
126  * and search method)
127  */
128  inline bool
130 
131  /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
132  * \param[in] key a string that uniquely identifies the feature
133  * \param[in] fr the point feature representation to be used
134  */
135  template <typename FeatureT> inline void
136  setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
137  const std::string &key);
138 
139  protected:
140 
141  /** \brief Apply the rejection algorithm.
142  * \param[out] correspondences the set of resultant correspondences.
143  */
144  inline void
145  applyRejection (pcl::Correspondences &correspondences) override
146  {
147  getRemainingCorrespondences (*input_correspondences_, correspondences);
148  }
149 
150  /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
151  * distance is larger than this threshold, the points will not be ignored in the alignment process.
152  */
154 
156  {
157  public:
158  /** \brief Empty destructor */
159  virtual ~FeatureContainerInterface () = default;
160  virtual bool isValid () = 0;
161  virtual double getCorrespondenceScore (int index) = 0;
162  virtual bool isCorrespondenceValid (int index) = 0;
163 
164  using Ptr = shared_ptr<FeatureContainerInterface>;
165  };
166 
167  using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
168 
169  /** \brief An STL map containing features to use when performing the correspondence search.*/
171 
172  /** \brief An inner class containing pointers to the source and target feature clouds
173  * and the parameters needed to perform the correspondence search. This class extends
174  * FeatureContainerInterface, which contains abstract methods for any methods that do not depend on the
175  * FeatureT --- these methods can thus be called from a pointer to FeatureContainerInterface without
176  * casting to the derived class.
177  */
178  template <typename FeatureT>
180  {
181  public:
183  using SearchMethod = std::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &, std::vector<float> &)>;
184 
186 
187  FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
188  {
189  }
190 
191  /** \brief Empty destructor */
193 
194  inline void
195  setSourceFeature (const FeatureCloudConstPtr &source_features)
196  {
197  source_features_ = source_features;
198  }
199 
200  inline FeatureCloudConstPtr
202  {
203  return (source_features_);
204  }
205 
206  inline void
207  setTargetFeature (const FeatureCloudConstPtr &target_features)
208  {
209  target_features_ = target_features;
210  }
211 
212  inline FeatureCloudConstPtr
214  {
215  return (target_features_);
216  }
217 
218  inline void
219  setDistanceThreshold (double thresh)
220  {
221  thresh_ = thresh;
222  }
223 
224  inline bool
225  isValid () override
226  {
227  if (!source_features_ || !target_features_)
228  return (false);
229  return (source_features_->size () > 0 &&
230  target_features_->size () > 0);
231  }
232 
233  /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
234  * \param[in] fr the point feature representation to be used
235  */
236  inline void
238  {
239  feature_representation_ = fr;
240  }
241 
242  /** \brief Obtain a score between a pair of correspondences.
243  * \param[in] index the index to check in the list of correspondences
244  * \return score the resultant computed score
245  */
246  inline double
247  getCorrespondenceScore (int index) override
248  {
249  // If no feature representation was given, reset to the default implementation for FeatureT
250  if (!feature_representation_)
251  feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);
252 
253  // Get the source and the target feature from the list
254  const FeatureT &feat_src = (*source_features_)[index];
255  const FeatureT &feat_tgt = (*target_features_)[index];
256 
257  // Check if the representations are valid
258  if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
259  {
260  PCL_ERROR ("[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
261  return (std::numeric_limits<double>::max ());
262  }
263 
264  // Set the internal feature point representation of choice
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);
269 
270  // Compute the L2 norm
271  return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
272  }
273 
274  /** \brief Check whether the correspondence pair at the given index is valid
275  * by computing the score and testing it against the user given threshold
276  * \param[in] index the index to check in the list of correspondences
277  * \return true if the correspondence is good, false otherwise
278  */
279  inline bool
280  isCorrespondenceValid (int index) override
281  {
282  return (getCorrespondenceScore (index) < thresh_ * thresh_);
283  }
284 
285  private:
286  FeatureCloudConstPtr source_features_, target_features_;
287  SearchMethod search_method_;
288 
289  /** \brief The L2 squared Euclidean threshold. */
290  double thresh_;
291 
292  /** \brief The internal point feature representation used. */
293  PointRepresentationConstPtr feature_representation_;
294  };
295  };
296  }
297 }
298 
299 #include <pcl/registration/impl/correspondence_rejection_features.hpp>
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:61
pcl
Definition: convolution.h:46
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setDistanceThreshold
void setDistanceThreshold(double thresh)
Definition: correspondence_rejection_features.h:219
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::isCorrespondenceValid
bool isCorrespondenceValid(int index) override
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
Definition: correspondence_rejection_features.h:280
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition: correspondence_rejection.h:62
pcl::registration::CorrespondenceRejectorFeatures::getRemainingCorrespondences
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.
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::isCorrespondenceValid
virtual bool isCorrespondenceValid(int index)=0
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: correspondence_rejection.h:132
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::FeatureCloudConstPtr
typename pcl::PointCloud< FeatureT >::ConstPtr FeatureCloudConstPtr
Definition: correspondence_rejection_features.h:182
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setTargetFeature
void setTargetFeature(const FeatureCloudConstPtr &target_features)
Definition: correspondence_rejection_features.h:207
pcl::registration::CorrespondenceRejectorFeatures::~CorrespondenceRejectorFeatures
~CorrespondenceRejectorFeatures()
Empty destructor.
Definition: correspondence_rejection_features.h:78
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::~FeatureContainerInterface
virtual ~FeatureContainerInterface()=default
Empty destructor.
pcl::PointRepresentation::ConstPtr
shared_ptr< const PointRepresentation< PointT > > ConstPtr
Definition: point_representation.h:79
pcl::PointCloud< FeatureT >
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getTargetFeature
FeatureCloudConstPtr getTargetFeature()
Definition: correspondence_rejection_features.h:213
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getSourceFeature
FeatureCloudConstPtr getSourceFeature()
Definition: correspondence_rejection_features.h:201
pcl::registration::CorrespondenceRejectorFeatures
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
Definition: correspondence_rejection_features.h:62
pcl::DefaultFeatureRepresentation
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
Definition: point_representation.h:222
pcl::registration::CorrespondenceRejectorFeatures::features_map_
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
Definition: correspondence_rejection_features.h:170
pcl::registration::CorrespondenceRejectorFeatures::max_distance_
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: correspondence_rejection_features.h:153
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
Definition: correspondence_rejection_features.h:180
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::Ptr
shared_ptr< FeatureContainerInterface > Ptr
Definition: correspondence_rejection_features.h:164
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::~FeatureContainer
~FeatureContainer()
Empty destructor.
Definition: correspondence_rejection_features.h:192
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::isValid
bool isValid() override
Definition: correspondence_rejection_features.h:225
pcl::registration::CorrespondenceRejectorFeatures::hasValidFeatures
bool hasValidFeatures()
Test that all features are valid (i.e., does each key have a valid source cloud, target cloud,...
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::isValid
virtual bool isValid()=0
pcl::registration::CorrespondenceRejectorFeatures::CorrespondenceRejectorFeatures
CorrespondenceRejectorFeatures()
Empty constructor.
Definition: correspondence_rejection_features.h:72
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setSourceFeature
void setSourceFeature(const FeatureCloudConstPtr &source_features)
Definition: correspondence_rejection_features.h:195
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::SearchMethod
std::function< int(const pcl::PointCloud< FeatureT > &, int, std::vector< int > &, std::vector< float > &)> SearchMethod
Definition: correspondence_rejection_features.h:183
pcl::registration::CorrespondenceRejectorFeatures::FeaturesMap
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
Definition: correspondence_rejection_features.h:167
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getCorrespondenceScore
double getCorrespondenceScore(int index) override
Obtain a score between a pair of correspondences.
Definition: correspondence_rejection_features.h:247
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::getCorrespondenceScore
virtual double getCorrespondenceScore(int index)=0
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:90
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
Definition: correspondence_rejection_features.h:156
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setFeatureRepresentation
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing features.
Definition: correspondence_rejection_features.h:237
pcl::registration::CorrespondenceRejector::input_correspondences_
CorrespondencesConstPtr input_correspondences_
The input correspondences.
Definition: correspondence_rejection.h:188
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition: correspondence_rejection.h:185
pcl::registration::CorrespondenceRejectorFeatures::applyRejection
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
Definition: correspondence_rejection_features.h:145
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:328
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition: correspondence_rejection.h:59
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::PointRepresentationConstPtr
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
Definition: correspondence_rejection_features.h:185
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::FeatureContainer
FeatureContainer()
Definition: correspondence_rejection_features.h:187