Point Cloud Library (PCL)  1.14.1-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/conversions.h> // for fromPCLPointCloud2
45 #include <pcl/memory.h> // for static_pointer_cast
46 #include <pcl/point_cloud.h>
47 
48 namespace pcl {
49 namespace registration {
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
59  * (https://github.com/ethz-asl/libpointmatcher) \ingroup registration
60  */
65 
66 public:
67  using Ptr = shared_ptr<CorrespondenceRejectorMedianDistance>;
68  using ConstPtr = shared_ptr<const CorrespondenceRejectorMedianDistance>;
69 
70  /** \brief Empty constructor. */
72  {
73  rejection_name_ = "CorrespondenceRejectorMedianDistance";
74  }
75 
76  /** \brief Get a list of valid correspondences after rejection from the original set
77  * of correspondences. \param[in] original_correspondences the set of initial
78  * correspondences given \param[out] remaining_correspondences the resultant filtered
79  * set of remaining correspondences
80  */
81  void
82  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
83  pcl::Correspondences& remaining_correspondences) override;
84 
85  /** \brief Get the median distance used for thresholding in correspondence rejection.
86  */
87  inline double
89  {
90  return (median_distance_);
91  };
92 
93  /** \brief Provide a source point cloud dataset (must contain XYZ
94  * data!), used to compute the correspondence distance.
95  * \param[in] cloud a cloud containing XYZ data
96  */
97  template <typename PointT>
98  inline void
100  {
101  if (!data_container_)
102  data_container_.reset(new DataContainer<PointT>);
103  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
104  }
105 
106  /** \brief Provide a target point cloud dataset (must contain XYZ
107  * data!), used to compute the correspondence distance.
108  * \param[in] target a cloud containing XYZ data
109  */
110  template <typename PointT>
111  inline void
113  {
114  if (!data_container_)
115  data_container_.reset(new DataContainer<PointT>);
116  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
117  }
118 
119  /** \brief See if this rejector requires source points */
120  bool
121  requiresSourcePoints() const override
122  {
123  return (true);
124  }
125 
126  /** \brief Blob method for setting the source cloud */
127  void
129  {
131  fromPCLPointCloud2(*cloud2, *cloud);
132  setInputSource<PointXYZ>(cloud);
133  }
134 
135  /** \brief See if this rejector requires a target cloud */
136  bool
137  requiresTargetPoints() const override
138  {
139  return (true);
140  }
141 
142  /** \brief Method for setting the target cloud */
143  void
145  {
147  fromPCLPointCloud2(*cloud2, *cloud);
148  setInputTarget<PointXYZ>(cloud);
149  }
150 
151  /** \brief Provide a pointer to the search object used to find correspondences in
152  * the target cloud.
153  * \param[in] tree a pointer to the spatial search object.
154  * \param[in] force_no_recompute If set to true, this tree will NEVER be
155  * recomputed, regardless of calls to setInputTarget. Only use if you are
156  * confident that the tree will be set correctly.
157  */
158  template <typename PointT>
159  inline void
161  bool force_no_recompute = false)
162  {
163  static_pointer_cast<DataContainer<PointT>>(data_container_)
164  ->setSearchMethodTarget(tree, force_no_recompute);
165  }
166 
167  /** \brief Set the factor for correspondence rejection. Points with distance greater
168  * than median times factor will be rejected \param[in] factor value
169  */
170  inline void
171  setMedianFactor(double factor)
172  {
173  factor_ = factor;
174  };
175 
176  /** \brief Get the factor used for thresholding in correspondence rejection. */
177  inline double
179  {
180  return factor_;
181  };
182 
183 protected:
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
194  * <-> target.
195  */
196  double median_distance_{0.0};
197 
198  /** \brief The factor for correspondence rejection. Points with distance greater than
199  * median times factor will be rejected
200  */
201  double factor_{1.0};
202 
204 
205  /** \brief A pointer to the DataContainer object containing the input and target point
206  * clouds */
208 };
209 } // namespace registration
210 } // namespace pcl
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
CorrespondenceRejector represents the base class for correspondence rejection methods
shared_ptr< const CorrespondenceRejector > ConstPtr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< CorrespondenceRejector > Ptr
std::string rejection_name_
The name of the rejection method.
CorrespondenceRejectorMedianDistance implements a simple correspondence rejection method based on thr...
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.
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...
double getMedianDistance() const
Get the median distance used for thresholding in correspondence rejection.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
bool requiresSourcePoints() const override
See if this rejector requires source points.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
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.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setMedianFactor(double factor)
Set the factor for correspondence rejection.
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...
double getMedianFactor() const
Get the factor used for thresholding in correspondence rejection.
DataContainer is a container for the input and target point clouds and implements the interface to co...
shared_ptr< DataContainerInterface > Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
Defines functions, macros and traits for allocating and using memory.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:229
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
#define PCL_EXPORTS
Definition: pcl_macros.h:323
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr