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