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>
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
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:328