Point Cloud Library (PCL)  1.11.0
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 = shared_ptr<CorrespondenceRejector>;
62  using ConstPtr = 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 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 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:
202  using Ptr = shared_ptr<DataContainerInterface>;
203  using ConstPtr = shared_ptr<const DataContainerInterface>;
204 
205  virtual ~DataContainerInterface () = default;
206  virtual double getCorrespondenceScore (int index) = 0;
207  virtual double getCorrespondenceScore (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  using PointCloudPtr = typename PointCloud::Ptr;
220  using PointCloudConstPtr = typename PointCloud::ConstPtr;
221 
222  using KdTreePtr = typename pcl::search::KdTree<PointT>::Ptr;
223 
225  using NormalsPtr = typename Normals::Ptr;
226  using NormalsConstPtr = typename Normals::ConstPtr;
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 */
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  inline void
254  setInputSource (const PointCloudConstPtr &cloud)
255  {
256  input_ = cloud;
257  }
258 
259  /** \brief Get a pointer to the input point cloud dataset target. */
260  inline PointCloudConstPtr const
261  getInputSource () { return (input_); }
262 
263  /** \brief Provide a target point cloud dataset (must contain XYZ
264  * data!), used to compute the correspondence distance.
265  * \param[in] target a cloud containing XYZ data
266  */
267  inline void
268  setInputTarget (const PointCloudConstPtr &target)
269  {
270  target_ = target;
271  target_cloud_updated_ = true;
272  }
273 
274  /** \brief Get a pointer to the input point cloud dataset target. */
275  inline PointCloudConstPtr const
276  getInputTarget () { return (target_); }
277 
278  /** \brief Provide a pointer to the search object used to find correspondences in
279  * the target cloud.
280  * \param[in] tree a pointer to the spatial search object.
281  * \param[in] force_no_recompute If set to true, this tree will NEVER be
282  * recomputed, regardless of calls to setInputTarget. Only use if you are
283  * confident that the tree will be set correctly.
284  */
285  inline void
286  setSearchMethodTarget (const KdTreePtr &tree,
287  bool force_no_recompute = false)
288  {
289  tree_ = tree;
290  if (force_no_recompute)
291  {
292  force_no_recompute_ = true;
293  }
294  target_cloud_updated_ = true;
295  }
296 
297  /** \brief Set the normals computed on the input point cloud
298  * \param[in] normals the normals computed for the input cloud
299  */
300  inline void
301  setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; }
302 
303  /** \brief Get the normals computed on the input point cloud */
304  inline NormalsConstPtr
305  getInputNormals () { return (input_normals_); }
306 
307  /** \brief Set the normals computed on the target point cloud
308  * \param[in] normals the normals computed for the input cloud
309  */
310  inline void
311  setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; }
312 
313  /** \brief Get the normals computed on the target point cloud */
314  inline NormalsConstPtr
315  getTargetNormals () { return (target_normals_); }
316 
317  /** \brief Get the correspondence score for a point in the input cloud
318  * \param[in] index index of the point in the input cloud
319  */
320  inline double
321  getCorrespondenceScore (int index) override
322  {
323  if ( target_cloud_updated_ && !force_no_recompute_ )
324  {
325  tree_->setInputCloud (target_);
326  }
327  std::vector<int> indices (1);
328  std::vector<float> distances (1);
329  if (tree_->nearestKSearch (input_->points[index], 1, indices, distances))
330  return (distances[0]);
331  return (std::numeric_limits<double>::max ());
332  }
333 
334  /** \brief Get the correspondence score for a given pair of correspondent points
335  * \param[in] corr Correspondent points
336  */
337  inline double
339  {
340  // Get the source and the target feature from the list
341  const PointT &src = input_->points[corr.index_query];
342  const PointT &tgt = target_->points[corr.index_match];
343 
344  return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
345  }
346 
347  /** \brief Get the correspondence score for a given pair of correspondent points based on
348  * the angle between the normals. The normmals for the in put and target clouds must be
349  * set before using this function
350  * \param[in] corr Correspondent points
351  */
352  inline double
354  {
355  //assert ( (input_normals_->points.size () != 0) && (target_normals_->points.size () != 0) && "Normals are not set for the input and target point clouds");
356  assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds");
357  const NormalT &src = input_normals_->points[corr.index_query];
358  const NormalT &tgt = target_normals_->points[corr.index_match];
359  return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2])));
360  }
361 
362  private:
363  /** \brief The input point cloud dataset */
364  PointCloudConstPtr input_;
365 
366  /** \brief The input transformed point cloud dataset */
367  PointCloudPtr input_transformed_;
368 
369  /** \brief The target point cloud datase. */
370  PointCloudConstPtr target_;
371 
372  /** \brief Normals to the input point cloud */
373  NormalsConstPtr input_normals_;
374 
375  /** \brief Normals to the input point cloud */
376  NormalsPtr input_normals_transformed_;
377 
378  /** \brief Normals to the target point cloud */
379  NormalsConstPtr target_normals_;
380 
381  /** \brief A pointer to the spatial search object. */
382  KdTreePtr tree_;
383 
384  /** \brief The name of the rejection method. */
385  std::string class_name_;
386 
387  /** \brief Should the current data container use normals? */
388  bool needs_normals_;
389 
390  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
391  * This way, we avoid rebuilding the kd-tree */
392  bool target_cloud_updated_;
393 
394  /** \brief A flag which, if set, means the tree operating on the target cloud
395  * will never be recomputed*/
396  bool force_no_recompute_;
397 
398 
399 
400  /** \brief Get a string representation of the name of this class. */
401  inline const std::string&
402  getClassName () const { return (class_name_); }
403  };
404  }
405 }
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:61
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:353
pcl::registration::DataContainerInterface::ConstPtr
shared_ptr< const DataContainerInterface > ConstPtr
Definition: correspondence_rejection.h:203
pcl
Definition: convolution.h:46
pcl::Correspondence::index_match
int index_match
Index of the matching (target) point.
Definition: correspondence.h:64
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:813
pcl::registration::CorrespondenceRejector::setInputCorrespondences
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
Definition: correspondence_rejection.h:75
pcl::registration::CorrespondenceRejector::requiresTargetNormals
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
Definition: correspondence_rejection.h:172
pcl::KdTree
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:57
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition: correspondence_rejection.h:62
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:90
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: correspondence_rejection.h:132
pcl::registration::DataContainerInterface::getCorrespondenceScore
virtual double getCorrespondenceScore(const pcl::Correspondence &)=0
pcl::registration::DataContainer::getInputNormals
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
Definition: correspondence_rejection.h:305
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::registration::DataContainer::getInputTarget
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
Definition: correspondence_rejection.h:276
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:202
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:254
pcl::registration::DataContainer::DataContainer
DataContainer(bool needs_normals=false)
Empty constructor.
Definition: correspondence_rejection.h:231
pcl::registration::CorrespondenceRejector::requiresSourcePoints
virtual bool requiresSourcePoints() const
See if this rejector requires source points.
Definition: correspondence_rejection.h:137
pcl::registration::CorrespondenceRejector::getCorrespondences
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
Definition: correspondence_rejection.h:90
pcl::PCLPointCloud2::ConstPtr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
Definition: PCLPointCloud2.h:35
pcl::registration::DataContainerInterface
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
Definition: correspondence_rejection.h:200
pcl::registration::CorrespondenceRejector::getInputCorrespondences
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
Definition: correspondence_rejection.h:84
pcl::registration::DataContainer::getCorrespondenceScore
double getCorrespondenceScore(int index) override
Get the correspondence score for a point in the input cloud.
Definition: correspondence_rejection.h:321
pcl::registration::DataContainer::getInputSource
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
Definition: correspondence_rejection.h:261
pcl::registration::DataContainer
DataContainer is a container for the input and target point clouds and implements the interface to co...
Definition: correspondence_rejection.h:217
pcl::registration::CorrespondenceRejector::setTargetPoints
virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target cloud.
Definition: correspondence_rejection.h:165
pcl::registration::DataContainer::getTargetNormals
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
Definition: correspondence_rejection.h:315
pcl::registration::CorrespondenceRejector::~CorrespondenceRejector
virtual ~CorrespondenceRejector()
Empty destructor.
Definition: correspondence_rejection.h:69
pcl::registration::DataContainer::setInputNormals
void setInputNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
Definition: correspondence_rejection.h:301
pcl::registration::DataContainerInterface::getCorrespondenceScore
virtual double getCorrespondenceScore(int index)=0
pcl::registration::DataContainerInterface::getCorrespondenceScoreFromNormals
virtual double getCorrespondenceScoreFromNormals(const pcl::Correspondence &)=0
pcl::Correspondence::index_query
int index_query
Index of the query (source) point.
Definition: correspondence.h:62
pcl::getRejectedQueryIndices
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 ...
pcl::registration::CorrespondenceRejector::setSourcePoints
virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source cloud.
Definition: correspondence_rejection.h:142
pcl::registration::DataContainer::setTargetNormals
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
Definition: correspondence_rejection.h:311
pcl::registration::DataContainer::~DataContainer
~DataContainer()
Empty destructor.
Definition: correspondence_rejection.h:247
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:286
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:118
pcl::registration::DataContainerInterface::~DataContainerInterface
virtual ~DataContainerInterface()=default
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::registration::CorrespondenceRejector::CorrespondenceRejector
CorrespondenceRejector()
Empty constructor.
Definition: correspondence_rejection.h:65
pcl::registration::CorrespondenceRejector::setTargetNormals
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
Definition: correspondence_rejection.h:177
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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:268
pcl::registration::CorrespondenceRejector::requiresSourceNormals
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
Definition: correspondence_rejection.h:149
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88
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:188
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition: correspondence_rejection.h:185
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:338
pcl::registration::CorrespondenceRejector::setSourceNormals
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
Definition: correspondence_rejection.h:154
pcl::registration::CorrespondenceRejector::requiresTargetPoints
virtual bool requiresTargetPoints() const
See if this rejector requires a target cloud.
Definition: correspondence_rejection.h:160
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition: correspondence_rejection.h:59
pcl::registration::CorrespondenceRejector::applyRejection
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.