Point Cloud Library (PCL)  1.11.1
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/memory.h> // for static_pointer_cast
44 #include <pcl/point_cloud.h>
45 
46 
47 namespace pcl
48 {
49  namespace registration
50  {
51  /**
52  * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence
53  * rejection method based on the angle between the normals at correspondent points.
54  *
55  * \note If \ref setInputCloud and \ref setInputTarget are given, then the
56  * distances between correspondences will be estimated using the given XYZ
57  * data, and not read from the set of input correspondences.
58  *
59  * \author Aravindhan K Krishnan (original code from libpointmatcher: https://github.com/ethz-asl/libpointmatcher)
60  * \ingroup registration
61  */
63  {
67 
68  public:
69  using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
70  using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
71 
72  /** \brief Empty constructor. Sets the threshold to 1.0. */
74  : threshold_ (1.0)
75  {
76  rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
77  }
78 
79  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
80  * \param[in] original_correspondences the set of initial correspondences given
81  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
82  */
83  void
84  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
85  pcl::Correspondences& remaining_correspondences) override;
86 
87  /** \brief Set the thresholding angle between the normals for correspondence rejection.
88  * \param[in] threshold cosine of the thresholding angle between the normals for rejection
89  */
90  inline void
91  setThreshold (double threshold) { threshold_ = threshold; };
92 
93  /** \brief Get the thresholding angle between the normals for correspondence rejection. */
94  inline double
95  getThreshold () const { return threshold_; };
96 
97  /** \brief Initialize the data container object for the point type and the normal type. */
98  template <typename PointT, typename NormalT> inline void
100  {
101  data_container_.reset (new DataContainer<PointT, NormalT>);
102  }
103 
104  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
105  * \param[in] input a cloud containing XYZ data
106  */
107  template <typename PointT>
108  PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputCloud is deprecated. Please use setInputSource instead")
109  inline void
110  setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &input)
111  {
112  if (!data_container_)
113  {
114  PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
115  return;
116  }
117  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
118  }
119 
120  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
121  * \param[in] input a cloud containing XYZ data
122  */
123  template <typename PointT> inline void
125  {
126  if (!data_container_)
127  {
128  PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
129  return;
130  }
131  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input);
132  }
133 
134  /** \brief Get the target input point cloud */
135  template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
136  getInputSource () const
137  {
138  if (!data_container_)
139  {
140  PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
141  return;
142  }
143  return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ());
144  }
145 
146  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
147  * \param[in] target a cloud containing XYZ data
148  */
149  template <typename PointT> inline void
151  {
152  if (!data_container_)
153  {
154  PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
155  return;
156  }
157  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
158  }
159 
160  /** \brief Provide a pointer to the search object used to find correspondences in
161  * the target cloud.
162  * \param[in] tree a pointer to the spatial search object.
163  * \param[in] force_no_recompute If set to true, this tree will NEVER be
164  * recomputed, regardless of calls to setInputTarget. Only use if you are
165  * confident that the tree will be set correctly.
166  */
167  template <typename PointT> inline void
169  bool force_no_recompute = false)
170  {
171  static_pointer_cast< DataContainer<PointT> >
172  (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
173  }
174 
175  /** \brief Get the target input point cloud */
176  template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr
177  getInputTarget () const
178  {
179  if (!data_container_)
180  {
181  PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
182  return;
183  }
184  return (static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ());
185  }
186 
187  /** \brief Set the normals computed on the input point cloud
188  * \param[in] normals the normals computed for the input cloud
189  */
190  template <typename PointT, typename NormalT> inline void
192  {
193  if (!data_container_)
194  {
195  PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
196  return;
197  }
198  static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals);
199  }
200 
201  /** \brief Get the normals computed on the input point cloud */
202  template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
203  getInputNormals () const
204  {
205  if (!data_container_)
206  {
207  PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
208  return;
209  }
210  return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ());
211  }
212 
213  /** \brief Set the normals computed on the target point cloud
214  * \param[in] normals the normals computed for the input cloud
215  */
216  template <typename PointT, typename NormalT> inline void
218  {
219  if (!data_container_)
220  {
221  PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
222  return;
223  }
224  static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals);
225  }
226 
227  /** \brief Get the normals computed on the target point cloud */
228  template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr
230  {
231  if (!data_container_)
232  {
233  PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ());
234  return;
235  }
236  return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ());
237  }
238 
239 
240  /** \brief See if this rejector requires source points */
241  bool
242  requiresSourcePoints () const override
243  { return (true); }
244 
245  /** \brief Blob method for setting the source cloud */
246  void
248  {
249  if (!data_container_)
250  initializeDataContainer<PointXYZ, Normal> ();
252  fromPCLPointCloud2 (*cloud2, *cloud);
253  setInputSource<PointXYZ> (cloud);
254  }
255 
256  /** \brief See if this rejector requires a target cloud */
257  bool
258  requiresTargetPoints () const override
259  { return (true); }
260 
261  /** \brief Method for setting the target cloud */
262  void
264  {
265  if (!data_container_)
266  initializeDataContainer<PointXYZ, Normal> ();
268  fromPCLPointCloud2 (*cloud2, *cloud);
269  setInputTarget<PointXYZ> (cloud);
270  }
271 
272  /** \brief See if this rejector requires source normals */
273  bool
274  requiresSourceNormals () const override
275  { return (true); }
276 
277  /** \brief Blob method for setting the source normals */
278  void
280  {
281  if (!data_container_)
282  initializeDataContainer<PointXYZ, Normal> ();
284  fromPCLPointCloud2 (*cloud2, *cloud);
285  setInputNormals<PointXYZ, Normal> (cloud);
286  }
287 
288  /** \brief See if this rejector requires target normals*/
289  bool
290  requiresTargetNormals () const override
291  { return (true); }
292 
293  /** \brief Method for setting the target normals */
294  void
296  {
297  if (!data_container_)
298  initializeDataContainer<PointXYZ, Normal> ();
300  fromPCLPointCloud2 (*cloud2, *cloud);
301  setTargetNormals<PointXYZ, Normal> (cloud);
302  }
303 
304  protected:
305 
306  /** \brief Apply the rejection algorithm.
307  * \param[out] correspondences the set of resultant correspondences.
308  */
309  inline void
310  applyRejection (pcl::Correspondences &correspondences) override
311  {
312  getRemainingCorrespondences (*input_correspondences_, correspondences);
313  }
314 
315  /** \brief The median distance threshold between two correspondent points in source <-> target. */
316  double threshold_;
317 
319  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
321  };
322  }
323 }
324 
325 #include <pcl/registration/impl/correspondence_rejection_surface_normal.hpp>
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
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:429
Defines functions, macros and traits for allocating and using memory.
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.
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. ...
Definition: distances.h:55
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:430
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.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major...
Definition: pcl_macros.h:147
A point structure representing Euclidean xyz coordinates, and the RGB color.
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...
#define PCL_EXPORTS
Definition: pcl_macros.h:328
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...