Point Cloud Library (PCL)  1.10.0-dev
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 
41 #pragma once
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:
70 
71  /** \brief Empty constructor. */
73  : median_distance_ (0)
74  , factor_ (1.0)
75  {
76  rejection_name_ = "CorrespondenceRejectorMedianDistance";
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 Get the median distance used for thresholding in correspondence rejection. */
88  inline double
89  getMedianDistance () const { return (median_distance_); };
90 
91  /** \brief Provide a source point cloud dataset (must contain XYZ
92  * data!), used to compute the correspondence distance.
93  * \param[in] cloud a cloud containing XYZ data
94  */
95  template <typename PointT> inline void
97  {
98  if (!data_container_)
99  data_container_.reset (new DataContainer<PointT>);
100  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
101  }
102 
103  /** \brief Provide a source point cloud dataset (must contain XYZ
104  * data!), used to compute the correspondence distance.
105  * \param[in] cloud a cloud containing XYZ data
106  */
107  template <typename PointT> inline void
109  {
110  PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
111  if (!data_container_)
112  data_container_.reset (new DataContainer<PointT>);
113  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
114  }
115 
116  /** \brief Provide a target point cloud dataset (must contain XYZ
117  * data!), used to compute the correspondence distance.
118  * \param[in] target a cloud containing XYZ data
119  */
120  template <typename PointT> inline void
122  {
123  if (!data_container_)
124  data_container_.reset (new DataContainer<PointT>);
125  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
126  }
127 
128  /** \brief See if this rejector requires source points */
129  bool
130  requiresSourcePoints () const override
131  { return (true); }
132 
133  /** \brief Blob method for setting the source cloud */
134  void
136  {
138  fromPCLPointCloud2 (*cloud2, *cloud);
139  setInputSource<PointXYZ> (cloud);
140  }
141 
142  /** \brief See if this rejector requires a target cloud */
143  bool
144  requiresTargetPoints () const override
145  { return (true); }
146 
147  /** \brief Method for setting the target cloud */
148  void
150  {
152  fromPCLPointCloud2 (*cloud2, *cloud);
153  setInputTarget<PointXYZ> (cloud);
154  }
155 
156  /** \brief Provide a pointer to the search object used to find correspondences in
157  * the target cloud.
158  * \param[in] tree a pointer to the spatial search object.
159  * \param[in] force_no_recompute If set to true, this tree will NEVER be
160  * recomputed, regardless of calls to setInputTarget. Only use if you are
161  * confident that the tree will be set correctly.
162  */
163  template <typename PointT> inline void
165  bool force_no_recompute = false)
166  {
167  boost::static_pointer_cast< DataContainer<PointT> >
168  (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
169  }
170 
171  /** \brief Set the factor for correspondence rejection. Points with distance greater than median times factor
172  * will be rejected
173  * \param[in] factor value
174  */
175  inline void
176  setMedianFactor (double factor) { factor_ = factor; };
177 
178  /** \brief Get the factor used for thresholding in correspondence rejection. */
179  inline double
180  getMedianFactor () const { return factor_; };
181 
182  protected:
183 
184  /** \brief Apply the rejection algorithm.
185  * \param[out] correspondences the set of resultant correspondences.
186  */
187  inline void
188  applyRejection (pcl::Correspondences &correspondences) override
189  {
190  getRemainingCorrespondences (*input_correspondences_, correspondences);
191  }
192 
193  /** \brief The median distance threshold between two correspondent points in source <-> target.
194  */
196 
197  /** \brief The factor for correspondence rejection. Points with distance greater than median times factor
198  * will be rejected
199  */
200  double factor_;
201 
203 
204  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
206  };
207  }
208 }
209 
210 #include <pcl/registration/impl/correspondence_rejection_median_distance.hpp>
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
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
shared_ptr< const CorrespondenceRejector > ConstPtr
DataContainer is a container for the input and target point clouds and implements the interface to co...
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...
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
shared_ptr< CorrespondenceRejector > 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 ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
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...
double getMedianFactor() const
Get the factor used for thresholding in correspondence rejection.
double getMedianDistance() const
Get the median distance used for thresholding in correspondence rejection.
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.
CorrespondenceRejectorMedianDistance implements a simple correspondence rejection method based on thr...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
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...
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...
bool requiresSourcePoints() const override
See if this rejector requires source points.
void setMedianFactor(double factor)
Set the factor for correspondence rejection.
shared_ptr< DataContainerInterface > Ptr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
double median_distance_
The median distance threshold between two correspondent points in source <-> target.
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
#define PCL_EXPORTS
Definition: pcl_macros.h:253
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.