Point Cloud Library (PCL)  1.14.0-dev
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 
41 namespace pcl {
42 namespace 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  */
50 template <typename PointSource,
51  typename PointTarget,
52  typename NormalT = pcl::Normal,
53  typename Scalar = float>
55 : public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
56 public:
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 
162 protected:
166 
173 
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  */
238  float lower_trl_boundary_{-1.f};
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  */
244  float upper_trl_boundary_{-1.f};
245 
246  /** \brief Weighting factor for translation costs (standard = 0.5). */
247  float lambda_{0.5f};
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). */
254  bool use_trl_score_{false};
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
typename VectorType::iterator iterator
Definition: point_cloud.h:425
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
shared_ptr< const Registration< PointSource, PointTarget, float > > ConstPtr
Definition: registration.h:67
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:77
shared_ptr< Registration< PointSource, PointTarget, float > > Ptr
Definition: registration.h:66
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: registration.h:81
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:81
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:751
float max_inlier_dist_sqr_
Maximal squared point distance between source and target points to count as inlier.
Definition: ia_fpcs.h:552
float approx_overlap_
Estimated overlap between source and target (standard = 0.5).
Definition: ia_fpcs.h:476
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:267
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:192
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:105
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:230
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:142
~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: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.