Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
correspondence_rejection_features.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/point_cloud.h>
45#include <pcl/point_representation.h>
46
47#include <unordered_map>
48
49namespace pcl {
50namespace registration {
51/** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method
52 * based on a set of feature descriptors. Given an input feature space, the method
53 * checks if each feature in the source cloud has a correspondence in the target cloud,
54 * either by checking the first K (given) point correspondences, or by defining a
55 * tolerance threshold via a radius in feature space. \todo explain this better. \author
56 * Radu B. Rusu \ingroup registration
57 */
59 using CorrespondenceRejector::getClassName;
60 using CorrespondenceRejector::input_correspondences_;
61 using CorrespondenceRejector::rejection_name_;
62
63public:
64 using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
65 using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
66
67 /** \brief Empty constructor. */
68 CorrespondenceRejectorFeatures() : max_distance_(std::numeric_limits<float>::max())
69 {
70 rejection_name_ = "CorrespondenceRejectorFeatures";
71 }
72
73 /** \brief Empty destructor. */
75
76 /** \brief Get a list of valid correspondences after rejection from the original set
77 * of correspondences \param[in] original_correspondences the set of initial
78 * correspondences given \param[out] remaining_correspondences the resultant filtered
79 * set of remaining correspondences
80 */
81 void
82 getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
83 pcl::Correspondences& remaining_correspondences) override;
84
85 /** \brief Provide a pointer to a cloud of feature descriptors associated with the
86 * source point cloud \param[in] source_feature a cloud of feature descriptors
87 * associated with the source point cloud \param[in] key a string that uniquely
88 * identifies the feature
89 */
90 template <typename FeatureT>
91 inline void
92 setSourceFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& source_feature,
93 const std::string& key);
94
95 /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
96 * given \a key \param[in] key a string that uniquely identifies the feature (must
97 * match the key provided by setSourceFeature)
98 */
99 template <typename FeatureT>
101 getSourceFeature(const std::string& key);
102
103 /** \brief Provide a pointer to a cloud of feature descriptors associated with the
104 * target point cloud \param[in] target_feature a cloud of feature descriptors
105 * associated with the target point cloud \param[in] key a string that uniquely
106 * identifies the feature
107 */
108 template <typename FeatureT>
109 inline void
110 setTargetFeature(const typename pcl::PointCloud<FeatureT>::ConstPtr& target_feature,
111 const std::string& key);
112
113 /** \brief Get a pointer to the source cloud's feature descriptors, specified by the
114 * given \a key \param[in] key a string that uniquely identifies the feature (must
115 * match the key provided by setTargetFeature)
116 */
117 template <typename FeatureT>
119 getTargetFeature(const std::string& key);
120
121 /** \brief Set a hard distance threshold in the feature \a FeatureT space, between
122 * source and target features. Any feature correspondence that is above this threshold
123 * will be considered bad and will be filtered out. \param[in] thresh the distance
124 * threshold \param[in] key a string that uniquely identifies the feature
125 */
126 template <typename FeatureT>
127 inline void
128 setDistanceThreshold(double thresh, const std::string& key);
129
130 /** \brief Test that all features are valid (i.e., does each key have a valid source
131 * cloud, target cloud, and search method)
132 */
133 inline bool
135
136 /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
137 * comparing features \param[in] key a string that uniquely identifies the feature
138 * \param[in] fr the point feature representation to be used
139 */
140 template <typename FeatureT>
141 inline void
142 setFeatureRepresentation(
144 const std::string& key);
145
146protected:
147 /** \brief Apply the rejection algorithm.
148 * \param[out] correspondences the set of resultant correspondences.
149 */
150 inline void
151 applyRejection(pcl::Correspondences& correspondences) override
152 {
153 getRemainingCorrespondences(*input_correspondences_, correspondences);
154 }
155
156 /** \brief The maximum distance threshold between two correspondent points in source
157 * <-> target. If the distance is larger than this threshold, the points will not be
158 * ignored in the alignment process.
159 */
161
163 public:
164 /** \brief Empty destructor */
165 virtual ~FeatureContainerInterface() = default;
166 virtual bool
167 isValid() = 0;
168 virtual double
170 virtual bool
171 isCorrespondenceValid(int index) = 0;
172
173 using Ptr = shared_ptr<FeatureContainerInterface>;
174 };
175
176 using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
177
178 /** \brief An STL map containing features to use when performing the correspondence
179 * search.*/
181
182 /** \brief An inner class containing pointers to the source and target feature clouds
183 * and the parameters needed to perform the correspondence search. This class extends
184 * FeatureContainerInterface, which contains abstract methods for any methods that do
185 * not depend on the FeatureT --- these methods can thus be called from a pointer to
186 * FeatureContainerInterface without casting to the derived class.
187 */
188 template <typename FeatureT>
191 public:
193 using SearchMethod = std::function<int(
194 const pcl::PointCloud<FeatureT>&, int, pcl::Indices&, std::vector<float>&)>;
195
198
200 : thresh_(std::numeric_limits<double>::max()), feature_representation_()
201 {}
202
203 /** \brief Empty destructor */
205
206 inline void
208 {
209 source_features_ = source_features;
210 }
211
212 inline FeatureCloudConstPtr
214 {
215 return (source_features_);
216 }
217
218 inline void
220 {
221 target_features_ = target_features;
222 }
223
224 inline FeatureCloudConstPtr
226 {
227 return (target_features_);
228 }
229
230 inline void
231 setDistanceThreshold(double thresh)
232 {
233 thresh_ = thresh;
234 }
235
236 inline bool
237 isValid() override
238 {
239 if (!source_features_ || !target_features_)
240 return (false);
241 return (source_features_->size() > 0 && target_features_->size() > 0);
242 }
243
244 /** \brief Provide a boost shared pointer to a PointRepresentation to be used when
245 * comparing features \param[in] fr the point feature representation to be used
246 */
247 inline void
249 {
250 feature_representation_ = fr;
251 }
252
253 /** \brief Obtain a score between a pair of correspondences.
254 * \param[in] index the index to check in the list of correspondences
255 * \return score the resultant computed score
256 */
257 inline double
258 getCorrespondenceScore(int index) override
259 {
260 // If no feature representation was given, reset to the default implementation for
261 // FeatureT
262 if (!feature_representation_)
263 feature_representation_.reset(new DefaultFeatureRepresentation<FeatureT>);
264
265 // Get the source and the target feature from the list
266 const FeatureT& feat_src = (*source_features_)[index];
267 const FeatureT& feat_tgt = (*target_features_)[index];
268
269 // Check if the representations are valid
270 if (!feature_representation_->isValid(feat_src) ||
271 !feature_representation_->isValid(feat_tgt)) {
272 PCL_ERROR("[pcl::registration::%s::getCorrespondenceScore] Invalid feature "
273 "representation given!\n",
274 this->getClassName().c_str());
275 return (std::numeric_limits<double>::max());
276 }
277
278 // Set the internal feature point representation of choice
279 Eigen::VectorXf feat_src_ptr =
280 Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
281 feature_representation_->vectorize(FeatureT(feat_src), feat_src_ptr);
282 Eigen::VectorXf feat_tgt_ptr =
283 Eigen::VectorXf::Zero(feature_representation_->getNumberOfDimensions());
284 feature_representation_->vectorize(FeatureT(feat_tgt), feat_tgt_ptr);
285
286 // Compute the L2 norm
287 return ((feat_src_ptr - feat_tgt_ptr).squaredNorm());
288 }
289
290 /** \brief Check whether the correspondence pair at the given index is valid
291 * by computing the score and testing it against the user given threshold
292 * \param[in] index the index to check in the list of correspondences
293 * \return true if the correspondence is good, false otherwise
294 */
295 inline bool
296 isCorrespondenceValid(int index) override
297 {
298 return (getCorrespondenceScore(index) < thresh_ * thresh_);
299 }
300
301 private:
302 FeatureCloudConstPtr source_features_, target_features_;
303 SearchMethod search_method_;
304
305 /** \brief The L2 squared Euclidean threshold. */
306 double thresh_;
307
308 /** \brief The internal point feature representation used. */
309 PointRepresentationConstPtr feature_representation_;
310 };
311};
312} // namespace registration
313} // namespace pcl
314
315#include <pcl/registration/impl/correspondence_rejection_features.hpp>
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< const PointRepresentation< PointT > > ConstPtr
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing features.
bool isCorrespondenceValid(int index) override
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
double getCorrespondenceScore(int index) override
Obtain a score between a pair of correspondences.
std::function< int( const pcl::PointCloud< FeatureT > &, int, pcl::Indices &, std::vector< float > &)> SearchMethod
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
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.
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
bool hasValidFeatures()
Test that all features are valid (i.e., does each key have a valid source cloud, target cloud,...
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
shared_ptr< const CorrespondenceRejectorFeatures > ConstPtr
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
CorrespondenceRejector represents the base class for correspondence rejection methods
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133