Point Cloud Library (PCL)  1.13.1-dev
correspondence_rejection_var_trimmed.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 Open Perception, Inc. 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 
40 #pragma once
41 
42 #include <pcl/registration/correspondence_rejection.h>
43 #include <pcl/conversions.h> // for fromPCLPointCloud2
44 #include <pcl/memory.h> // for static_pointer_cast
45 #include <pcl/point_cloud.h>
46 
47 #include <vector>
48 
49 namespace pcl {
50 namespace registration {
51 /**
52  * @b CorrespondenceRejectoVarTrimmed implements a simple correspondence
53  * rejection method by considering as inliers a certain percentage of correspondences
54  * with the least distances. The percentage of inliers is computed internally as
55  * mentioned in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M.
56  * Philips et al'
57  *
58  * \note If \ref setInputCloud and \ref setInputTarget are given, then the
59  * distances between correspondences will be estimated using the given XYZ
60  * data, and not read from the set of input correspondences.
61  *
62  * \author Aravindhan K Krishnan. This code is ported from libpointmatcher
63  * (https://github.com/ethz-asl/libpointmatcher) \ingroup registration
64  */
69 
70 public:
71  using Ptr = shared_ptr<CorrespondenceRejectorVarTrimmed>;
72  using ConstPtr = shared_ptr<const CorrespondenceRejectorVarTrimmed>;
73 
74  /** \brief Empty constructor. */
76  : trimmed_distance_(0), factor_(), min_ratio_(0.05), max_ratio_(0.95), lambda_(0.95)
77  {
78  rejection_name_ = "CorrespondenceRejectorVarTrimmed";
79  }
80 
81  /** \brief Get a list of valid correspondences after rejection from the original set
82  * of correspondences. \param[in] original_correspondences the set of initial
83  * correspondences given \param[out] remaining_correspondences the resultant filtered
84  * set of remaining correspondences
85  */
86  void
87  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
88  pcl::Correspondences& remaining_correspondences) override;
89 
90  /** \brief Get the trimmed distance used for thresholding in correspondence rejection.
91  */
92  inline double
94  {
95  return trimmed_distance_;
96  };
97 
98  /** \brief Provide a source point cloud dataset (must contain XYZ
99  * data!), used to compute the correspondence distance.
100  * \param[in] cloud a cloud containing XYZ data
101  */
102  template <typename PointT>
103  inline void
105  {
106  if (!data_container_)
107  data_container_.reset(new DataContainer<PointT>);
108  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
109  }
110 
111  /** \brief Provide a target point cloud dataset (must contain XYZ
112  * data!), used to compute the correspondence distance.
113  * \param[in] target a cloud containing XYZ data
114  */
115  template <typename PointT>
116  inline void
118  {
119  if (!data_container_)
120  data_container_.reset(new DataContainer<PointT>);
121  static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
122  }
123 
124  /** \brief See if this rejector requires source points */
125  bool
126  requiresSourcePoints() const override
127  {
128  return (true);
129  }
130 
131  /** \brief Blob method for setting the source cloud */
132  void
134  {
136  fromPCLPointCloud2(*cloud2, *cloud);
137  setInputSource<PointXYZ>(cloud);
138  }
139 
140  /** \brief See if this rejector requires a target cloud */
141  bool
142  requiresTargetPoints() const override
143  {
144  return (true);
145  }
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>
164  inline void
166  bool force_no_recompute = false)
167  {
168  static_pointer_cast<DataContainer<PointT>>(data_container_)
169  ->setSearchMethodTarget(tree, force_no_recompute);
170  }
171 
172  /** \brief Get the computed inlier ratio used for thresholding in correspondence
173  * rejection. */
174  inline double
176  {
177  return factor_;
178  }
179 
180  /** brief set the minimum overlap ratio
181  * \param[in] ratio the overlap ratio [0..1]
182  */
183  inline void
184  setMinRatio(double ratio)
185  {
186  min_ratio_ = ratio;
187  }
188 
189  /** brief get the minimum overlap ratio
190  */
191  inline double
192  getMinRatio() const
193  {
194  return min_ratio_;
195  }
196 
197  /** brief set the maximum overlap ratio
198  * \param[in] ratio the overlap ratio [0..1]
199  */
200  inline void
201  setMaxRatio(double ratio)
202  {
203  max_ratio_ = ratio;
204  }
205 
206  /** brief get the maximum overlap ratio
207  */
208  inline double
209  getMaxRatio() const
210  {
211  return max_ratio_;
212  }
213 
214 protected:
215  /** \brief Apply the rejection algorithm.
216  * \param[out] correspondences the set of resultant correspondences.
217  */
218  inline void
219  applyRejection(pcl::Correspondences& correspondences) override
220  {
221  getRemainingCorrespondences(*input_correspondences_, correspondences);
222  }
223 
224  /** \brief The inlier distance threshold (based on the computed trim factor) between
225  * two correspondent points in source <-> target.
226  */
228 
229  /** \brief The factor for correspondence rejection. Only factor times the total points
230  * sorted based on the correspondence distances will be considered as inliers.
231  * Remaining points are rejected. This factor is computed internally
232  */
233  double factor_;
234 
235  /** \brief The minimum overlap ratio between the input and target clouds
236  */
237  double min_ratio_;
238 
239  /** \brief The maximum overlap ratio between the input and target clouds
240  */
241  double max_ratio_;
242 
243  /** \brief part of the term that balances the root mean square difference. This is an
244  * internal parameter
245  */
246  double lambda_;
247 
249 
250  /** \brief A pointer to the DataContainer object containing the input and target point
251  * clouds */
253 
254 private:
255  /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust
256  * ICP for minimizing Fractional RMSD, J. M. Philips et al'
257  */
258  inline float
259  optimizeInlierRatio(std::vector<double>& dists) const;
260 };
261 } // namespace registration
262 } // 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.
CorrespondenceRejectoVarTrimmed implements a simple correspondence rejection method by considering as...
bool requiresSourcePoints() const override
See if this rejector requires source points.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void setMaxRatio(double ratio)
brief set the maximum overlap ratio
double trimmed_distance_
The inlier distance threshold (based on the computed trim factor) between two correspondent points in...
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 getMaxRatio() const
brief get the maximum overlap ratio
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 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 setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
double getTrimmedDistance() const
Get the trimmed distance used for thresholding in correspondence rejection.
void setMinRatio(double ratio)
brief set the minimum overlap ratio
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
double lambda_
part of the term that balances the root mean square difference.
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.
double getMinRatio() const
brief get the minimum overlap ratio
double min_ratio_
The minimum overlap ratio between the input and target clouds.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
double max_ratio_
The maximum overlap ratio between the input and target clouds.
double getTrimFactor() const
Get the computed inlier ratio 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:163
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
#define PCL_EXPORTS
Definition: pcl_macros.h:323
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr