Point Cloud Library (PCL)  1.14.1-dev
correspondence_estimation.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/common/io.h> // for getFields
44 #include <pcl/registration/correspondence_types.h>
45 #include <pcl/search/kdtree.h>
46 #include <pcl/memory.h>
47 #include <pcl/pcl_base.h>
48 #include <pcl/pcl_macros.h>
49 
50 #include <string>
51 
52 namespace pcl {
53 namespace registration {
54 /** \brief Abstract @b CorrespondenceEstimationBase class.
55  * All correspondence estimation methods should inherit from this.
56  * \author Radu B. Rusu
57  * \ingroup registration
58  */
59 template <typename PointSource, typename PointTarget, typename Scalar = float>
60 class CorrespondenceEstimationBase : public PCLBase<PointSource> {
61 public:
62  using Ptr =
63  shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
64  using ConstPtr =
65  shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
66 
67  // using PCLBase<PointSource>::initCompute;
72 
74  using KdTreePtr = typename KdTree::Ptr;
75  using KdTreeConstPtr = typename KdTree::ConstPtr;
76 
80 
84 
88 
92 
93  /** \brief Empty constructor. */
95  : corr_name_("CorrespondenceEstimationBase")
96  , tree_(new pcl::search::KdTree<PointTarget>)
97  , tree_reciprocal_(new pcl::search::KdTree<PointSource>)
98  , target_()
101  {}
102 
103  /** \brief Empty destructor */
104  ~CorrespondenceEstimationBase() override = default;
105 
106  /** \brief Provide a pointer to the input source
107  * (e.g., the point cloud that we want to align to the target)
108  *
109  * \param[in] cloud the input point cloud source
110  */
111  inline void
113  {
114  source_cloud_updated_ = true;
116  input_fields_ = pcl::getFields<PointSource>();
117  }
118 
119  /** \brief Get a pointer to the input point cloud dataset target. */
120  inline PointCloudSourceConstPtr const
122  {
123  return (input_);
124  }
125 
126  /** \brief Provide a pointer to the input target
127  * (e.g., the point cloud that we want to align the input source to)
128  * \param[in] cloud the input point cloud target
129  */
130  inline void
132 
133  /** \brief Get a pointer to the input point cloud dataset target. */
134  inline PointCloudTargetConstPtr const
136  {
137  return (target_);
138  }
139 
140  /** \brief Set the number of threads to use.
141  * \param nr_threads the number of hardware threads to use (0 sets the value back to
142  * automatic)
143  */
144  void
145  setNumberOfThreads(unsigned int nr_threads)
146  {
147 #ifdef _OPENMP
148  num_threads_ = nr_threads != 0 ? nr_threads : omp_get_num_procs();
149 #else
150  if (nr_threads != 1) {
151  PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n");
152  }
153  num_threads_ = 1;
154 #endif
155  }
156 
157  /** \brief See if this rejector requires source normals */
158  virtual bool
160  {
161  return (false);
162  }
163 
164  /** \brief Abstract method for setting the source normals */
165  virtual void
167  {
168  PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
169  "input source normals\n",
170  getClassName().c_str());
171  }
172 
173  /** \brief See if this rejector requires target normals */
174  virtual bool
176  {
177  return (false);
178  }
179 
180  /** \brief Abstract method for setting the target normals */
181  virtual void
183  {
184  PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
185  "input target normals\n",
186  getClassName().c_str());
187  }
188 
189  /** \brief Provide a pointer to the vector of indices that represent the
190  * input source point cloud.
191  * \param[in] indices a pointer to the vector of indices
192  */
193  inline void
194  setIndicesSource(const IndicesPtr& indices)
195  {
196  setIndices(indices);
197  }
198 
199  /** \brief Get a pointer to the vector of indices used for the source dataset. */
200  inline IndicesPtr const
202  {
203  return (indices_);
204  }
205 
206  /** \brief Provide a pointer to the vector of indices that represent the input target
207  * point cloud. \param[in] indices a pointer to the vector of indices
208  */
209  inline void
210  setIndicesTarget(const IndicesPtr& indices)
211  {
212  target_cloud_updated_ = true;
213  target_indices_ = indices;
214  }
215 
216  /** \brief Get a pointer to the vector of indices used for the target dataset. */
217  inline IndicesPtr const
219  {
220  return (target_indices_);
221  }
222 
223  /** \brief Provide a pointer to the search object used to find correspondences in
224  * the target cloud.
225  * \param[in] tree a pointer to the spatial search object.
226  * \param[in] force_no_recompute If set to true, this tree will NEVER be
227  * recomputed, regardless of calls to setInputTarget. Only use if you are
228  * confident that the tree will be set correctly.
229  */
230  inline void
231  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
232  {
233  tree_ = tree;
234  force_no_recompute_ = force_no_recompute;
235  // Since we just set a new tree, we need to check for updates
236  target_cloud_updated_ = true;
237  }
238 
239  /** \brief Get a pointer to the search method used to find correspondences in the
240  * target cloud. */
241  inline KdTreePtr
243  {
244  return (tree_);
245  }
246 
247  /** \brief Provide a pointer to the search object used to find correspondences in
248  * the source cloud (usually used by reciprocal correspondence finding).
249  * \param[in] tree a pointer to the spatial search object.
250  * \param[in] force_no_recompute If set to true, this tree will NEVER be
251  * recomputed, regardless of calls to setInputSource. Only use if you are
252  * extremely confident that the tree will be set correctly.
253  */
254  inline void
256  bool force_no_recompute = false)
257  {
258  tree_reciprocal_ = tree;
259  force_no_recompute_reciprocal_ = force_no_recompute;
260  // Since we just set a new tree, we need to check for updates
261  source_cloud_updated_ = true;
262  }
263 
264  /** \brief Get a pointer to the search method used to find correspondences in the
265  * source cloud. */
266  inline KdTreeReciprocalPtr
268  {
269  return (tree_reciprocal_);
270  }
271 
272  /** \brief Determine the correspondences between input and target cloud.
273  * \param[out] correspondences the found correspondences (index of query point, index
274  * of target point, distance) \param[in] max_distance maximum allowed distance between
275  * correspondences
276  */
277  virtual void
279  pcl::Correspondences& correspondences,
280  double max_distance = std::numeric_limits<double>::max()) = 0;
281 
282  /** \brief Determine the reciprocal correspondences between input and target cloud.
283  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
284  * correspondence, and Tgt_i has Src_i as one.
285  *
286  * \param[out] correspondences the found correspondences (index of query and target
287  * point, distance) \param[in] max_distance maximum allowed distance between
288  * correspondences
289  */
290  virtual void
292  pcl::Correspondences& correspondences,
293  double max_distance = std::numeric_limits<double>::max()) = 0;
294 
295  /** \brief Provide a boost shared pointer to the PointRepresentation for target cloud
296  * to be used when searching for nearest neighbors.
297  *
298  * \param[in] point_representation the PointRepresentation to be used by the
299  * k-D tree for nearest neighbor search
300  */
301  inline void
303  {
304  point_representation_ = point_representation;
305  }
306 
307  /** \brief Provide a boost shared pointer to the PointRepresentation for source cloud
308  * to be used when searching for nearest neighbors.
309  *
310  * \param[in] point_representation the PointRepresentation to be used by the
311  * k-D tree for nearest neighbor search
312  */
313  inline void
315  const PointRepresentationReciprocalConstPtr& point_representation_reciprocal)
316  {
317  point_representation_reciprocal_ = point_representation_reciprocal;
318  }
319 
320  /** \brief Clone and cast to CorrespondenceEstimationBase */
322  clone() const = 0;
323 
324 protected:
325  /** \brief The correspondence estimation method name. */
326  std::string corr_name_;
327 
328  /** \brief A pointer to the spatial search object used for the target dataset. */
330 
331  /** \brief A pointer to the spatial search object used for the source dataset. */
333 
334  /** \brief The input point cloud dataset target. */
336 
337  /** \brief The target point cloud dataset indices. */
339 
340  /** \brief The target point representation used (internal). */
342 
343  /** \brief The source point representation used (internal). */
345 
346  /** \brief The transformed input source point cloud dataset. */
348 
349  /** \brief The types of input point fields available. */
350  std::vector<pcl::PCLPointField> input_fields_;
351 
352  /** \brief Abstract class get name method. */
353  inline const std::string&
354  getClassName() const
355  {
356  return (corr_name_);
357  }
358 
359  /** \brief Internal computation initialization. */
360  bool
361  initCompute();
362 
363  /** \brief Internal computation initialization for reciprocal correspondences. */
364  bool
366 
367  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
368  * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
369  * cloud every time the determineCorrespondences () method is called. */
371  /** \brief Variable that stores whether we have a new source cloud, meaning we need to
372  * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
373  * source cloud every time the determineCorrespondences () method is called. */
375  /** \brief A flag which, if set, means the tree operating on the target cloud
376  * will never be recomputed*/
377  bool force_no_recompute_{false};
378 
379  /** \brief A flag which, if set, means the tree operating on the source cloud
380  * will never be recomputed*/
382 
383  unsigned int num_threads_{1};
384 };
385 
386 /** \brief @b CorrespondenceEstimation represents a simple class for
387  * determining correspondences between target and query point
388  * sets/features, using nearest neighbor search.
389  *
390  * Code example:
391  *
392  * \code
393  * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
394  * // ... read or fill in source and target
395  * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
396  * est.setInputSource (source);
397  * est.setInputTarget (target);
398  *
399  * pcl::Correspondences all_correspondences;
400  * // Determine all reciprocal correspondences
401  * est.determineReciprocalCorrespondences (all_correspondences);
402  * \endcode
403  *
404  * \author Radu B. Rusu, Michael Dixon, Dirk Holz
405  * \ingroup registration
406  */
407 template <typename PointSource, typename PointTarget, typename Scalar = float>
409 : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
410 public:
411  using Ptr = shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
412  using ConstPtr =
413  shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
414 
433 
434  using KdTree =
436  using KdTreePtr = typename CorrespondenceEstimationBase<PointSource,
437  PointTarget,
438  Scalar>::KdTreePtr;
439  using KdTreeConstPtr = typename CorrespondenceEstimationBase<PointSource,
440  PointTarget,
441  Scalar>::KdTreeConstPtr;
448 
452 
456 
460 
463  typename KdTreeReciprocal::PointRepresentationConstPtr;
464 
465  /** \brief Empty constructor. */
466  CorrespondenceEstimation() { corr_name_ = "CorrespondenceEstimation"; }
467 
468  /** \brief Empty destructor */
469  ~CorrespondenceEstimation() override = default;
470 
471  /** \brief Determine the correspondences between input and target cloud.
472  * \param[out] correspondences the found correspondences (index of query point, index
473  * of target point, distance) \param[in] max_distance maximum allowed distance between
474  * correspondences
475  */
476  void
478  pcl::Correspondences& correspondences,
479  double max_distance = std::numeric_limits<double>::max()) override;
480 
481  /** \brief Determine the reciprocal correspondences between input and target cloud.
482  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
483  * correspondence, and Tgt_i has Src_i as one.
484  *
485  * \param[out] correspondences the found correspondences (index of query and target
486  * point, distance) \param[in] max_distance maximum allowed distance between
487  * correspondences
488  */
489  void
491  pcl::Correspondences& correspondences,
492  double max_distance = std::numeric_limits<double>::max()) override;
493 
494  /** \brief Clone and cast to CorrespondenceEstimationBase */
496  clone() const override
497  {
499  return (copy);
500  }
501 
502 protected:
504 };
505 } // namespace registration
506 } // namespace pcl
507 
508 #include <pcl/registration/impl/correspondence_estimation.hpp>
typename PointRepresentation::ConstPtr PointRepresentationConstPtr
Definition: kdtree.h:66
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:150
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
Abstract CorrespondenceEstimationBase class.
void setPointRepresentationReciprocal(const PointRepresentationReciprocalConstPtr &point_representation_reciprocal)
Provide a boost shared pointer to the PointRepresentation for source cloud to be used when searching ...
typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
void setNumberOfThreads(unsigned int nr_threads)
Set the number of threads to use.
void setSearchMethodSource(const KdTreeReciprocalPtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the source cloud (usually used...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
PointCloudTargetConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
bool initCompute()
Internal computation initialization.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the reciprocal correspondences between input and target cloud.
IndicesPtr const getIndicesTarget()
Get a pointer to the vector of indices used for the target dataset.
const std::string & getClassName() const
Abstract class get name method.
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a boost shared pointer to the PointRepresentation for target cloud to be used when searching ...
bool source_cloud_updated_
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
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.
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
PointRepresentationReciprocalConstPtr point_representation_reciprocal_
The source point representation used (internal).
typename KdTreeReciprocal::PointRepresentationConstPtr PointRepresentationReciprocalConstPtr
KdTreeReciprocalPtr getSearchMethodSource() const
Get a pointer to the search method used to find correspondences in the source cloud.
PointRepresentationConstPtr point_representation_
The target point representation used (internal).
IndicesPtr const getIndicesSource()
Get a pointer to the vector of indices used for the source dataset.
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
std::vector< pcl::PCLPointField > input_fields_
The types of input point fields available.
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
std::string corr_name_
The correspondence estimation method name.
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
virtual CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const =0
Clone and cast to CorrespondenceEstimationBase.
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
void setIndicesSource(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input source point cloud.
KdTreePtr tree_
A pointer to the spatial search object used for the target dataset.
PointCloudSourceConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object used for the source dataset.
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
typename KdTreeReciprocal::ConstPtr KdTreeReciprocalConstPtr
virtual void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the correspondences between input and target cloud.
~CorrespondenceEstimationBase() override=default
Empty destructor.
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
bool force_no_recompute_reciprocal_
A flag which, if set, means the tree operating on the source cloud will never be recomputed.
IndicesPtr target_indices_
The target point cloud dataset indices.
void setIndicesTarget(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input target point cloud.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
bool force_no_recompute_
A flag which, if set, means the tree operating on the target cloud will never be recomputed.
KdTreePtr getSearchMethodTarget() const
Get a pointer to the search method used to find correspondences in the target cloud.
CorrespondenceEstimation represents a simple class for determining correspondences between target and...
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocalPtr KdTreeReciprocalPtr
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
typename KdTreeReciprocal::PointRepresentationConstPtr PointRepresentationReciprocalConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocalConstPtr KdTreeReciprocalConstPtr
CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const override
Clone and cast to CorrespondenceEstimationBase.
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTree KdTree
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeConstPtr KdTreeConstPtr
shared_ptr< CorrespondenceEstimation< PointSource, PointTarget, Scalar > > Ptr
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
shared_ptr< const CorrespondenceEstimation< PointSource, PointTarget, Scalar > > ConstPtr
typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
~CorrespondenceEstimation() override=default
Empty destructor.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreePtr KdTreePtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocal KdTreeReciprocal
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
shared_ptr< const KdTree< PointT, Tree > > ConstPtr
Definition: kdtree.h:76
typename PointRepresentation< PointT >::ConstPtr PointRepresentationConstPtr
Definition: kdtree.h:80
Defines functions, macros and traits for allocating and using memory.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
Defines all the PCL and non-PCL macros used.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr