Point Cloud Library (PCL)  1.14.1-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 #include <pcl/registration/correspondence_rejection.h>
44 #include <pcl/memory.h>
45 
46 namespace pcl {
47 namespace registration {
48 /** \brief CorrespondenceRejectorSampleConsensus implements a correspondence rejection
49  * using Random Sample Consensus to identify inliers (and reject outliers)
50  * \author Dirk Holz
51  * \ingroup registration
52  */
53 template <typename PointT>
56  using PointCloudPtr = typename PointCloud::Ptr;
57  using PointCloudConstPtr = typename PointCloud::ConstPtr;
58 
59 public:
63 
64  using Ptr = shared_ptr<CorrespondenceRejectorSampleConsensus<PointT>>;
65  using ConstPtr = shared_ptr<const CorrespondenceRejectorSampleConsensus<PointT>>;
66 
67  /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m),
68  * and the maximum number of iterations to 1000.
69  */
71  {
72  rejection_name_ = "CorrespondenceRejectorSampleConsensus";
73  }
74 
75  /** \brief Empty destructor. */
77 
78  /** \brief Get a list of valid correspondences after rejection from the original set
79  * of correspondences. \param[in] original_correspondences the set of initial
80  * correspondences given \param[out] remaining_correspondences the resultant filtered
81  * set of remaining correspondences
82  */
83  inline void
84  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
85  pcl::Correspondences& remaining_correspondences) override;
86 
87  /** \brief Provide a source point cloud dataset (must contain XYZ data!)
88  * \param[in] cloud a cloud containing XYZ data
89  */
90  virtual inline void
91  setInputSource(const PointCloudConstPtr& cloud)
92  {
93  input_ = cloud;
94  }
95 
96  /** \brief Get a pointer to the input point cloud dataset target. */
97  inline PointCloudConstPtr const
99  {
100  return (input_);
101  }
102 
103  /** \brief Provide a target point cloud dataset (must contain XYZ data!)
104  * \param[in] cloud a cloud containing XYZ data
105  */
106  virtual inline void
107  setInputTarget(const PointCloudConstPtr& cloud)
108  {
109  target_ = cloud;
110  }
111 
112  /** \brief Get a pointer to the input point cloud dataset target. */
113  inline PointCloudConstPtr const
115  {
116  return (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  {
130  PointCloudPtr cloud(new PointCloud);
131  fromPCLPointCloud2(*cloud2, *cloud);
132  setInputSource(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  {
146  PointCloudPtr cloud(new PointCloud);
147  fromPCLPointCloud2(*cloud2, *cloud);
148  setInputTarget(cloud);
149  }
150 
151  /** \brief Set the maximum distance between corresponding points.
152  * Correspondences with distances below the threshold are considered as inliers.
153  * \param[in] threshold Distance threshold in the same dimension as source and target
154  * data sets.
155  */
156  inline void
157  setInlierThreshold(double threshold)
158  {
159  inlier_threshold_ = threshold;
160  };
161 
162  /** \brief Get the maximum distance between corresponding points.
163  * \return Distance threshold in the same dimension as source and target data sets.
164  */
165  inline double
167  {
168  return inlier_threshold_;
169  };
170 
171  /** \brief Set the maximum number of iterations.
172  * \param[in] max_iterations Maximum number if iterations to run
173  */
174  inline void
175  setMaximumIterations(int max_iterations)
176  {
177  max_iterations_ = std::max(max_iterations, 0);
178  }
179 
180  /** \brief Get the maximum number of iterations.
181  * \return max_iterations Maximum number if iterations to run
182  */
183  inline int
185  {
186  return (max_iterations_);
187  }
188 
189  /** \brief Get the best transformation after RANSAC rejection.
190  * \return The homogeneous 4x4 transformation yielding the largest number of inliers.
191  */
192  inline Eigen::Matrix4f
194  {
195  return best_transformation_;
196  };
197 
198  /** \brief Specify whether the model should be refined internally using the variance
199  * of the inliers \param[in] refine true if the model should be refined, false
200  * otherwise
201  */
202  inline void
203  setRefineModel(const bool refine)
204  {
205  refine_ = refine;
206  }
207 
208  /** \brief Get the internal refine parameter value as set by the user using
209  * setRefineModel */
210  inline bool
212  {
213  return (refine_);
214  }
215 
216  /** \brief Get the inlier indices found by the correspondence rejector. This
217  * information is only saved if setSaveInliers(true) was called in advance.
218  * \param[out] inlier_indices Indices for the inliers
219  */
220  inline void
222  {
223  inlier_indices = inlier_indices_;
224  }
225 
226  /** \brief Set whether to save inliers or not
227  * \param[in] s True to save inliers / False otherwise
228  */
229  inline void
231  {
232  save_inliers_ = s;
233  }
234 
235  /** \brief Get whether the rejector is configured to save inliers */
236  inline bool
238  {
239  return save_inliers_;
240  }
241 
242 protected:
243  /** \brief Apply the rejection algorithm.
244  * \param[out] correspondences the set of resultant correspondences.
245  */
246  inline void
247  applyRejection(pcl::Correspondences& correspondences) override
248  {
250  }
251 
252  double inlier_threshold_{0.05};
253 
254  int max_iterations_{1000};
255 
256  PointCloudConstPtr input_;
257  PointCloudPtr input_transformed_;
258  PointCloudConstPtr target_;
259 
260  Eigen::Matrix4f best_transformation_;
261 
262  bool refine_{false};
264  bool save_inliers_{false};
265 
266 public:
268 };
269 } // namespace registration
270 } // namespace pcl
271 
272 #include <pcl/registration/impl/correspondence_rejection_sample_consensus.hpp>
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.
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
bool requiresSourcePoints() const override
See if this rejector requires source points.
double getInlierThreshold()
Get the maximum distance between corresponding points.
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
bool getSaveInliers()
Get whether the rejector is configured to save inliers.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
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 setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
bool getRefineModel() const
Get the internal refine parameter value as set by the user using setRefineModel.
~CorrespondenceRejectorSampleConsensus() override=default
Empty destructor.
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
std::string rejection_name_
The name of the rejection method.
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void getInliersIndices(pcl::Indices &inlier_indices)
Get the inlier indices found by the correspondence rejector.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setRefineModel(const bool refine)
Specify whether the model should be refined internally using the variance of the inliers.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:86
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr