Point Cloud Library (PCL)  1.10.0-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/point_cloud.h>
44 
45 namespace pcl
46 {
47  namespace registration
48  {
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: https://github.com/ethz-asl/libpointmatcher)
58  * \ingroup registration
59  */
61  {
65 
66  public:
69 
70  /** \brief Empty constructor. Sets the threshold to 1.0. */
72  : threshold_ (1.0)
73  {
74  rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
75  }
76 
77  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
78  * \param[in] original_correspondences the set of initial correspondences given
79  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
80  */
81  void
82  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
83  pcl::Correspondences& remaining_correspondences) override;
84 
85  /** \brief Set the thresholding angle between the normals for correspondence rejection.
86  * \param[in] threshold cosine of the thresholding angle between the normals for rejection
87  */
88  inline void
89  setThreshold (double threshold) { threshold_ = threshold; };
90 
91  /** \brief Get the thresholding angle between the normals for correspondence rejection. */
92  inline double
93  getThreshold () const { return threshold_; };
94 
95  /** \brief Initialize the data container object for the point type and the normal type. */
96  template <typename PointT, typename NormalT> inline void
98  {
99  data_container_.reset (new DataContainer<PointT, NormalT>);
100  }
101 
102  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
103  * \param[in] input a cloud containing XYZ data
104  */
105  template <typename PointT> inline void
107  {
108  PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
109  if (!data_container_)
110  {
111  PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
112  return;
113  }
114  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
115  }
116 
117  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
118  * \param[in] input a cloud containing XYZ data
119  */
120  template <typename PointT> inline void
122  {
123  if (!data_container_)
124  {
125  PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
126  return;
127  }
128  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
129  }
130 
131  /** \brief Get the target input point cloud */
132  template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
133  getInputSource () const
134  {
135  if (!data_container_)
136  {
137  PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
138  return;
139  }
140  return (boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ());
141  }
142 
143  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
144  * \param[in] target a cloud containing XYZ data
145  */
146  template <typename PointT> inline void
148  {
149  if (!data_container_)
150  {
151  PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
152  return;
153  }
154  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
155  }
156 
157  /** \brief Provide a pointer to the search object used to find correspondences in
158  * the target cloud.
159  * \param[in] tree a pointer to the spatial search object.
160  * \param[in] force_no_recompute If set to true, this tree will NEVER be
161  * recomputed, regardless of calls to setInputTarget. Only use if you are
162  * confident that the tree will be set correctly.
163  */
164  template <typename PointT> inline void
166  bool force_no_recompute = false)
167  {
168  boost::static_pointer_cast< DataContainer<PointT> >
169  (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
170  }
171 
172  /** \brief Get the target input point cloud */
173  template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
174  getInputTarget () const
175  {
176  if (!data_container_)
177  {
178  PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
179  return;
180  }
181  return (boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ());
182  }
183 
184  /** \brief Set the normals computed on the input point cloud
185  * \param[in] normals the normals computed for the input cloud
186  */
187  template <typename PointT, typename NormalT> inline void
189  {
190  if (!data_container_)
191  {
192  PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
193  return;
194  }
195  boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals);
196  }
197 
198  /** \brief Get the normals computed on the input point cloud */
199  template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
200  getInputNormals () const
201  {
202  if (!data_container_)
203  {
204  PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
205  return;
206  }
207  return (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ());
208  }
209 
210  /** \brief Set the normals computed on the target point cloud
211  * \param[in] normals the normals computed for the input cloud
212  */
213  template <typename PointT, typename NormalT> inline void
215  {
216  if (!data_container_)
217  {
218  PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
219  return;
220  }
221  boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals);
222  }
223 
224  /** \brief Get the normals computed on the target point cloud */
225  template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
227  {
228  if (!data_container_)
229  {
230  PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
231  return;
232  }
233  return (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ());
234  }
235 
236 
237  /** \brief See if this rejector requires source points */
238  bool
239  requiresSourcePoints () const override
240  { return (true); }
241 
242  /** \brief Blob method for setting the source cloud */
243  void
245  {
246  if (!data_container_)
247  initializeDataContainer<PointXYZ, Normal> ();
249  fromPCLPointCloud2 (*cloud2, *cloud);
250  setInputSource<PointXYZ> (cloud);
251  }
252 
253  /** \brief See if this rejector requires a target cloud */
254  bool
255  requiresTargetPoints () const override
256  { return (true); }
257 
258  /** \brief Method for setting the target cloud */
259  void
261  {
262  if (!data_container_)
263  initializeDataContainer<PointXYZ, Normal> ();
265  fromPCLPointCloud2 (*cloud2, *cloud);
266  setInputTarget<PointXYZ> (cloud);
267  }
268 
269  /** \brief See if this rejector requires source normals */
270  bool
271  requiresSourceNormals () const override
272  { return (true); }
273 
274  /** \brief Blob method for setting the source normals */
275  void
277  {
278  if (!data_container_)
279  initializeDataContainer<PointXYZ, Normal> ();
281  fromPCLPointCloud2 (*cloud2, *cloud);
282  setInputNormals<PointXYZ, Normal> (cloud);
283  }
284 
285  /** \brief See if this rejector requires target normals*/
286  bool
287  requiresTargetNormals () const override
288  { return (true); }
289 
290  /** \brief Method for setting the target normals */
291  void
293  {
294  if (!data_container_)
295  initializeDataContainer<PointXYZ, Normal> ();
297  fromPCLPointCloud2 (*cloud2, *cloud);
298  setTargetNormals<PointXYZ, Normal> (cloud);
299  }
300 
301  protected:
302 
303  /** \brief Apply the rejection algorithm.
304  * \param[out] correspondences the set of resultant correspondences.
305  */
306  inline void
307  applyRejection (pcl::Correspondences &correspondences) override
308  {
309  getRemainingCorrespondences (*input_correspondences_, correspondences);
310  }
311 
312  /** \brief The median distance threshold between two correspondent points in source <-> target. */
313  double threshold_;
314 
316  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
318  };
319  }
320 }
321 
322 #include <pcl/registration/impl/correspondence_rejection_surface_normal.hpp>
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
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:168
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
shared_ptr< const CorrespondenceRejector > ConstPtr
DataContainer is a container for the input and target point clouds and implements the interface to co...
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...
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
shared_ptr< CorrespondenceRejector > Ptr
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
bool requiresSourceNormals() const override
See if this rejector requires source normals.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &input)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
CorrespondenceRejector represents the base class for correspondence rejection methods ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
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.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
const std::string & getClassName() const
Get a string representation of the name of this class.
void setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target normals.
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
double threshold_
The median distance threshold between two correspondent points in source <-> target.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
bool requiresSourcePoints() const override
See if this rejector requires source points.
shared_ptr< DataContainerInterface > Ptr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > 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...
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
#define PCL_EXPORTS
Definition: pcl_macros.h:253
void initializeDataContainer()
Initialize the data container object for the point type and the normal type.
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...