Point Cloud Library (PCL)  1.10.0-dev
correspondence_rejection_sample_consensus.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 
44 #include <pcl/pcl_macros.h>
45 #include <pcl/registration/correspondence_rejection.h>
46 
47 #include <pcl/sample_consensus/ransac.h>
48 #include <pcl/sample_consensus/sac_model_registration.h>
49 #include <pcl/common/transforms.h>
50 
51 namespace pcl
52 {
53  namespace registration
54  {
55  /** \brief CorrespondenceRejectorSampleConsensus implements a correspondence rejection
56  * using Random Sample Consensus to identify inliers (and reject outliers)
57  * \author Dirk Holz
58  * \ingroup registration
59  */
60  template <typename PointT>
62  {
64  using PointCloudPtr = typename PointCloud::Ptr;
65  using PointCloudConstPtr = typename PointCloud::ConstPtr;
66 
67  public:
71 
74 
75  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
76  * and the maximum number of iterations to 1000.
77  */
79  : inlier_threshold_ (0.05)
80  , max_iterations_ (1000) // std::numeric_limits<int>::max ()
81  , input_ ()
83  , target_ ()
84  , refine_ (false)
85  , save_inliers_ (false)
86  {
87  rejection_name_ = "CorrespondenceRejectorSampleConsensus";
88  }
89 
90  /** \brief Empty destructor. */
92 
93  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
94  * \param[in] original_correspondences the set of initial correspondences given
95  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
96  */
97  inline void
98  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
99  pcl::Correspondences& remaining_correspondences) override;
100 
101  /** \brief Provide a source point cloud dataset (must contain XYZ data!)
102  * \param[in] cloud a cloud containing XYZ data
103  */
104  virtual inline void
105  setInputSource (const PointCloudConstPtr &cloud)
106  {
107  input_ = cloud;
108  }
109 
110  /** \brief Get a pointer to the input point cloud dataset target. */
111  inline PointCloudConstPtr const
112  getInputSource () { return (input_); }
113 
114  /** \brief Provide a target point cloud dataset (must contain XYZ data!)
115  * \param[in] cloud a cloud containing XYZ data
116  */
117  virtual inline void
118  setInputTarget (const PointCloudConstPtr &cloud) { target_ = cloud; }
119 
120  /** \brief Get a pointer to the input point cloud dataset target. */
121  inline PointCloudConstPtr const
122  getInputTarget () { return (target_ ); }
123 
124 
125  /** \brief See if this rejector requires source points */
126  bool
127  requiresSourcePoints () const override
128  { return (true); }
129 
130  /** \brief Blob method for setting the source cloud */
131  void
133  {
134  PointCloudPtr cloud (new PointCloud);
135  fromPCLPointCloud2 (*cloud2, *cloud);
136  setInputSource (cloud);
137  }
138 
139  /** \brief See if this rejector requires a target cloud */
140  bool
141  requiresTargetPoints () const override
142  { return (true); }
143 
144  /** \brief Method for setting the target cloud */
145  void
147  {
148  PointCloudPtr cloud (new PointCloud);
149  fromPCLPointCloud2 (*cloud2, *cloud);
150  setInputTarget (cloud);
151  }
152 
153  /** \brief Set the maximum distance between corresponding points.
154  * Correspondences with distances below the threshold are considered as inliers.
155  * \param[in] threshold Distance threshold in the same dimension as source and target data sets.
156  */
157  inline void
158  setInlierThreshold (double threshold) { inlier_threshold_ = threshold; };
159 
160  /** \brief Get the maximum distance between corresponding points.
161  * \return Distance threshold in the same dimension as source and target data sets.
162  */
163  inline double
165 
166  /** \brief Set the maximum number of iterations.
167  * \param[in] max_iterations Maximum number if iterations to run
168  */
169  inline void
170  setMaximumIterations (int max_iterations) { max_iterations_ = std::max (max_iterations, 0); }
171 
172  /** \brief Get the maximum number of iterations.
173  * \return max_iterations Maximum number if iterations to run
174  */
175  inline int
177 
178  /** \brief Get the best transformation after RANSAC rejection.
179  * \return The homogeneous 4x4 transformation yielding the largest number of inliers.
180  */
181  inline Eigen::Matrix4f
183 
184  /** \brief Specify whether the model should be refined internally using the variance of the inliers
185  * \param[in] refine true if the model should be refined, false otherwise
186  */
187  inline void
188  setRefineModel (const bool refine)
189  {
190  refine_ = refine;
191  }
192 
193  /** \brief Get the internal refine parameter value as set by the user using setRefineModel */
194  inline bool
195  getRefineModel () const
196  {
197  return (refine_);
198  }
199 
200  /** \brief Get the inlier indices found by the correspondence rejector. This information is only saved if setSaveInliers(true) was called in advance.
201  * \param[out] inlier_indices Indices for the inliers
202  */
203  inline void
204  getInliersIndices (std::vector<int> &inlier_indices) { inlier_indices = inlier_indices_; }
205 
206  /** \brief Set whether to save inliers or not
207  * \param[in] s True to save inliers / False otherwise
208  */
209  inline void
210  setSaveInliers (bool s) { save_inliers_ = s; }
211 
212  /** \brief Get whether the rejector is configured to save inliers */
213  inline bool
215 
216 
217  protected:
218 
219  /** \brief Apply the rejection algorithm.
220  * \param[out] correspondences the set of resultant correspondences.
221  */
222  inline void
223  applyRejection (pcl::Correspondences &correspondences) override
224  {
226  }
227 
229 
231 
232  PointCloudConstPtr input_;
233  PointCloudPtr input_transformed_;
234  PointCloudConstPtr target_;
235 
236  Eigen::Matrix4f best_transformation_;
237 
238  bool refine_;
239  std::vector<int> inlier_indices_;
241 
242  public:
244  };
245  }
246 }
247 
248 #include <pcl/registration/impl/correspondence_rejection_sample_consensus.hpp>
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
double getInlierThreshold()
Get the maximum distance between corresponding points.
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 setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void getInliersIndices(std::vector< int > &inlier_indices)
Get the inlier indices found by the correspondence rejector.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:371
const std::string & getClassName() const
Get a string representation of the name of this class.
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
bool requiresSourcePoints() const override
See if this rejector requires source points.
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
bool getRefineModel() const
Get the internal refine parameter value as set by the user using setRefineModel.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
bool getSaveInliers()
Get whether the rejector is configured to save inliers.
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.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
void setRefineModel(const bool refine)
Specify whether the model should be refined internally using the variance of the inliers.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
Defines all the PCL and non-PCL macros used.