Point Cloud Library (PCL)  1.10.0-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 #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:
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
130  hasValidFeatures ();
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 
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>
shared_ptr< const CorrespondenceRejector > ConstPtr
shared_ptr< CorrespondenceRejector > Ptr
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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.
std::function< int(const pcl::PointCloud< FeatureT > &, int, std::vector< int > &, std::vector< float > &)> SearchMethod
const std::string & getClassName() const
Get a string representation of the name of this class.
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
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.
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:416
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 ...
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
#define PCL_EXPORTS
Definition: pcl_macros.h:253