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