Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
ia_kfpcs.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2014-, 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 are met
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the copyright holder(s) nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 */
36
37#pragma once
38
39#include <pcl/registration/ia_fpcs.h>
40
41namespace pcl {
42namespace registration {
43/** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based
44 * on keypoints as described in: "Markerless point cloud registration with
45 * keypoint-based 4-points congruent sets", Pascal Theiler, Jan Dirk Wegner, Konrad
46 * Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop Laser Scanning,
47 * Antalya, Turkey, 2013.
48 * \note Method has since been improved and some variations to the paper exist.
49 *
50 * The main differences to FPCSInitialAlignment are:
51 * <ol>
52 * <li> KFPCSInitialAlignment stores all solution candidates instead of only the best
53 * one
54 * <li> KFPCSInitialAlignment uses an MSAC approach to score candidates instead of
55 * counting inliers
56 * </ol>
57 * \author P.W.Theiler
58 * \ingroup registration
59 */
60template <typename PointSource,
61 typename PointTarget,
62 typename NormalT = pcl::Normal,
63 typename Scalar = float>
65: public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
66public:
67 /** \cond */
68 using Ptr =
69 shared_ptr<KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
70 using ConstPtr = shared_ptr<
72
75 using PointCloudSourceIterator = typename PointCloudSource::iterator;
76
79 using PointCloudTargetIterator = typename PointCloudTarget::iterator;
80
83 /** \endcond */
84
85 /** \brief Constructor. */
87
88 /** \brief Destructor. */
89 ~KFPCSInitialAlignment() override = default;
90
91 /** \brief Set the upper translation threshold used for score evaluation.
92 * \param[in] upper_trl_boundary upper translation threshold
93 */
94 inline void
95 setUpperTranslationThreshold(float upper_trl_boundary)
96 {
97 upper_trl_boundary_ = upper_trl_boundary;
98 };
99
100 /** \return the upper translation threshold used for score evaluation. */
101 inline float
103 {
104 return (upper_trl_boundary_);
105 };
106
107 /** \brief Set the lower translation threshold used for score evaluation.
108 * \param[in] lower_trl_boundary lower translation threshold
109 */
110 inline void
111 setLowerTranslationThreshold(float lower_trl_boundary)
112 {
113 lower_trl_boundary_ = lower_trl_boundary;
114 };
115
116 /** \return the lower translation threshold used for score evaluation. */
117 inline float
119 {
120 return (lower_trl_boundary_);
121 };
122
123 /** \brief Set the weighting factor of the translation cost term.
124 * \param[in] lambda the weighting factor of the translation cost term
125 */
126 inline void
127 setLambda(float lambda)
128 {
129 lambda_ = lambda;
130 };
131
132 /** \return the weighting factor of the translation cost term. */
133 inline float
134 getLambda() const
135 {
136 return (lambda_);
137 };
138
139 /** \brief Get the N best unique candidate matches according to their fitness score.
140 * The method only returns unique transformations comparing the translation
141 * and the 3D rotation to already returned transformations.
142 *
143 * \note The method may return less than N candidates, if the number of unique
144 * candidates is smaller than N
145 *
146 * \param[in] n number of best candidates to return
147 * \param[in] min_angle3d minimum 3D angle difference in radian
148 * \param[in] min_translation3d minimum 3D translation difference
149 * \param[out] candidates vector of unique candidates
150 */
151 void
152 getNBestCandidates(int n,
153 float min_angle3d,
154 float min_translation3d,
155 MatchingCandidates& candidates);
156
157 /** \brief Get all unique candidate matches with fitness scores above a threshold t.
158 * The method only returns unique transformations comparing the translation
159 * and the 3D rotation to already returned transformations.
160 *
161 * \param[in] t fitness score threshold
162 * \param[in] min_angle3d minimum 3D angle difference in radian
163 * \param[in] min_translation3d minimum 3D translation difference
164 * \param[out] candidates vector of unique candidates
165 */
166 void
167 getTBestCandidates(float t,
168 float min_angle3d,
169 float min_translation3d,
170 MatchingCandidates& candidates);
171
172protected:
173 using PCLBase<PointSource>::deinitCompute;
174 using PCLBase<PointSource>::input_;
175 using PCLBase<PointSource>::indices_;
176
177 using Registration<PointSource, PointTarget, Scalar>::reg_name_;
178 using Registration<PointSource, PointTarget, Scalar>::tree_;
179 using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
180 using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
181 using Registration<PointSource, PointTarget, Scalar>::correspondences_;
182 using Registration<PointSource, PointTarget, Scalar>::converged_;
183
184 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::delta_;
185 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
187 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
188 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
189 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
191 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_mse_;
192 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
194 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::diameter_;
195 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
197 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
198 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
200 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::validateMatch;
201
202 /** \brief Internal computation initialization. */
203 bool
204 initCompute() override;
205
206 /** \brief Method to handle current candidate matches. Here we validate and evaluate
207 * the matches w.r.t the base and store the sorted matches (together with score values
208 * and estimated transformations).
209 *
210 * \param[in] base_indices indices of base B
211 * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
212 * matches are reordered during this step. \param[out] candidates vector which
213 * contains the candidates matches M
214 */
215 void
216 handleMatches(const pcl::Indices& base_indices,
217 std::vector<pcl::Indices>& matches,
218 MatchingCandidates& candidates) override;
219
220 /** \brief Validate the transformation by calculating the score value after
221 * transforming the input source cloud. The resulting score is later used as the
222 * decision criteria of the best fitting match.
223 *
224 * \param[out] transformation updated orientation matrix using all inliers
225 * \param[out] fitness_score current best score
226 * \note fitness score is only updated if the score of the current transformation
227 * exceeds the input one. \return
228 * * < 0 if previous result is better than the current one (score remains)
229 * * = 0 current result is better than the previous one (score updated)
230 */
231 int
232 validateTransformation(Eigen::Matrix4f& transformation,
233 float& fitness_score) override;
234
235 /** \brief Final computation of best match out of vector of matches. To avoid cross
236 * thread dependencies during parallel running, a best match for each try was
237 * calculated. \note For forwards compatibility the candidates are stored in vectors
238 * of 'vectors of size 1'. \param[in] candidates vector of candidate matches
239 */
240 void
241 finalCompute(const std::vector<MatchingCandidates>& candidates) override;
242
243 /** \brief Lower boundary for translation costs calculation.
244 * \note If not set by the user, the translation costs are not used during evaluation.
245 */
247
248 /** \brief Upper boundary for translation costs calculation.
249 * \note If not set by the user, it is calculated from the estimated overlap and the
250 * diameter of the point cloud.
251 */
253
254 /** \brief Weighting factor for translation costs (standard = 0.5). */
255 float lambda_{0.5f};
256
257 /** \brief Container for resulting vector of registration candidates. */
259
260 /** \brief Flag if translation score should be used in validation (internal
261 * calculation). */
262 bool use_trl_score_{false};
263
264 /** \brief Subset of input indices on which we evaluate candidates.
265 * To speed up the evaluation, we only use a fix number of indices defined during
266 * initialization.
267 */
269};
270}; // namespace registration
271}; // namespace pcl
272
273#include <pcl/registration/impl/ia_kfpcs.hpp>
PCL base class.
Definition pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
typename VectorType::iterator iterator
shared_ptr< PointCloud< PointSource > > Ptr
Registration represents the base registration class for general purpose, ICP-like methods.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
std::string reg_name_
The registration method name.
shared_ptr< const Registration< PointSource, PointTarget, Scalar > > ConstPtr
typename PointCloudSource::Ptr PointCloudSourcePtr
shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
KdTreePtr tree_
A pointer to the spatial search object.
typename PointCloudTarget::Ptr PointCloudTargetPtr
bool converged_
Holds internal convergence state, given user parameters.
int ransac_iterations_
The number of iterations RANSAC should run for.
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition ia_fpcs.h:81
float max_edge_diff_
Maximal difference between the length of the base edges and valid match edges.
Definition ia_fpcs.h:538
float delta_
Delta value of 4pcs algorithm (standard = 1.0).
Definition ia_fpcs.h:486
float coincidation_limit_
Maximal distance between coinciding intersection points to find valid matches.
Definition ia_fpcs.h:543
float max_inlier_dist_sqr_
Maximal squared point distance between source and target points to count as inlier.
Definition ia_fpcs.h:553
bool normalize_delta_
Normalize delta flag.
Definition ia_fpcs.h:522
float max_mse_
Maximal mean squared errors of a transformation calculated from a candidate match.
Definition ia_fpcs.h:548
float approx_overlap_
Estimated overlap between source and target (standard = 0.5).
Definition ia_fpcs.h:477
virtual int validateMatch(const pcl::Indices &base_indices, const pcl::Indices &match_indices, const pcl::Correspondences &correspondences, Eigen::Matrix4f &transformation)
Validate the matching by computing the transformation between the source and target based on the four...
Definition ia_fpcs.hpp:849
float fitness_score_
Resulting fitness score of the best match.
Definition ia_fpcs.h:506
float diameter_
Estimated diameter of the target point cloud.
Definition ia_fpcs.h:509
float max_pair_diff_
Maximal difference between corresponding point pairs in source and target.
Definition ia_fpcs.h:533
float score_threshold_
Score threshold to stop calculation with success.
Definition ia_fpcs.h:491
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition ia_kfpcs.h:65
void getTBestCandidates(float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates)
Get all unique candidate matches with fitness scores above a threshold t.
Definition ia_kfpcs.hpp:294
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition ia_kfpcs.h:95
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition ia_kfpcs.h:268
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition ia_kfpcs.h:252
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition ia_kfpcs.hpp:215
void handleMatches(const pcl::Indices &base_indices, std::vector< pcl::Indices > &matches, MatchingCandidates &candidates) override
Method to handle current candidate matches.
Definition ia_kfpcs.hpp:118
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition ia_kfpcs.h:127
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition ia_kfpcs.h:111
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition ia_kfpcs.h:255
void getNBestCandidates(int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates)
Get the N best unique candidate matches according to their fitness score.
Definition ia_kfpcs.hpp:257
MatchingCandidates candidates_
Container for resulting vector of registration candidates.
Definition ia_kfpcs.h:258
int validateTransformation(Eigen::Matrix4f &transformation, float &fitness_score) override
Validate the transformation by calculating the score value after transforming the input source cloud.
Definition ia_kfpcs.hpp:165
~KFPCSInitialAlignment() override=default
Destructor.
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition ia_kfpcs.h:246
bool use_trl_score_
Flag if translation score should be used in validation (internal calculation).
Definition ia_kfpcs.h:262
bool initCompute() override
Internal computation initialization.
Definition ia_kfpcs.hpp:56
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
A point structure representing normal coordinates and the surface curvature estimate.
Container for matching candidate consisting of.