Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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.*/
167  FeaturesMap features_map_;
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
236  setFeatureRepresentation (const PointRepresentationConstPtr &fr)
237  {
238  feature_representation_ = fr;
239  }
240 
241  /** \brief Obtain a score between a pair of correspondences.
242  * \param[in] index 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] index 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_ */
boost::shared_ptr< const CorrespondenceRejectorFeatures > ConstPtr
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
CorrespondenceRejector represents the base class for correspondence rejection methods ...
boost::shared_ptr< const PointRepresentation< PointT > > ConstPtr
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
const std::string & getClassName() const
Get a string representation of the name of this class.
virtual bool isCorrespondenceValid(int index)
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
boost::shared_ptr< CorrespondenceRejectorFeatures > Ptr
virtual double getCorrespondenceScore(int index)
Obtain a score between a pair of correspondences.
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
boost::unordered_map< std::string, boost::shared_ptr< FeatureContainerInterface > > FeaturesMap
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
CorrespondencesConstPtr input_correspondences_
The input correspondences.
boost::function< int(const pcl::PointCloud< FeatureT > &, int, std::vector< int > &, std::vector< float > &)> SearchMethod
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...
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.