Point Cloud Library (PCL)  1.8.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 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_VAR_TRIMMED_H_
40 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_VAR_TRIMMED_H_
41 
42 #include <pcl/registration/correspondence_rejection.h>
43 #include <pcl/point_cloud.h>
44 
45 #include <vector>
46 
47 namespace pcl
48 {
49  namespace registration
50  {
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 mentioned
55  * in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al'
56  *
57  * \note If \ref setInputCloud and \ref setInputTarget are given, then the
58  * distances between correspondences will be estimated using the given XYZ
59  * data, and not read from the set of input correspondences.
60  *
61  * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
62  * \ingroup registration
63  */
65  {
69 
70  public:
71  typedef boost::shared_ptr<CorrespondenceRejectorVarTrimmed> Ptr;
72  typedef boost::shared_ptr<const CorrespondenceRejectorVarTrimmed> ConstPtr;
73 
74  /** \brief Empty constructor. */
76  trimmed_distance_ (0),
77  factor_ (),
78  min_ratio_ (0.05),
79  max_ratio_ (0.95),
80  lambda_ (0.95),
81  data_container_ ()
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);
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  boost::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> inline void
116  {
117  PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ());
118  if (!data_container_)
119  data_container_.reset (new DataContainer<PointT>);
120  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (cloud);
121  }
122 
123  /** \brief Provide a target point cloud dataset (must contain XYZ
124  * data!), used to compute the correspondence distance.
125  * \param[in] target a cloud containing XYZ data
126  */
127  template <typename PointT> inline void
129  {
130  if (!data_container_)
131  data_container_.reset (new DataContainer<PointT>);
132  boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target);
133  }
134 
135 
136 
137  /** \brief See if this rejector requires source points */
138  bool
140  { return (true); }
141 
142  /** \brief Blob method for setting the source cloud */
143  void
145  {
147  fromPCLPointCloud2 (*cloud2, *cloud);
148  setInputSource<PointXYZ> (cloud);
149  }
150 
151  /** \brief See if this rejector requires a target cloud */
152  bool
154  { return (true); }
155 
156  /** \brief Method for setting the target cloud */
157  void
159  {
161  fromPCLPointCloud2 (*cloud2, *cloud);
162  setInputTarget<PointXYZ> (cloud);
163  }
164 
165  /** \brief Provide a pointer to the search object used to find correspondences in
166  * the target cloud.
167  * \param[in] tree a pointer to the spatial search object.
168  * \param[in] force_no_recompute If set to true, this tree will NEVER be
169  * recomputed, regardless of calls to setInputTarget. Only use if you are
170  * confident that the tree will be set correctly.
171  */
172  template <typename PointT> inline void
173  setSearchMethodTarget (const boost::shared_ptr<pcl::search::KdTree<PointT> > &tree,
174  bool force_no_recompute = false)
175  {
176  boost::static_pointer_cast< DataContainer<PointT> >
177  (data_container_)->setSearchMethodTarget (tree, force_no_recompute );
178  }
179 
180  /** \brief Get the computed inlier ratio used for thresholding in correspondence rejection. */
181  inline double
182  getTrimFactor () const { return factor_; }
183 
184  /** brief set the minimum overlap ratio
185  * \param[in] ratio the overlap ratio [0..1]
186  */
187  inline void
188  setMinRatio (double ratio) { min_ratio_ = ratio; }
189 
190  /** brief get the minimum overlap ratio
191  */
192  inline double
193  getMinRatio () const { return min_ratio_; }
194 
195  /** brief set the maximum overlap ratio
196  * \param[in] ratio the overlap ratio [0..1]
197  */
198  inline void
199  setMaxRatio (double ratio) { max_ratio_ = ratio; }
200 
201  /** brief get the maximum overlap ratio
202  */
203  inline double
204  getMaxRatio () const { return max_ratio_; }
205 
206  protected:
207 
208  /** \brief Apply the rejection algorithm.
209  * \param[out] correspondences the set of resultant correspondences.
210  */
211  inline void
213  {
214  getRemainingCorrespondences (*input_correspondences_, correspondences);
215  }
216 
217  /** \brief The inlier distance threshold (based on the computed trim factor) between two correspondent points in source <-> target.
218  */
220 
221  /** \brief The factor for correspondence rejection. Only factor times the total points sorted based on
222  * the correspondence distances will be considered as inliers. Remaining points are rejected. This factor is
223  * computed internally
224  */
225  double factor_;
226 
227  /** \brief The minimum overlap ratio between the input and target clouds
228  */
229  double min_ratio_;
230 
231  /** \brief The maximum overlap ratio between the input and target clouds
232  */
233  double max_ratio_;
234 
235  /** \brief part of the term that balances the root mean square difference. This is an internal parameter
236  */
237  double lambda_;
238 
239  typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
240 
241  /** \brief A pointer to the DataContainer object containing the input and target point clouds */
242  DataContainerPtr data_container_;
243 
244  private:
245 
246  /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al'
247  */
248  inline float optimizeInlierRatio (std::vector <double> &dists);
249  };
250  }
251 }
252 
253 #include <pcl/registration/impl/correspondence_rejection_var_trimmed.hpp>
254 
255 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_VAR_TRIMMED_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<T> 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...
CorrespondenceRejectoVarTrimmed implements a simple correspondence rejection method by considering as...
boost::shared_ptr< const CorrespondenceRejectorVarTrimmed > ConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods ...
double trimmed_distance_
The inlier distance threshold (based on the computed trim factor) between two correspondent points in...
double getMinRatio() const
brief get the minimum overlap ratio
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Blob method for setting the source cloud.
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 setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Method for setting the target cloud.
const std::string & getClassName() const
Get a string representation of the name of this class.
boost::shared_ptr< CorrespondenceRejectorVarTrimmed > Ptr
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.
bool requiresTargetPoints() const
See if this rejector requires a target cloud.
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...
void setMinRatio(double ratio)
brief set the minimum overlap ratio
void setMaxRatio(double ratio)
brief set the maximum overlap ratio
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
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...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
double min_ratio_
The minimum overlap ratio between the input and target clouds.
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 getMaxRatio() const
brief get the maximum overlap ratio
CorrespondencesConstPtr input_correspondences_
The input correspondences.
std::string rejection_name_
The name of the rejection method.
double getTrimmedDistance() const
Get the trimmed distance used for thresholding in correspondence rejection.
double getTrimFactor() const
Get the computed inlier ratio used for thresholding in correspondence rejection.
bool requiresSourcePoints() const
See if this rejector requires source points.
double max_ratio_
The maximum overlap ratio between the input and target clouds.