Point Cloud Library (PCL)  1.8.1
correspondence_rejection.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 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_
42 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_
43 
44 #include <pcl/registration/correspondence_types.h>
45 #include <pcl/registration/correspondence_sorting.h>
46 #include <pcl/console/print.h>
47 #include <pcl/common/transforms.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/search/kdtree.h>
50 
51 namespace pcl
52 {
53  namespace registration
54  {
55  /** @b CorrespondenceRejector represents the base class for correspondence rejection methods
56  * \author Dirk Holz
57  * \ingroup registration
58  */
60  {
61  public:
62  typedef boost::shared_ptr<CorrespondenceRejector> Ptr;
63  typedef boost::shared_ptr<const CorrespondenceRejector> ConstPtr;
64 
65  /** \brief Empty constructor. */
67  : rejection_name_ ()
69  {}
70 
71  /** \brief Empty destructor. */
73 
74  /** \brief Provide a pointer to the vector of the input correspondences.
75  * \param[in] correspondences the const boost shared pointer to a correspondence vector
76  */
77  virtual inline void
79  {
80  input_correspondences_ = correspondences;
81  };
82 
83  /** \brief Get a pointer to the vector of the input correspondences.
84  * \return correspondences the const boost shared pointer to a correspondence vector
85  */
88 
89  /** \brief Run correspondence rejection
90  * \param[out] correspondences Vector of correspondences that have not been rejected.
91  */
92  inline void
94  {
96  return;
97 
98  applyRejection (correspondences);
99  }
100 
101  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
102  * Pure virtual. Compared to \a getCorrespondences this function is
103  * stateless, i.e., input correspondences do not need to be provided beforehand,
104  * but are directly provided in the function call.
105  * \param[in] original_correspondences the set of initial correspondences given
106  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
107  */
108  virtual inline void
109  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
110  pcl::Correspondences& remaining_correspondences) = 0;
111 
112  /** \brief Determine the indices of query points of
113  * correspondences that have been rejected, i.e., the difference
114  * between the input correspondences (set via \a setInputCorrespondences)
115  * and the given correspondence vector.
116  * \param[in] correspondences Vector of correspondences after rejection
117  * \param[out] indices Vector of query point indices of those correspondences
118  * that have been rejected.
119  */
120  inline void
122  std::vector<int>& indices)
123  {
125  {
126  PCL_WARN ("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n", getClassName ().c_str ());
127  return;
128  }
129 
130  pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
131  }
132 
133  /** \brief Get a string representation of the name of this class. */
134  inline const std::string&
135  getClassName () const { return (rejection_name_); }
136 
137 
138  /** \brief See if this rejector requires source points */
139  virtual bool
141  { return (false); }
142 
143  /** \brief Abstract method for setting the source cloud */
144  virtual void
146  {
147  PCL_WARN ("[pcl::registration::%s::setSourcePoints] This class does not require an input source cloud", getClassName ().c_str ());
148  }
149 
150  /** \brief See if this rejector requires source normals */
151  virtual bool
153  { return (false); }
154 
155  /** \brief Abstract method for setting the source normals */
156  virtual void
158  {
159  PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ());
160  }
161  /** \brief See if this rejector requires a target cloud */
162  virtual bool
164  { return (false); }
165 
166  /** \brief Abstract method for setting the target cloud */
167  virtual void
169  {
170  PCL_WARN ("[pcl::registration::%s::setTargetPoints] This class does not require an input target cloud", getClassName ().c_str ());
171  }
172 
173  /** \brief See if this rejector requires target normals */
174  virtual bool
176  { return (false); }
177 
178  /** \brief Abstract method for setting the target normals */
179  virtual void
181  {
182  PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ());
183  }
184 
185  protected:
186 
187  /** \brief The name of the rejection method. */
188  std::string rejection_name_;
189 
190  /** \brief The input correspondences. */
192 
193  /** \brief Abstract rejection method. */
194  virtual void
195  applyRejection (Correspondences &correspondences) = 0;
196  };
197 
198  /** @b DataContainerInterface provides a generic interface for computing correspondence scores between correspondent
199  * points in the input and target clouds
200  * \ingroup registration
201  */
203  {
204  public:
206  virtual double getCorrespondenceScore (int index) = 0;
207  virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0;
208  virtual double getCorrespondenceScoreFromNormals (const pcl::Correspondence &) = 0;
209  };
210 
211  /** @b DataContainer is a container for the input and target point clouds and implements the interface
212  * to compute correspondence scores between correspondent points in the input and target clouds
213  * \ingroup registration
214  */
215  template <typename PointT, typename NormalT = pcl::PointNormal>
217  {
219  typedef typename PointCloud::Ptr PointCloudPtr;
220  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
221 
222  typedef typename pcl::search::KdTree<PointT>::Ptr KdTreePtr;
223 
225  typedef typename Normals::Ptr NormalsPtr;
226  typedef typename Normals::ConstPtr NormalsConstPtr;
227 
228  public:
229 
230  /** \brief Empty constructor. */
231  DataContainer (bool needs_normals = false)
232  : input_ ()
233  , input_transformed_ ()
234  , target_ ()
235  , input_normals_ ()
236  , input_normals_transformed_ ()
237  , target_normals_ ()
238  , tree_ (new pcl::search::KdTree<PointT>)
239  , class_name_ ("DataContainer")
240  , needs_normals_ (needs_normals)
241  , target_cloud_updated_ (true)
242  , force_no_recompute_ (false)
243  {
244  }
245 
246  /** \brief Empty destructor */
247  virtual ~DataContainer () {}
248 
249  /** \brief Provide a source point cloud dataset (must contain XYZ
250  * data!), used to compute the correspondence distance.
251  * \param[in] cloud a cloud containing XYZ data
252  */
253  PCL_DEPRECATED ("[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
254  void
255  setInputCloud (const PointCloudConstPtr &cloud);
256 
257  /** \brief Get a pointer to the input point cloud dataset target. */
258  PCL_DEPRECATED ("[pcl::registration::DataContainer::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.")
259  PointCloudConstPtr const
260  getInputCloud ();
261 
262  /** \brief Provide a source point cloud dataset (must contain XYZ
263  * data!), used to compute the correspondence distance.
264  * \param[in] cloud a cloud containing XYZ data
265  */
266  inline void
267  setInputSource (const PointCloudConstPtr &cloud)
268  {
269  input_ = cloud;
270  }
271 
272  /** \brief Get a pointer to the input point cloud dataset target. */
273  inline PointCloudConstPtr const
274  getInputSource () { return (input_); }
275 
276  /** \brief Provide a target point cloud dataset (must contain XYZ
277  * data!), used to compute the correspondence distance.
278  * \param[in] target a cloud containing XYZ data
279  */
280  inline void
281  setInputTarget (const PointCloudConstPtr &target)
282  {
283  target_ = target;
284  target_cloud_updated_ = true;
285  }
286 
287  /** \brief Get a pointer to the input point cloud dataset target. */
288  inline PointCloudConstPtr const
289  getInputTarget () { return (target_); }
290 
291  /** \brief Provide a pointer to the search object used to find correspondences in
292  * the target cloud.
293  * \param[in] tree a pointer to the spatial search object.
294  * \param[in] force_no_recompute If set to true, this tree will NEVER be
295  * recomputed, regardless of calls to setInputTarget. Only use if you are
296  * confident that the tree will be set correctly.
297  */
298  inline void
299  setSearchMethodTarget (const KdTreePtr &tree,
300  bool force_no_recompute = false)
301  {
302  tree_ = tree;
303  if (force_no_recompute)
304  {
305  force_no_recompute_ = true;
306  }
307  target_cloud_updated_ = true;
308  }
309 
310  /** \brief Set the normals computed on the input point cloud
311  * \param[in] normals the normals computed for the input cloud
312  */
313  inline void
314  setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; }
315 
316  /** \brief Get the normals computed on the input point cloud */
317  inline NormalsConstPtr
318  getInputNormals () { return (input_normals_); }
319 
320  /** \brief Set the normals computed on the target point cloud
321  * \param[in] normals the normals computed for the input cloud
322  */
323  inline void
324  setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; }
325 
326  /** \brief Get the normals computed on the target point cloud */
327  inline NormalsConstPtr
328  getTargetNormals () { return (target_normals_); }
329 
330  /** \brief Get the correspondence score for a point in the input cloud
331  * \param[in] index index of the point in the input cloud
332  */
333  inline double
335  {
336  if ( target_cloud_updated_ && !force_no_recompute_ )
337  {
338  tree_->setInputCloud (target_);
339  }
340  std::vector<int> indices (1);
341  std::vector<float> distances (1);
342  if (tree_->nearestKSearch (input_->points[index], 1, indices, distances))
343  return (distances[0]);
344  else
345  return (std::numeric_limits<double>::max ());
346  }
347 
348  /** \brief Get the correspondence score for a given pair of correspondent points
349  * \param[in] corr Correspondent points
350  */
351  inline double
353  {
354  // Get the source and the target feature from the list
355  const PointT &src = input_->points[corr.index_query];
356  const PointT &tgt = target_->points[corr.index_match];
357 
358  return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
359  }
360 
361  /** \brief Get the correspondence score for a given pair of correspondent points based on
362  * the angle betweeen the normals. The normmals for the in put and target clouds must be
363  * set before using this function
364  * \param[in] corr Correspondent points
365  */
366  inline double
368  {
369  //assert ( (input_normals_->points.size () != 0) && (target_normals_->points.size () != 0) && "Normals are not set for the input and target point clouds");
370  assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds");
371  const NormalT &src = input_normals_->points[corr.index_query];
372  const NormalT &tgt = target_normals_->points[corr.index_match];
373  return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2])));
374  }
375 
376  private:
377  /** \brief The input point cloud dataset */
378  PointCloudConstPtr input_;
379 
380  /** \brief The input transformed point cloud dataset */
381  PointCloudPtr input_transformed_;
382 
383  /** \brief The target point cloud datase. */
384  PointCloudConstPtr target_;
385 
386  /** \brief Normals to the input point cloud */
387  NormalsConstPtr input_normals_;
388 
389  /** \brief Normals to the input point cloud */
390  NormalsPtr input_normals_transformed_;
391 
392  /** \brief Normals to the target point cloud */
393  NormalsConstPtr target_normals_;
394 
395  /** \brief A pointer to the spatial search object. */
396  KdTreePtr tree_;
397 
398  /** \brief The name of the rejection method. */
399  std::string class_name_;
400 
401  /** \brief Should the current data container use normals? */
402  bool needs_normals_;
403 
404  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
405  * This way, we avoid rebuilding the kd-tree */
406  bool target_cloud_updated_;
407 
408  /** \brief A flag which, if set, means the tree operating on the target cloud
409  * will never be recomputed*/
410  bool force_no_recompute_;
411 
412 
413 
414  /** \brief Get a string representation of the name of this class. */
415  inline const std::string&
416  getClassName () const { return (class_name_); }
417  };
418  }
419 }
420 
421 #include <pcl/registration/impl/correspondence_rejection.hpp>
422 
423 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_ */
424 
A point structure representing normal coordinates and the surface curvature estimate.
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
DataContainer is a container for the input and target point clouds and implements the interface to co...
void setSearchMethodTarget(const KdTreePtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud.
int index_match
Index of the matching (target) point.
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
PointCloudConstPtr const getInputCloud()
Get a pointer to the input point cloud dataset target.
boost::shared_ptr< const CorrespondenceRejector > ConstPtr
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
CorrespondenceRejector represents the base class for correspondence rejection methods
double getCorrespondenceScoreFromNormals(const pcl::Correspondence &corr)
Get the correspondence score for a given pair of correspondent points based on the angle betweeen the...
void setInputCloud(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
virtual void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences)=0
Get a list of valid correspondences after rejection from the original set of correspondences.
virtual bool requiresSourcePoints() const
See if this rejector requires source points.
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
Correspondence represents a match between two entities (e.g., points, descriptors,...
const std::string & getClassName() const
Get a string representation of the name of this class.
DataContainer(bool needs_normals=false)
Empty constructor.
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
virtual double getCorrespondenceScore(int index)=0
int index_query
Index of the query (source) point.
virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target cloud.
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
void setInputNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
boost::shared_ptr< const Correspondences > CorrespondencesConstPtr
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
void getRejectedQueryIndices(const pcl::Correspondences &correspondences_before, const pcl::Correspondences &correspondences_after, std::vector< int > &indices, bool presorting_required=true)
Get the query points of correspondences that are present in one correspondence vector but not in the ...
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source cloud.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
void setInputTarget(const PointCloudConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
double getCorrespondenceScore(const pcl::Correspondence &corr)
Get the correspondence score for a given pair of correspondent points.
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
std::string rejection_name_
The name of the rejection method.
void getRejectedQueryIndices(const pcl::Correspondences &correspondences, std::vector< int > &indices)
Determine the indices of query points of correspondences that have been rejected, i....
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual bool requiresTargetPoints() const
See if this rejector requires a target cloud.
double getCorrespondenceScore(int index)
Get the correspondence score for a point in the input cloud.
boost::shared_ptr< CorrespondenceRejector > Ptr
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
virtual ~DataContainer()
Empty destructor.
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:56
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
virtual double getCorrespondenceScoreFromNormals(const pcl::Correspondence &)=0