Point Cloud Library (PCL) 1.13.1
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. \note Method has since been improved and some variations to
48 * the paper exist. \author P.W.Theiler \ingroup registration
49 */
50template <typename PointSource,
51 typename PointTarget,
52 typename NormalT = pcl::Normal,
53 typename Scalar = float>
55: public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
56public:
57 /** \cond */
58 using Ptr =
59 shared_ptr<KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
60 using ConstPtr = shared_ptr<
62
65 using PointCloudSourceIterator = typename PointCloudSource::iterator;
66
69 using PointCloudTargetIterator = typename PointCloudTarget::iterator;
70
73 /** \endcond */
74
75 /** \brief Constructor. */
77
78 /** \brief Destructor. */
79 ~KFPCSInitialAlignment() override = default;
80
81 /** \brief Set the upper translation threshold used for score evaluation.
82 * \param[in] upper_trl_boundary upper translation threshold
83 */
84 inline void
85 setUpperTranslationThreshold(float upper_trl_boundary)
86 {
87 upper_trl_boundary_ = upper_trl_boundary;
88 };
89
90 /** \return the upper translation threshold used for score evaluation. */
91 inline float
93 {
94 return (upper_trl_boundary_);
95 };
96
97 /** \brief Set the lower translation threshold used for score evaluation.
98 * \param[in] lower_trl_boundary lower translation threshold
99 */
100 inline void
101 setLowerTranslationThreshold(float lower_trl_boundary)
102 {
103 lower_trl_boundary_ = lower_trl_boundary;
104 };
105
106 /** \return the lower translation threshold used for score evaluation. */
107 inline float
109 {
110 return (lower_trl_boundary_);
111 };
112
113 /** \brief Set the weighting factor of the translation cost term.
114 * \param[in] lambda the weighting factor of the translation cost term
115 */
116 inline void
117 setLambda(float lambda)
118 {
119 lambda_ = lambda;
120 };
121
122 /** \return the weighting factor of the translation cost term. */
123 inline float
124 getLambda() const
125 {
126 return (lambda_);
127 };
128
129 /** \brief Get the N best unique candidate matches according to their fitness score.
130 * The method only returns unique transformations comparing the translation
131 * and the 3D rotation to already returned transformations.
132 *
133 * \note The method may return less than N candidates, if the number of unique
134 * candidates is smaller than N
135 *
136 * \param[in] n number of best candidates to return
137 * \param[in] min_angle3d minimum 3D angle difference in radian
138 * \param[in] min_translation3d minimum 3D translation difference
139 * \param[out] candidates vector of unique candidates
140 */
141 void
142 getNBestCandidates(int n,
143 float min_angle3d,
144 float min_translation3d,
145 MatchingCandidates& candidates);
146
147 /** \brief Get all unique candidate matches with fitness scores above a threshold t.
148 * The method only returns unique transformations comparing the translation
149 * and the 3D rotation to already returned transformations.
150 *
151 * \param[in] t fitness score threshold
152 * \param[in] min_angle3d minimum 3D angle difference in radian
153 * \param[in] min_translation3d minimum 3D translation difference
154 * \param[out] candidates vector of unique candidates
155 */
156 void
157 getTBestCandidates(float t,
158 float min_angle3d,
159 float min_translation3d,
160 MatchingCandidates& candidates);
161
162protected:
163 using PCLBase<PointSource>::deinitCompute;
164 using PCLBase<PointSource>::input_;
165 using PCLBase<PointSource>::indices_;
166
167 using Registration<PointSource, PointTarget, Scalar>::reg_name_;
168 using Registration<PointSource, PointTarget, Scalar>::tree_;
169 using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
170 using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
171 using Registration<PointSource, PointTarget, Scalar>::correspondences_;
172 using Registration<PointSource, PointTarget, Scalar>::converged_;
173
174 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::delta_;
175 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
177 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
178 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
179 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
181 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_mse_;
182 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
184 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::diameter_;
185 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
187 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
188 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
190 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
192 using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::validateMatch;
193
194 /** \brief Internal computation initialization. */
195 bool
196 initCompute() override;
197
198 /** \brief Method to handle current candidate matches. Here we validate and evaluate
199 * the matches w.r.t the base and store the sorted matches (together with score values
200 * and estimated transformations).
201 *
202 * \param[in] base_indices indices of base B
203 * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
204 * matches are reordered during this step. \param[out] candidates vector which
205 * contains the candidates matches M
206 */
207 void
208 handleMatches(const pcl::Indices& base_indices,
209 std::vector<pcl::Indices>& matches,
210 MatchingCandidates& candidates) override;
211
212 /** \brief Validate the transformation by calculating the score value after
213 * transforming the input source cloud. The resulting score is later used as the
214 * decision criteria of the best fitting match.
215 *
216 * \param[out] transformation updated orientation matrix using all inliers
217 * \param[out] fitness_score current best score
218 * \note fitness score is only updated if the score of the current transformation
219 * exceeds the input one. \return
220 * * < 0 if previous result is better than the current one (score remains)
221 * * = 0 current result is better than the previous one (score updated)
222 */
223 int
224 validateTransformation(Eigen::Matrix4f& transformation,
225 float& fitness_score) override;
226
227 /** \brief Final computation of best match out of vector of matches. To avoid cross
228 * thread dependencies during parallel running, a best match for each try was
229 * calculated. \note For forwards compatibility the candidates are stored in vectors
230 * of 'vectors of size 1'. \param[in] candidates vector of candidate matches
231 */
232 void
233 finalCompute(const std::vector<MatchingCandidates>& candidates) override;
234
235 /** \brief Lower boundary for translation costs calculation.
236 * \note If not set by the user, the translation costs are not used during evaluation.
237 */
239
240 /** \brief Upper boundary for translation costs calculation.
241 * \note If not set by the user, it is calculated from the estimated overlap and the
242 * diameter of the point cloud.
243 */
245
246 /** \brief Weighting factor for translation costs (standard = 0.5). */
247 float lambda_;
248
249 /** \brief Container for resulting vector of registration candidates. */
251
252 /** \brief Flag if translation score should be used in validation (internal
253 * calculation). */
255
256 /** \brief Subset of input indices on which we evaluate candidates.
257 * To speed up the evaluation, we only use a fix number of indices defined during
258 * initialization.
259 */
261};
262}; // namespace registration
263}; // namespace pcl
264
265#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:174
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:537
float delta_
Delta value of 4pcs algorithm (standard = 1.0).
Definition ia_fpcs.h:485
float coincidation_limit_
Maximal distance between coinciding intersection points to find valid matches.
Definition ia_fpcs.h:542
virtual void linkMatchWithBase(const pcl::Indices &base_indices, pcl::Indices &match_indices, pcl::Correspondences &correspondences)
Sets the correspondences between the base B and the match M by using the distance of each point to th...
Definition ia_fpcs.hpp:779
float max_inlier_dist_sqr_
Maximal squared point distance between source and target points to count as inlier.
Definition ia_fpcs.h:552
bool normalize_delta_
Normalize delta flag.
Definition ia_fpcs.h:521
float max_mse_
Maximal mean squared errors of a transformation calculated from a candidate match.
Definition ia_fpcs.h:547
float approx_overlap_
Estimated overlap between source and target (standard = 0.5).
Definition ia_fpcs.h:476
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:832
float fitness_score_
Resulting fitness score of the best match.
Definition ia_fpcs.h:505
float diameter_
Estimated diameter of the target point cloud.
Definition ia_fpcs.h:508
float max_pair_diff_
Maximal difference between corresponding point pairs in source and target.
Definition ia_fpcs.h:532
float score_threshold_
Score threshold to stop calculation with success.
Definition ia_fpcs.h:490
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition ia_kfpcs.h:55
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:271
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition ia_kfpcs.h:85
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition ia_kfpcs.h:260
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition ia_kfpcs.h:244
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition ia_kfpcs.hpp:196
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:109
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition ia_kfpcs.h:117
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition ia_kfpcs.h:101
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition ia_kfpcs.h:247
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:234
MatchingCandidates candidates_
Container for resulting vector of registration candidates.
Definition ia_kfpcs.h:250
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:146
~KFPCSInitialAlignment() override=default
Destructor.
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition ia_kfpcs.h:238
bool use_trl_score_
Flag if translation score should be used in validation (internal calculation).
Definition ia_kfpcs.h:254
bool initCompute() override
Internal computation initialization.
Definition ia_kfpcs.hpp:60
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.