Point Cloud Library (PCL)  1.12.1-dev
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 namespace registration {
51 /** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method
52  * based on a set of feature descriptors. Given an input feature space, the method
53  * checks if each feature in the source cloud has a correspondence in the target cloud,
54  * either by checking the first K (given) point correspondences, or by defining a
55  * tolerance threshold via a radius in feature space. \todo explain this better. \author
56  * Radu B. Rusu \ingroup registration
57  */
62 
63 public:
64  using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
65  using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
66 
67  /** \brief Empty constructor. */
68  CorrespondenceRejectorFeatures() : max_distance_(std::numeric_limits<float>::max())
69  {
70  rejection_name_ = "CorrespondenceRejectorFeatures";
71  }
72 
73  /** \brief Empty destructor. */
74  ~CorrespondenceRejectorFeatures() override = default;
75 
76  /** \brief Get a list of valid correspondences after rejection from the original set
77  * of correspondences \param[in] original_correspondences the set of initial
78  * correspondences given \param[out] remaining_correspondences the resultant filtered
79  * set of remaining correspondences
80  */
81  void
82  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
83  pcl::Correspondences& remaining_correspondences) override;
84 
85  /** \brief Provide a pointer to a cloud of feature descriptors associated with the
86  * source point cloud \param[in] source_feature a cloud of feature descriptors
87  * associated with the source point cloud \param[in] key a string that uniquely
88  * identifies the feature
89  */
90  template <typename FeatureT>
91  inline void
92  setSourceFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& source_feature,
93  const std::string& key);
94 
95  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
96  * given \a key \param[in] key a string that uniquely identifies the feature (must
97  * match the key provided by setSourceFeature)
98  */
99  template <typename FeatureT>
101  getSourceFeature(const std::string& key);
102 
103  /** \brief Provide a pointer to a cloud of feature descriptors associated with the
104  * target point cloud \param[in] target_feature a cloud of feature descriptors
105  * associated with the target point cloud \param[in] key a string that uniquely
106  * identifies the feature
107  */
108  template <typename FeatureT>
109  inline void
110  setTargetFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& target_feature,
111  const std::string& key);
112 
113  /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
114  * given \a key \param[in] key a string that uniquely identifies the feature (must
115  * match the key provided by setTargetFeature)
116  */
117  template <typename FeatureT>
119  getTargetFeature(const std::string& key);
120 
121  /** \brief Set a hard distance threshold in the feature \a FeatureT space, between
122  * source and target features. Any feature correspondence that is above this threshold
123  * will be considered bad and will be filtered out. \param[in] thresh the distance
124  * threshold \param[in] key a string that uniquely identifies the feature
125  */
126  template <typename FeatureT>
127  inline void
128  setDistanceThreshold(double thresh, const std::string& key);
129 
130  /** \brief Test that all features are valid (i.e., does each key have a valid source
131  * cloud, target cloud, and search method)
132  */
133  inline bool
135 
136  /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
137  * comparing features \param[in] key a string that uniquely identifies the feature
138  * \param[in] fr the point feature representation to be used
139  */
140  template <typename FeatureT>
141  inline void
142  setFeatureRepresentation(
144  const std::string& key);
145 
146 protected:
147  /** \brief Apply the rejection algorithm.
148  * \param[out] correspondences the set of resultant correspondences.
149  */
150  inline void
151  applyRejection(pcl::Correspondences& correspondences) override
152  {
153  getRemainingCorrespondences(*input_correspondences_, correspondences);
154  }
155 
156  /** \brief The maximum distance threshold between two correspondent points in source
157  * <-> target. If the distance is larger than this threshold, the points will not be
158  * ignored in the alignment process.
159  */
161 
163  public:
164  /** \brief Empty destructor */
165  virtual ~FeatureContainerInterface() = default;
166  virtual bool
167  isValid() = 0;
168  virtual double
169  getCorrespondenceScore(int index) = 0;
170  virtual bool
171  isCorrespondenceValid(int index) = 0;
172 
173  using Ptr = shared_ptr<FeatureContainerInterface>;
174  };
175 
176  using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
177 
178  /** \brief An STL map containing features to use when performing the correspondence
179  * search.*/
181 
182  /** \brief An inner class containing pointers to the source and target feature clouds
183  * and the parameters needed to perform the correspondence search. This class extends
184  * FeatureContainerInterface, which contains abstract methods for any methods that do
185  * not depend on the FeatureT --- these methods can thus be called from a pointer to
186  * FeatureContainerInterface without casting to the derived class.
187  */
188  template <typename FeatureT>
191  public:
193  using SearchMethod = std::function<int(
194  const pcl::PointCloud<FeatureT>&, int, pcl::Indices&, std::vector<float>&)>;
195 
198 
200  : thresh_(std::numeric_limits<double>::max()), feature_representation_()
201  {}
202 
203  /** \brief Empty destructor */
204  ~FeatureContainer() override = default;
205 
206  inline void
207  setSourceFeature(const FeatureCloudConstPtr& source_features)
208  {
209  source_features_ = source_features;
210  }
211 
212  inline FeatureCloudConstPtr
214  {
215  return (source_features_);
216  }
217 
218  inline void
219  setTargetFeature(const FeatureCloudConstPtr& target_features)
220  {
221  target_features_ = target_features;
222  }
223 
224  inline FeatureCloudConstPtr
226  {
227  return (target_features_);
228  }
229 
230  inline void
231  setDistanceThreshold(double thresh)
232  {
233  thresh_ = thresh;
234  }
235 
236  inline bool
237  isValid() override
238  {
239  if (!source_features_ || !target_features_)
240  return (false);
241  return (source_features_->size() > 0 && target_features_->size() > 0);
242  }
243 
244  /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
245  * comparing features \param[in] fr the point feature representation to be used
246  */
247  inline void
249  {
250  feature_representation_ = fr;
251  }
252 
253  /** \brief Obtain a score between a pair of correspondences.
254  * \param[in] index the index to check in the list of correspondences
255  * \return score the resultant computed score
256  */
257  inline double
258  getCorrespondenceScore(int index) override
259  {
260  // If no feature representation was given, reset to the default implementation for
261  // FeatureT
262  if (!feature_representation_)
263  feature_representation_.reset(new DefaultFeatureRepresentation<FeatureT>);
264 
265  // Get the source and the target feature from the list
266  const FeatureT& feat_src = (*source_features_)[index];
267  const FeatureT& feat_tgt = (*target_features_)[index];
268 
269  // Check if the representations are valid
270  if (!feature_representation_->isValid(feat_src) ||
271  !feature_representation_->isValid(feat_tgt)) {
272  PCL_ERROR("[pcl::registration::%s::getCorrespondenceScore] Invalid feature "
273  "representation given!\n",
274  this->getClassName().c_str());
275  return (std::numeric_limits<double>::max());
276  }
277 
278  // Set the internal feature point representation of choice
279  Eigen::VectorXf feat_src_ptr =
280  Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
281  feature_representation_->vectorize(FeatureT(feat_src), feat_src_ptr);
282  Eigen::VectorXf feat_tgt_ptr =
283  Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
284  feature_representation_->vectorize(FeatureT(feat_tgt), feat_tgt_ptr);
285 
286  // Compute the L2 norm
287  return ((feat_src_ptr - feat_tgt_ptr).squaredNorm());
288  }
289 
290  /** \brief Check whether the correspondence pair at the given index is valid
291  * by computing the score and testing it against the user given threshold
292  * \param[in] index the index to check in the list of correspondences
293  * \return true if the correspondence is good, false otherwise
294  */
295  inline bool
296  isCorrespondenceValid(int index) override
297  {
298  return (getCorrespondenceScore(index) < thresh_ * thresh_);
299  }
300 
301  private:
302  FeatureCloudConstPtr source_features_, target_features_;
303  SearchMethod search_method_;
304 
305  /** \brief The L2 squared Euclidean threshold. */
306  double thresh_;
307 
308  /** \brief The internal point feature representation used. */
309  PointRepresentationConstPtr feature_representation_;
310  };
311 };
312 } // namespace registration
313 } // namespace pcl
314 
315 #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
Definition: point_cloud.h:414
shared_ptr< const PointRepresentation< PointT > > ConstPtr
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
std::function< int(const pcl::PointCloud< FeatureT > &, int, pcl::Indices &, std::vector< float > &)> SearchMethod
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing 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
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() override=default
Empty destructor.
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_EXPORTS
Definition: pcl_macros.h:323