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