Point Cloud Library (PCL)  1.14.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 
106  KdTreePtr;
109 
116 
120 
124 
128 
129  /** \brief Empty constructor.
130  *
131  * \note
132  * Sets the number of neighbors to be considered in the target point cloud (k_) to 10.
133  */
135  : source_normals_(), source_normals_transformed_()
136  {
137  corr_name_ = "CorrespondenceEstimationNormalShooting";
138  }
139 
140  /** \brief Empty destructor */
142 
143  /** \brief Set the normals computed on the source point cloud
144  * \param[in] normals the normals computed for the source cloud
145  */
146  inline void
148  {
149  source_normals_ = normals;
150  }
151 
152  /** \brief Get the normals of the source point cloud
153  */
154  inline NormalsConstPtr
156  {
157  return (source_normals_);
158  }
159 
160  /** \brief See if this rejector requires source normals */
161  bool
162  requiresSourceNormals() const override
163  {
164  return (true);
165  }
166 
167  /** \brief Blob method for setting the source normals */
168  void
170  {
171  NormalsPtr cloud(new PointCloudNormals);
172  fromPCLPointCloud2(*cloud2, *cloud);
173  setSourceNormals(cloud);
174  }
175 
176  /** \brief Determine the correspondences between input and target cloud.
177  * \param[out] correspondences the found correspondences (index of query point, index
178  * of target point, distance) \param[in] max_distance maximum distance between the
179  * normal on the source point cloud and the corresponding point in the target point
180  * cloud
181  */
182  void
184  pcl::Correspondences& correspondences,
185  double max_distance = std::numeric_limits<double>::max()) override;
186 
187  /** \brief Determine the reciprocal correspondences between input and target cloud.
188  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
189  * correspondence, and Tgt_i has Src_i as one.
190  *
191  * \param[out] correspondences the found correspondences (index of query and target
192  * point, distance) \param[in] max_distance maximum allowed distance between
193  * correspondences
194  */
195  void
197  pcl::Correspondences& correspondences,
198  double max_distance = std::numeric_limits<double>::max()) override;
199 
200  /** \brief Set the number of nearest neighbours to be considered in the target
201  * point cloud. By default, we use k = 10 nearest neighbors.
202  *
203  * \param[in] k the number of nearest neighbours to be considered
204  */
205  inline void
206  setKSearch(unsigned int k)
207  {
208  k_ = k;
209  }
210 
211  /** \brief Get the number of nearest neighbours considered in the target point
212  * cloud for computing correspondences. By default we use k = 10 nearest
213  * neighbors.
214  */
215  inline unsigned int
216  getKSearch() const
217  {
218  return (k_);
219  }
220 
221  /** \brief Clone and cast to CorrespondenceEstimationBase */
223  clone() const override
224  {
225  Ptr copy(new CorrespondenceEstimationNormalShooting<PointSource,
226  PointTarget,
227  NormalT,
228  Scalar>(*this));
229  return (copy);
230  }
231 
232 protected:
238 
239  /** \brief Internal computation initialization. */
240  bool
241  initCompute();
242 
243 private:
244  /** \brief The normals computed at each point in the source cloud */
245  NormalsConstPtr source_normals_;
246 
247  /** \brief The normals computed at each point in the source cloud */
248  NormalsPtr source_normals_transformed_;
249 
250  /** \brief The number of neighbours to be considered in the target point cloud */
251  unsigned int k_{10};
252 };
253 } // namespace registration
254 } // namespace pcl
255 
256 #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 target 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
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:229
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr