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>
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:61
pcl::registration::CorrespondenceRejectorSurfaceNormal::threshold_
double threshold_
The median distance threshold between two correspondent points in source <-> target.
Definition: correspondence_rejection_surface_normal.h:316
pcl::registration::CorrespondenceRejectorSurfaceNormal::getRemainingCorrespondences
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.
pcl
Definition: convolution.h:46
pcl::registration::CorrespondenceRejectorSurfaceNormal::setTargetNormals
void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target normals.
Definition: correspondence_rejection_surface_normal.h:295
pcl::registration::CorrespondenceRejectorSurfaceNormal::requiresTargetNormals
bool requiresTargetNormals() const override
See if this rejector requires target normals.
Definition: correspondence_rejection_surface_normal.h:290
pcl::registration::CorrespondenceRejectorSurfaceNormal::getInputSource
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
Definition: correspondence_rejection_surface_normal.h:136
pcl::registration::CorrespondenceRejectorSurfaceNormal::requiresTargetPoints
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
Definition: correspondence_rejection_surface_normal.h:258
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition: correspondence_rejection.h:62
pcl::registration::CorrespondenceRejectorSurfaceNormal::getTargetNormals
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
Definition: correspondence_rejection_surface_normal.h:229
pcl::registration::CorrespondenceRejectorSurfaceNormal::CorrespondenceRejectorSurfaceNormal
CorrespondenceRejectorSurfaceNormal()
Empty constructor.
Definition: correspondence_rejection_surface_normal.h:73
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: correspondence_rejection.h:132
pcl::registration::CorrespondenceRejectorSurfaceNormal::setSourcePoints
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
Definition: correspondence_rejection_surface_normal.h:247
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputNormals
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
Definition: correspondence_rejection_surface_normal.h:191
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
pcl::registration::DataContainerInterface::Ptr
shared_ptr< DataContainerInterface > Ptr
Definition: correspondence_rejection.h:202
pcl::registration::CorrespondenceRejectorSurfaceNormal::getThreshold
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
Definition: correspondence_rejection_surface_normal.h:95
pcl::registration::CorrespondenceRejectorSurfaceNormal::requiresSourceNormals
bool requiresSourceNormals() const override
See if this rejector requires source normals.
Definition: correspondence_rejection_surface_normal.h:274
pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputTarget
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...
Definition: correspondence_rejection_surface_normal.h:150
PCL_DEPRECATED
#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
pcl::PCLPointCloud2::ConstPtr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
Definition: PCLPointCloud2.h:35
pcl::registration::CorrespondenceRejectorSurfaceNormal::DataContainerPtr
DataContainerInterface::Ptr DataContainerPtr
Definition: correspondence_rejection_surface_normal.h:318
pcl::registration::DataContainer
DataContainer is a container for the input and target point clouds and implements the interface to co...
Definition: correspondence_rejection.h:217
pcl::registration::CorrespondenceRejectorSurfaceNormal::data_container_
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
Definition: correspondence_rejection_surface_normal.h:320
pcl::registration::CorrespondenceRejectorSurfaceNormal::getInputTarget
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
Definition: correspondence_rejection_surface_normal.h:177
pcl::registration::CorrespondenceRejectorSurfaceNormal::initializeDataContainer
void initializeDataContainer()
Initialize the data container object for the point type and the normal type.
Definition: correspondence_rejection_surface_normal.h:99
pcl::registration::CorrespondenceRejectorSurfaceNormal::setSourceNormals
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
Definition: correspondence_rejection_surface_normal.h:279
pcl::registration::CorrespondenceRejectorSurfaceNormal::requiresSourcePoints
bool requiresSourcePoints() const override
See if this rejector requires source points.
Definition: correspondence_rejection_surface_normal.h:242
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
pcl::registration::CorrespondenceRejectorSurfaceNormal::setTargetNormals
void setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
Definition: correspondence_rejection_surface_normal.h:217
pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputSource
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...
Definition: correspondence_rejection_surface_normal.h:124
pcl::registration::CorrespondenceRejectorSurfaceNormal::setThreshold
void setThreshold(double threshold)
Set the thresholding angle between the normals for correspondence rejection.
Definition: correspondence_rejection_surface_normal.h:91
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::registration::CorrespondenceRejectorSurfaceNormal::applyRejection
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
Definition: correspondence_rejection_surface_normal.h:310
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:90
pcl::registration::CorrespondenceRejectorSurfaceNormal::setSearchMethodTarget
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.
Definition: correspondence_rejection_surface_normal.h:168
pcl::registration::CorrespondenceRejector::input_correspondences_
CorrespondencesConstPtr input_correspondences_
The input correspondences.
Definition: correspondence_rejection.h:188
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition: correspondence_rejection.h:185
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::registration::CorrespondenceRejectorSurfaceNormal
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
Definition: correspondence_rejection_surface_normal.h:63
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:328
pcl::registration::CorrespondenceRejectorSurfaceNormal::setTargetPoints
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
Definition: correspondence_rejection_surface_normal.h:263
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition: correspondence_rejection.h:59
pcl::fromPCLPointCloud2
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
pcl::registration::CorrespondenceRejectorSurfaceNormal::getInputNormals
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
Definition: correspondence_rejection_surface_normal.h:203