Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
correspondence_rejection_median_distance.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_
41 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_
42 
43 #include <pcl/registration/correspondence_rejection.h>
44 #include <pcl/point_cloud.h>
45 
46 namespace pcl
47 {
48  namespace registration
49  {
50  /** \brief CorrespondenceRejectorMedianDistance implements a simple correspondence
51  * rejection method based on thresholding based on the median distance between the
52  * correspondences.
53  *
54  * \note If \ref setInputCloud and \ref setInputTarget are given, then the
55  * distances between correspondences will be estimated using the given XYZ
56  * data, and not read from the set of input correspondences.
57  *
58  * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
59  * \ingroup registration
60  */
62  {
66 
67  public:
68  typedef boost::shared_ptr<CorrespondenceRejectorMedianDistance> Ptr;
69  typedef boost::shared_ptr<const CorrespondenceRejectorMedianDistance> ConstPtr;
70 
71  /** \brief Empty constructor. */
73  : median_distance_ (0)
74  , factor_ (1.0)
75  , data_container_ ()
76  {
77  rejection_name_ = "CorrespondenceRejectorMedianDistance";
78  }
79 
80  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
81  * \param[in] original_correspondences the set of initial correspondences given
82  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
83  */
84  void
85  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
86  pcl::Correspondences& remaining_correspondences);
87 
88  /** \brief Get the median distance used for thresholding in correspondence rejection. */
89  inline double
90  getMedianDistance () const { return (median_distance_); };
91 
92  /** \brief Provide a source point cloud dataset (must contain XYZ
93  * data!), used to compute the correspondence distance.
94  * \param[in] cloud a cloud containing XYZ data
95  */
96  template <typename PointT> inline void
98  {
99  if (!data_container_)
100  data_container_.reset (new DataContainer<PointT>);
101  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
102  }
103 
104  /** \brief Provide a source point cloud dataset (must contain XYZ
105  * data!), used to compute the correspondence distance.
106  * \param[in] cloud a cloud containing XYZ data
107  */
108  template <typename PointT> inline void
110  {
111  PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
112  if (!data_container_)
113  data_container_.reset (new DataContainer<PointT>);
114  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
115  }
116 
117  /** \brief Provide a target point cloud dataset (must contain XYZ
118  * data!), used to compute the correspondence distance.
119  * \param[in] target a cloud containing XYZ data
120  */
121  template <typename PointT> inline void
123  {
124  if (!data_container_)
125  data_container_.reset (new DataContainer<PointT>);
126  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
127  }
128 
129  /** \brief See if this rejector requires source points */
130  bool
132  { return (true); }
133 
134  /** \brief Blob method for setting the source cloud */
135  void
137  {
139  fromPCLPointCloud2 (*cloud2, *cloud);
140  setInputSource<PointXYZ> (cloud);
141  }
142 
143  /** \brief See if this rejector requires a target cloud */
144  bool
146  { return (true); }
147 
148  /** \brief Method for setting the target cloud */
149  void
151  {
153  fromPCLPointCloud2 (*cloud2, *cloud);
154  setInputTarget<PointXYZ> (cloud);
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
165  setSearchMethodTarget (const boost::shared_ptr<pcl::search::KdTree<PointT> > &tree,
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 Set the factor for correspondence rejection. Points with distance greater than median times factor
173  * will be rejected
174  * \param[in] factor value
175  */
176  inline void
177  setMedianFactor (double factor) { factor_ = factor; };
178 
179  /** \brief Get the factor used for thresholding in correspondence rejection. */
180  inline double
181  getMedianFactor () const { return factor_; };
182 
183  protected:
184 
185  /** \brief Apply the rejection algorithm.
186  * \param[out] correspondences the set of resultant correspondences.
187  */
188  inline void
190  {
191  getRemainingCorrespondences (*input_correspondences_, correspondences);
192  }
193 
194  /** \brief The median distance threshold between two correspondent points in source <-> target.
195  */
197 
198  /** \brief The factor for correspondence rejection. Points with distance greater than median times factor
199  * will be rejected
200  */
201  double factor_;
202 
203  typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
204 
205  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
206  DataContainerPtr data_container_;
207  };
208  }
209 }
210 
211 #include <pcl/registration/impl/correspondence_rejection_median_distance.hpp>
212 
213 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_H_
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
Definition: conversions.h:169
DataContainer is a container for the input and target point clouds and implements the interface to co...
double getMedianDistance() const
Get the median distance used for thresholding in correspondence rejection.
boost::shared_ptr< CorrespondenceRejectorMedianDistance > Ptr
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
CorrespondenceRejector represents the base class for correspondence rejection methods ...
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
bool requiresTargetPoints() const
See if this rejector requires a target cloud.
boost::shared_ptr< const CorrespondenceRejectorMedianDistance > ConstPtr
double getMedianFactor() const
Get the factor used for thresholding in correspondence rejection.
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Blob method for setting the source cloud.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
const std::string & getClassName() const
Get a string representation of the name of this class.
bool requiresSourcePoints() const
See if this rejector requires source points.
CorrespondenceRejectorMedianDistance implements a simple correspondence rejection method based on thr...
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Method for setting the target cloud.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
void setInputSource(const typename pcl::PointCloud< PointT >::ConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 setMedianFactor(double factor)
Set the factor for correspondence rejection.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
double median_distance_
The median distance threshold between two correspondent points in source <-> target.
std::string rejection_name_
The name of the rejection method.
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
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...