Point Cloud Library (PCL)  1.12.1-dev
correspondence_estimation_normal_shooting.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_estimation.h>
44 #include <pcl/registration/correspondence_types.h>
45 
46 namespace pcl {
47 namespace registration {
48 /** \brief @b CorrespondenceEstimationNormalShooting computes
49  * correspondences as points in the target cloud which have minimum
50  * distance to normals computed on the input cloud
51  *
52  * Code example:
53  *
54  * \code
55  * pcl::PointCloud<pcl::PointNormal>::Ptr source, target;
56  * // ... read or fill in source and target
57  * pcl::CorrespondenceEstimationNormalShooting
58  * <pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> est;
59  * est.setInputSource (source);
60  * est.setSourceNormals (source);
61  *
62  * est.setInputTarget (target);
63  *
64  * // Test the first 10 correspondences for each point in source, and return the best
65  * est.setKSearch (10);
66  *
67  * pcl::Correspondences all_correspondences;
68  * // Determine all correspondences
69  * est.determineCorrespondences (all_correspondences);
70  * \endcode
71  *
72  * \author Aravindhan K. Krishnan, Radu B. Rusu
73  * \ingroup registration
74  */
75 template <typename PointSource,
76  typename PointTarget,
77  typename NormalT,
78  typename Scalar = float>
80 : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
81 public:
82  using Ptr = shared_ptr<CorrespondenceEstimationNormalShooting<PointSource,
83  PointTarget,
84  NormalT,
85  Scalar>>;
86  using ConstPtr = shared_ptr<const CorrespondenceEstimationNormalShooting<PointSource,
87  PointTarget,
88  NormalT,
89  Scalar>>;
90 
103 
105  using KdTreePtr = typename KdTree::Ptr;
106 
110 
114 
118 
119  /** \brief Empty constructor.
120  *
121  * \note
122  * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
123  */
125  : source_normals_(), source_normals_transformed_(), k_(10)
126  {
127  corr_name_ = "CorrespondenceEstimationNormalShooting";
128  }
129 
130  /** \brief Empty destructor */
132 
133  /** \brief Set the normals computed on the source point cloud
134  * \param[in] normals the normals computed for the source cloud
135  */
136  inline void
138  {
139  source_normals_ = normals;
140  }
141 
142  /** \brief Get the normals of the source point cloud
143  */
144  inline NormalsConstPtr
146  {
147  return (source_normals_);
148  }
149 
150  /** \brief See if this rejector requires source normals */
151  bool
152  requiresSourceNormals() const override
153  {
154  return (true);
155  }
156 
157  /** \brief Blob method for setting the source normals */
158  void
160  {
161  NormalsPtr cloud(new PointCloudNormals);
162  fromPCLPointCloud2(*cloud2, *cloud);
163  setSourceNormals(cloud);
164  }
165 
166  /** \brief Determine the correspondences between input and target cloud.
167  * \param[out] correspondences the found correspondences (index of query point, index
168  * of target point, distance) \param[in] max_distance maximum distance between the
169  * normal on the source point cloud and the corresponding point in the target point
170  * cloud
171  */
172  void
174  pcl::Correspondences& correspondences,
175  double max_distance = std::numeric_limits<double>::max()) override;
176 
177  /** \brief Determine the reciprocal correspondences between input and target cloud.
178  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
179  * correspondence, and Tgt_i has Src_i as one.
180  *
181  * \param[out] correspondences the found correspondences (index of query and target
182  * point, distance) \param[in] max_distance maximum allowed distance between
183  * correspondences
184  */
185  void
187  pcl::Correspondences& correspondences,
188  double max_distance = std::numeric_limits<double>::max()) override;
189 
190  /** \brief Set the number of nearest neighbours to be considered in the target
191  * point cloud. By default, we use k = 10 nearest neighbors.
192  *
193  * \param[in] k the number of nearest neighbours to be considered
194  */
195  inline void
196  setKSearch(unsigned int k)
197  {
198  k_ = k;
199  }
200 
201  /** \brief Get the number of nearest neighbours considered in the target point
202  * cloud for computing correspondences. By default we use k = 10 nearest
203  * neighbors.
204  */
205  inline unsigned int
206  getKSearch() const
207  {
208  return (k_);
209  }
210 
211  /** \brief Clone and cast to CorrespondenceEstimationBase */
213  clone() const override
214  {
215  Ptr copy(new CorrespondenceEstimationNormalShooting<PointSource,
216  PointTarget,
217  NormalT,
218  Scalar>(*this));
219  return (copy);
220  }
221 
222 protected:
228 
229  /** \brief Internal computation initialization. */
230  bool
231  initCompute();
232 
233 private:
234  /** \brief The normals computed at each point in the source cloud */
235  NormalsConstPtr source_normals_;
236 
237  /** \brief The normals computed at each point in the source cloud */
238  NormalsPtr source_normals_transformed_;
239 
240  /** \brief The number of neighbours to be considered in the target point cloud */
241  unsigned int k_;
242 };
243 } // namespace registration
244 } // namespace pcl
245 
246 #include <pcl/registration/impl/correspondence_estimation_normal_shooting.hpp>
PCL base class.
Definition: pcl_base.h:70
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
Abstract CorrespondenceEstimationBase class.
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
PointRepresentationConstPtr point_representation_
The point representation used (internal).
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object used for the source dataset.
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
CorrespondenceEstimationNormalShooting computes correspondences as points in the target cloud which h...
shared_ptr< const CorrespondenceEstimationNormalShooting< PointSource, PointTarget, NormalT, Scalar > > ConstPtr
CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const override
Clone and cast to CorrespondenceEstimationBase.
shared_ptr< CorrespondenceEstimationNormalShooting< PointSource, PointTarget, NormalT, Scalar > > Ptr
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
unsigned int getKSearch() const
Get the number of nearest neighbours considered in the target point cloud for computing correspondenc...
void setKSearch(unsigned int k)
Set the number of nearest neighbours to be considered in the target point cloud.
bool requiresSourceNormals() const override
See if this rejector requires source normals.
~CorrespondenceEstimationNormalShooting() override=default
Empty destructor.
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
NormalsConstPtr getSourceNormals() const
Get the normals of the source point cloud.
void setSourceNormals(const NormalsConstPtr &normals)
Set the normals computed on the source point cloud.
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
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:167
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr