Point Cloud Library (PCL)  1.14.1-dev
correspondence_rejection_surface_normal.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/registration/correspondence_rejection.h>
43 #include <pcl/conversions.h> // for fromPCLPointCloud2
44 #include <pcl/memory.h> // for static_pointer_cast
45 #include <pcl/point_cloud.h>
46 
47 namespace pcl {
48 namespace registration {
49 /**
50  * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence
51  * rejection method based on the angle between the normals at correspondent points.
52  *
53  * \note If \ref setInputCloud and \ref setInputTarget are given, then the
54  * distances between correspondences will be estimated using the given XYZ
55  * data, and not read from the set of input correspondences.
56  *
57  * \author Aravindhan K Krishnan (original code from libpointmatcher:
58  * https://github.com/ethz-asl/libpointmatcher) \ingroup registration
59  */
64 
65 public:
66  using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
67  using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
68 
69  /** \brief Empty constructor. Sets the threshold to 1.0. */
71  {
72  rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
73  }
74 
75  /** \brief Get a list of valid correspondences after rejection from the original set
76  * of correspondences. \param[in] original_correspondences the set of initial
77  * correspondences given \param[out] remaining_correspondences the resultant filtered
78  * set of remaining correspondences
79  */
80  void
81  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
82  pcl::Correspondences& remaining_correspondences) override;
83 
84  /** \brief Set the thresholding angle between the normals for correspondence
85  * rejection. \param[in] threshold cosine of the thresholding angle between the
86  * normals for rejection
87  */
88  inline void
89  setThreshold(double threshold)
90  {
91  threshold_ = threshold;
92  };
93 
94  /** \brief Get the thresholding angle between the normals for correspondence
95  * rejection. */
96  inline double
97  getThreshold() const
98  {
99  return threshold_;
100  };
101 
102  /** \brief Initialize the data container object for the point type and the normal
103  * type. */
104  template <typename PointT, typename NormalT>
105  inline void
107  {
108  data_container_.reset(new DataContainer<PointT, NormalT>);
109  }
110 
111  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to
112  * compute the correspondence distance. \param[in] input a cloud containing XYZ data
113  */
114  template <typename PointT>
115  inline void
117  {
118  if (!data_container_) {
119  PCL_ERROR(
120  "[pcl::registration::%s::setInputCloud] Initialize the data container object "
121  "by calling intializeDataContainer () before using this function.\n",
122  getClassName().c_str());
123  return;
124  }
125  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(input);
126  }
127 
128  /** \brief Get the target input point cloud */
129  template <typename PointT>
130  inline typename pcl::PointCloud<PointT>::ConstPtr
132  {
133  if (!data_container_) {
134  PCL_ERROR(
135  "[pcl::registration::%s::getInputSource] Initialize the data container "
136  "object by calling intializeDataContainer () before using this function.\n",
137  getClassName().c_str());
138  return;
139  }
140  return (
141  static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputSource());
142  }
143 
144  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to
145  * compute the correspondence distance. \param[in] target a cloud containing XYZ data
146  */
147  template <typename PointT>
148  inline void
150  {
151  if (!data_container_) {
152  PCL_ERROR(
153  "[pcl::registration::%s::setInputTarget] Initialize the data container "
154  "object by calling intializeDataContainer () before using this function.\n",
155  getClassName().c_str());
156  return;
157  }
158  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
159  }
160 
161  /** \brief Provide a pointer to the search object used to find correspondences in
162  * the target cloud.
163  * \param[in] tree a pointer to the spatial search object.
164  * \param[in] force_no_recompute If set to true, this tree will NEVER be
165  * recomputed, regardless of calls to setInputTarget. Only use if you are
166  * confident that the tree will be set correctly.
167  */
168  template <typename PointT>
169  inline void
171  bool force_no_recompute = false)
172  {
173  static_pointer_cast<DataContainer<PointT>>(data_container_)
174  ->setSearchMethodTarget(tree, force_no_recompute);
175  }
176 
177  /** \brief Get the target input point cloud */
178  template <typename PointT>
179  inline typename pcl::PointCloud<PointT>::ConstPtr
181  {
182  if (!data_container_) {
183  PCL_ERROR(
184  "[pcl::registration::%s::getInputTarget] Initialize the data container "
185  "object by calling intializeDataContainer () before using this function.\n",
186  getClassName().c_str());
187  return;
188  }
189  return (
190  static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputTarget());
191  }
192 
193  /** \brief Set the normals computed on the input point cloud
194  * \param[in] normals the normals computed for the input cloud
195  */
196  template <typename PointT, typename NormalT>
197  inline void
199  {
200  if (!data_container_) {
201  PCL_ERROR(
202  "[pcl::registration::%s::setInputNormals] Initialize the data container "
203  "object by calling intializeDataContainer () before using this function.\n",
204  getClassName().c_str());
205  return;
206  }
207  static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
208  ->setInputNormals(normals);
209  }
210 
211  /** \brief Get the normals computed on the input point cloud */
212  template <typename NormalT>
213  inline typename pcl::PointCloud<NormalT>::Ptr
215  {
216  if (!data_container_) {
217  PCL_ERROR(
218  "[pcl::registration::%s::getInputNormals] Initialize the data container "
219  "object by calling intializeDataContainer () before using this function.\n",
220  getClassName().c_str());
221  return;
222  }
223  return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
224  ->getInputNormals());
225  }
226 
227  /** \brief Set the normals computed on the target point cloud
228  * \param[in] normals the normals computed for the input cloud
229  */
230  template <typename PointT, typename NormalT>
231  inline void
233  {
234  if (!data_container_) {
235  PCL_ERROR(
236  "[pcl::registration::%s::setTargetNormals] Initialize the data container "
237  "object by calling intializeDataContainer () before using this function.\n",
238  getClassName().c_str());
239  return;
240  }
241  static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
242  ->setTargetNormals(normals);
243  }
244 
245  /** \brief Get the normals computed on the target point cloud */
246  template <typename NormalT>
247  inline typename pcl::PointCloud<NormalT>::Ptr
249  {
250  if (!data_container_) {
251  PCL_ERROR(
252  "[pcl::registration::%s::getTargetNormals] Initialize the data container "
253  "object by calling intializeDataContainer () before using this function.\n",
254  getClassName().c_str());
255  return;
256  }
257  return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
258  ->getTargetNormals());
259  }
260 
261  /** \brief See if this rejector requires source points */
262  bool
263  requiresSourcePoints() const override
264  {
265  return (true);
266  }
267 
268  /** \brief Blob method for setting the source cloud */
269  void
271  {
272  if (!data_container_)
273  initializeDataContainer<PointXYZ, Normal>();
275  fromPCLPointCloud2(*cloud2, *cloud);
276  setInputSource<PointXYZ>(cloud);
277  }
278 
279  /** \brief See if this rejector requires a target cloud */
280  bool
281  requiresTargetPoints() const override
282  {
283  return (true);
284  }
285 
286  /** \brief Method for setting the target cloud */
287  void
289  {
290  if (!data_container_)
291  initializeDataContainer<PointXYZ, Normal>();
293  fromPCLPointCloud2(*cloud2, *cloud);
294  setInputTarget<PointXYZ>(cloud);
295  }
296 
297  /** \brief See if this rejector requires source normals */
298  bool
299  requiresSourceNormals() const override
300  {
301  return (true);
302  }
303 
304  /** \brief Blob method for setting the source normals */
305  void
307  {
308  if (!data_container_)
309  initializeDataContainer<PointXYZ, Normal>();
311  fromPCLPointCloud2(*cloud2, *cloud);
312  setInputNormals<PointXYZ, Normal>(cloud);
313  }
314 
315  /** \brief See if this rejector requires target normals*/
316  bool
317  requiresTargetNormals() const override
318  {
319  return (true);
320  }
321 
322  /** \brief Method for setting the target normals */
323  void
325  {
326  if (!data_container_)
327  initializeDataContainer<PointXYZ, Normal>();
329  fromPCLPointCloud2(*cloud2, *cloud);
330  setTargetNormals<PointXYZ, Normal>(cloud);
331  }
332 
333 protected:
334  /** \brief Apply the rejection algorithm.
335  * \param[out] correspondences the set of resultant correspondences.
336  */
337  inline void
338  applyRejection(pcl::Correspondences& correspondences) override
339  {
340  getRemainingCorrespondences(*input_correspondences_, correspondences);
341  }
342 
343  /** \brief The median distance threshold between two correspondent points in source
344  * <-> target. */
345  double threshold_{1.0};
346 
348  /** \brief A pointer to the DataContainer object containing the input and target point
349  * clouds */
351 };
352 } // namespace registration
353 } // namespace pcl
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
CorrespondenceRejector represents the base class for correspondence rejection methods
shared_ptr< const CorrespondenceRejector > ConstPtr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< CorrespondenceRejector > Ptr
std::string rejection_name_
The name of the rejection method.
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
void setInputTarget(const typename pcl::PointCloud< PointT >::ConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
void initializeDataContainer()
Initialize the data container object for the point type and the normal type.
void setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target normals.
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
bool requiresTargetNormals() const override
See if this rejector requires target normals.
void setInputSource(const typename pcl::PointCloud< PointT >::ConstPtr &input)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
void setThreshold(double threshold)
Set the thresholding angle between the normals for correspondence rejection.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
bool requiresSourcePoints() const override
See if this rejector requires source points.
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
void setSearchMethodTarget(const typename pcl::search::KdTree< PointT >::Ptr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud.
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
bool requiresSourceNormals() const override
See if this rejector requires source normals.
DataContainer is a container for the input and target point clouds and implements the interface to co...
shared_ptr< DataContainerInterface > Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
Defines functions, macros and traits for allocating and using memory.
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
#define PCL_EXPORTS
Definition: pcl_macros.h:323
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr