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