Point Cloud Library (PCL)  1.11.1
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/memory.h> // for static_pointer_cast
44 #include <pcl/point_cloud.h>
45 
46 #include <vector>
47 
48 namespace pcl
49 {
50  namespace registration
51  {
52  /**
53  * @b CorrespondenceRejectoVarTrimmed implements a simple correspondence
54  * rejection method by considering as inliers a certain percentage of correspondences
55  * with the least distances. The percentage of inliers is computed internally as mentioned
56  * in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. 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 (https://github.com/ethz-asl/libpointmatcher)
63  * \ingroup registration
64  */
66  {
70 
71  public:
72  using Ptr = shared_ptr<CorrespondenceRejectorVarTrimmed>;
73  using ConstPtr = shared_ptr<const CorrespondenceRejectorVarTrimmed>;
74 
75  /** \brief Empty constructor. */
77  trimmed_distance_ (0),
78  factor_ (),
79  min_ratio_ (0.05),
80  max_ratio_ (0.95),
81  lambda_ (0.95)
82  {
83  rejection_name_ = "CorrespondenceRejectorVarTrimmed";
84  }
85 
86  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
87  * \param[in] original_correspondences the set of initial correspondences given
88  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
89  */
90  void
91  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
92  pcl::Correspondences& remaining_correspondences) override;
93 
94  /** \brief Get the trimmed distance used for thresholding in correspondence rejection. */
95  inline double
96  getTrimmedDistance () const { return trimmed_distance_; };
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> inline void
104  {
105  if (!data_container_)
106  data_container_.reset (new DataContainer<PointT>);
107  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
108  }
109 
110  /** \brief Provide a source point cloud dataset (must contain XYZ
111  * data!), used to compute the correspondence distance.
112  * \param[in] cloud a cloud containing XYZ data
113  */
114  template <typename PointT>
115  PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorVarTrimmed::setInputCloud is deprecated. Please use setInputSource instead")
116  inline void
117  setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud)
118  {
119  if (!data_container_)
120  data_container_.reset (new DataContainer<PointT>);
121  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
122  }
123 
124  /** \brief Provide a target point cloud dataset (must contain XYZ
125  * data!), used to compute the correspondence distance.
126  * \param[in] target a cloud containing XYZ data
127  */
128  template <typename PointT> inline void
130  {
131  if (!data_container_)
132  data_container_.reset (new DataContainer<PointT>);
133  static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
134  }
135 
136 
137 
138  /** \brief See if this rejector requires source points */
139  bool
140  requiresSourcePoints () const override
141  { return (true); }
142 
143  /** \brief Blob method for setting the source cloud */
144  void
146  {
148  fromPCLPointCloud2 (*cloud2, *cloud);
149  setInputSource<PointXYZ> (cloud);
150  }
151 
152  /** \brief See if this rejector requires a target cloud */
153  bool
154  requiresTargetPoints () const override
155  { return (true); }
156 
157  /** \brief Method for setting the target cloud */
158  void
160  {
162  fromPCLPointCloud2 (*cloud2, *cloud);
163  setInputTarget<PointXYZ> (cloud);
164  }
165 
166  /** \brief Provide a pointer to the search object used to find correspondences in
167  * the target cloud.
168  * \param[in] tree a pointer to the spatial search object.
169  * \param[in] force_no_recompute If set to true, this tree will NEVER be
170  * recomputed, regardless of calls to setInputTarget. Only use if you are
171  * confident that the tree will be set correctly.
172  */
173  template <typename PointT> inline void
175  bool force_no_recompute = false)
176  {
177  static_pointer_cast< DataContainer<PointT> >
178  (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
179  }
180 
181  /** \brief Get the computed inlier ratio used for thresholding in correspondence rejection. */
182  inline double
183  getTrimFactor () const { return factor_; }
184 
185  /** brief set the minimum overlap ratio
186  * \param[in] ratio the overlap ratio [0..1]
187  */
188  inline void
189  setMinRatio (double ratio) { min_ratio_ = ratio; }
190 
191  /** brief get the minimum overlap ratio
192  */
193  inline double
194  getMinRatio () const { return min_ratio_; }
195 
196  /** brief set the maximum overlap ratio
197  * \param[in] ratio the overlap ratio [0..1]
198  */
199  inline void
200  setMaxRatio (double ratio) { max_ratio_ = ratio; }
201 
202  /** brief get the maximum overlap ratio
203  */
204  inline double
205  getMaxRatio () const { return max_ratio_; }
206 
207  protected:
208 
209  /** \brief Apply the rejection algorithm.
210  * \param[out] correspondences the set of resultant correspondences.
211  */
212  inline void
213  applyRejection (pcl::Correspondences &correspondences) override
214  {
215  getRemainingCorrespondences (*input_correspondences_, correspondences);
216  }
217 
218  /** \brief The inlier distance threshold (based on the computed trim factor) between two correspondent points in source <-> target.
219  */
221 
222  /** \brief The factor for correspondence rejection. Only factor times the total points sorted based on
223  * the correspondence distances will be considered as inliers. Remaining points are rejected. This factor is
224  * computed internally
225  */
226  double factor_;
227 
228  /** \brief The minimum overlap ratio between the input and target clouds
229  */
230  double min_ratio_;
231 
232  /** \brief The maximum overlap ratio between the input and target clouds
233  */
234  double max_ratio_;
235 
236  /** \brief part of the term that balances the root mean square difference. This is an internal parameter
237  */
238  double lambda_;
239 
241 
242  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
244 
245  private:
246 
247  /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al'
248  */
249  inline float optimizeInlierRatio (std::vector <double> &dists) const;
250  };
251  }
252 }
253 
254 #include <pcl/registration/impl/correspondence_rejection_var_trimmed.hpp>
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
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...
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
Defines functions, macros and traits for allocating and using memory.
shared_ptr< CorrespondenceRejector > Ptr
CorrespondenceRejectoVarTrimmed implements a simple correspondence rejection method by considering as...
CorrespondenceRejector represents the base class for correspondence rejection methods ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
double trimmed_distance_
The inlier distance threshold (based on the computed trim factor) between two correspondent points in...
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
double getMinRatio() const
brief get the minimum overlap ratio
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...
const std::string & getClassName() const
Get a string representation of the name of this class.
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 setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setMinRatio(double ratio)
brief set the minimum overlap ratio
void setMaxRatio(double ratio)
brief set the maximum overlap ratio
bool requiresTargetPoints() const override
See if this rejector requires a target 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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
double min_ratio_
The minimum overlap ratio between the input and target clouds.
bool requiresSourcePoints() const override
See if this rejector requires source points.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
shared_ptr< DataContainerInterface > Ptr
double getMaxRatio() const
brief get the maximum overlap ratio
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
double getTrimmedDistance() const
Get the trimmed distance used for thresholding in correspondence rejection.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major...
Definition: pcl_macros.h:147
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...
A point structure representing Euclidean xyz coordinates, and the RGB color.
double getTrimFactor() const
Get the computed inlier ratio used for thresholding in correspondence rejection.
#define PCL_EXPORTS
Definition: pcl_macros.h:328
double max_ratio_
The maximum overlap ratio between the input and target clouds.