Point Cloud Library (PCL)  1.14.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 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
134  hasValidFeatures();
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>
shared_ptr< const CorrespondenceRejector > ConstPtr
shared_ptr< CorrespondenceRejector > Ptr
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods ...
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
const std::string & getClassName() const
Get a string representation of the name of this class.
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
shared_ptr< const PointRepresentation< PointT > > ConstPtr
bool isCorrespondenceValid(int index) override
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
std::function< int(const pcl::PointCloud< FeatureT > &, int, pcl::Indices &, std::vector< float > &)> SearchMethod
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
std::string rejection_name_
The name of the rejection method.
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing features...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
double getCorrespondenceScore(int index) override
Obtain a score between a pair of correspondences.
An inner class containing pointers to the source and target feature clouds and the parameters needed ...