Point Cloud Library (PCL)  1.7.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] cloud 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] cloud 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  protected:
238 
239  /** \brief Apply the rejection algorithm.
240  * \param[out] correspondences the set of resultant correspondences.
241  */
242  inline void
244  {
245  getRemainingCorrespondences (*input_correspondences_, correspondences);
246  }
247 
248  /** \brief The median distance threshold between two correspondent points in source <-> target. */
249  double threshold_;
250 
251  typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
252  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
253  DataContainerPtr data_container_;
254  };
255  }
256 }
257 
258 #include <pcl/registration/impl/correspondence_rejection_surface_normal.hpp>
259 
260 #endif
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< const CorrespondenceRejectorSurfaceNormal > ConstPtr
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
void initializeDataContainer()
Initialize the data container object for the point type and the normal type.
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...
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
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 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 setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
CorrespondenceRejector represents the base class for correspondence rejection methods ...
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
DataContainer is a container for the input and target point clouds and implements the interface to co...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
void setThreshold(double threshold)
Set the thresholding angle between the normals for correspondence rejection.
std::string rejection_name_
The name of the rejection method.
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
CorrespondencesConstPtr input_correspondences_
The input correspondences.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< CorrespondenceRejectorSurfaceNormal > Ptr
const std::string & getClassName() const
Get a string representation of the name of this class.
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...
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
double threshold_
The median distance threshold between two correspondent points in source <-> target.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62