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