Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
49namespace pcl {
50namespace 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 */
66 using CorrespondenceRejector::getClassName;
67 using CorrespondenceRejector::input_correspondences_;
68 using CorrespondenceRejector::rejection_name_;
69
70public:
71 using Ptr = shared_ptr<CorrespondenceRejectorVarTrimmed>;
72 using ConstPtr = shared_ptr<const CorrespondenceRejectorVarTrimmed>;
73
74 /** \brief Empty constructor. */
76 {
77 rejection_name_ = "CorrespondenceRejectorVarTrimmed";
78 }
79
80 /** \brief Get a list of valid correspondences after rejection from the original set
81 * of correspondences. \param[in] original_correspondences the set of initial
82 * correspondences given \param[out] remaining_correspondences the resultant filtered
83 * set of remaining correspondences
84 */
85 void
86 getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
87 pcl::Correspondences& remaining_correspondences) override;
88
89 /** \brief Get the trimmed distance used for thresholding in correspondence rejection.
90 */
91 inline double
93 {
94 return trimmed_distance_;
95 };
96
97 /** \brief Provide a source point cloud dataset (must contain XYZ
98 * data!), used to compute the correspondence distance.
99 * \param[in] cloud a cloud containing XYZ data
100 */
101 template <typename PointT>
102 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 target point cloud dataset (must contain XYZ
111 * data!), used to compute the correspondence distance.
112 * \param[in] target a cloud containing XYZ data
113 */
114 template <typename PointT>
115 inline void
117 {
118 if (!data_container_)
119 data_container_.reset(new DataContainer<PointT>);
120 static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
121 }
122
123 /** \brief See if this rejector requires source points */
124 bool
125 requiresSourcePoints() const override
126 {
127 return (true);
128 }
129
130 /** \brief Blob method for setting the source cloud */
131 void
133 {
135 fromPCLPointCloud2(*cloud2, *cloud);
136 setInputSource<PointXYZ>(cloud);
137 }
138
139 /** \brief See if this rejector requires a target cloud */
140 bool
141 requiresTargetPoints() const override
142 {
143 return (true);
144 }
145
146 /** \brief Method for setting the target cloud */
147 void
149 {
151 fromPCLPointCloud2(*cloud2, *cloud);
152 setInputTarget<PointXYZ>(cloud);
153 }
154
155 /** \brief Provide a pointer to the search object used to find correspondences in
156 * the target cloud.
157 * \param[in] tree a pointer to the spatial search object.
158 * \param[in] force_no_recompute If set to true, this tree will NEVER be
159 * recomputed, regardless of calls to setInputTarget. Only use if you are
160 * confident that the tree will be set correctly.
161 */
162 template <typename PointT>
163 inline void
165 bool force_no_recompute = false)
166 {
167 static_pointer_cast<DataContainer<PointT>>(data_container_)
168 ->setSearchMethodTarget(tree, force_no_recompute);
169 }
170
171 /** \brief Get the computed inlier ratio used for thresholding in correspondence
172 * rejection. */
173 inline double
175 {
176 return factor_;
177 }
178
179 /** brief set the minimum overlap ratio
180 * \param[in] ratio the overlap ratio [0..1]
181 */
182 inline void
183 setMinRatio(double ratio)
184 {
185 min_ratio_ = ratio;
186 }
187
188 /** brief get the minimum overlap ratio
189 */
190 inline double
192 {
193 return min_ratio_;
194 }
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)
201 {
202 max_ratio_ = ratio;
203 }
204
205 /** brief get the maximum overlap ratio
206 */
207 inline double
209 {
210 return max_ratio_;
211 }
212
213protected:
214 /** \brief Apply the rejection algorithm.
215 * \param[out] correspondences the set of resultant correspondences.
216 */
217 inline void
218 applyRejection(pcl::Correspondences& correspondences) override
219 {
220 getRemainingCorrespondences(*input_correspondences_, correspondences);
221 }
222
223 /** \brief The inlier distance threshold (based on the computed trim factor) between
224 * two correspondent points in source <-> target.
225 */
226 double trimmed_distance_{0.0};
227
228 /** \brief The factor for correspondence rejection. Only factor times the total points
229 * sorted based on the correspondence distances will be considered as inliers.
230 * Remaining points are rejected. This factor is computed internally
231 */
232 double factor_{0.0};
233
234 /** \brief The minimum overlap ratio between the input and target clouds
235 */
236 double min_ratio_{0.05};
237
238 /** \brief The maximum overlap ratio between the input and target clouds
239 */
240 double max_ratio_{0.95};
241
242 /** \brief part of the term that balances the root mean square difference. This is an
243 * internal parameter
244 */
245 double lambda_{0.95};
246
248
249 /** \brief A pointer to the DataContainer object containing the input and target point
250 * clouds */
252
253private:
254 /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust
255 * ICP for minimizing Fractional RMSD, J. M. Philips et al'
256 */
257 inline float
258 optimizeInlierRatio(std::vector<double>& dists) const;
259};
260} // namespace registration
261} // namespace pcl
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods
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
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.
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
shared_ptr< const CorrespondenceRejectorVarTrimmed > ConstPtr
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr