Point Cloud Library (PCL)  1.10.0
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 {
43  namespace registration
44  {
45  /** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints
46  * as described in: "Markerless point cloud registration with keypoint-based 4-points congruent sets",
47  * Pascal Theiler, Jan Dirk Wegner, Konrad Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop
48  * Laser Scanning, Antalya, Turkey, 2013.
49  * \note Method has since been improved and some variations to the paper exist.
50  * \author P.W.Theiler
51  * \ingroup registration
52  */
53  template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
54  class KFPCSInitialAlignment : public virtual FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>
55  {
56  public:
57  /** \cond */
60 
63  using PointCloudSourceIterator = typename PointCloudSource::iterator;
64 
67  using PointCloudTargetIterator = typename PointCloudTarget::iterator;
68 
71  /** \endcond */
72 
73 
74  /** \brief Constructor. */
76 
77  /** \brief Destructor. */
79  {};
80 
81 
82  /** \brief Set the upper translation threshold used for score evaluation.
83  * \param[in] upper_trl_boundary upper translation threshold
84  */
85  inline void
86  setUpperTranslationThreshold (float upper_trl_boundary)
87  {
88  upper_trl_boundary_ = upper_trl_boundary;
89  };
90 
91  /** \return the upper translation threshold used for score evaluation. */
92  inline float
94  {
95  return (upper_trl_boundary_);
96  };
97 
98 
99  /** \brief Set the lower translation threshold used for score evaluation.
100  * \param[in] lower_trl_boundary lower translation threshold
101  */
102  inline void
103  setLowerTranslationThreshold (float lower_trl_boundary)
104  {
105  lower_trl_boundary_ = lower_trl_boundary;
106  };
107 
108  /** \return the lower translation threshold used for score evaluation. */
109  inline float
111  {
112  return (lower_trl_boundary_);
113  };
114 
115 
116  /** \brief Set the weighting factor of the translation cost term.
117  * \param[in] lambda the weighting factor of the translation cost term
118  */
119  inline void
120  setLambda (float lambda)
121  {
122  lambda_ = lambda;
123  };
124 
125  /** \return the weighting factor of the translation cost term. */
126  inline float
127  getLambda () const
128  {
129  return (lambda_);
130  };
131 
132 
133  /** \brief Get the N best unique candidate matches according to their fitness score.
134  * The method only returns unique transformations comparing the translation
135  * and the 3D rotation to already returned transformations.
136  *
137  * \note The method may return less than N candidates, if the number of unique candidates
138  * is smaller than N
139  *
140  * \param[in] n number of best candidates to return
141  * \param[in] min_angle3d minimum 3D angle difference in radian
142  * \param[in] min_translation3d minimum 3D translation difference
143  * \param[out] candidates vector of unique candidates
144  */
145  void
146  getNBestCandidates (int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
147 
148  /** \brief Get all unique candidate matches with fitness scores above a threshold t.
149  * The method only returns unique transformations comparing the translation
150  * and the 3D rotation to already returned transformations.
151  *
152  * \param[in] t fitness score threshold
153  * \param[in] min_angle3d minimum 3D angle difference in radian
154  * \param[in] min_translation3d minimum 3D translation difference
155  * \param[out] candidates vector of unique candidates
156  */
157  void
158  getTBestCandidates (float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
159 
160 
161  protected:
162 
166 
173 
187 
188 
189  /** \brief Internal computation initialization. */
190  bool
191  initCompute () override;
192 
193  /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
194  * base and store the sorted matches (together with score values and estimated transformations).
195  *
196  * \param[in] base_indices indices of base B
197  * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are
198  * reordered during this step.
199  * \param[out] candidates vector which contains the candidates matches M
200  */
201  void
202  handleMatches (
203  const std::vector <int> &base_indices,
204  std::vector <std::vector <int> > &matches,
205  MatchingCandidates &candidates) override;
206 
207  /** \brief Validate the transformation by calculating the score value after transforming the input source cloud.
208  * The resulting score is later used as the decision criteria of the best fitting match.
209  *
210  * \param[out] transformation updated orientation matrix using all inliers
211  * \param[out] fitness_score current best score
212  * \note fitness score is only updated if the score of the current transformation exceeds the input one.
213  * \return
214  * * < 0 if previous result is better than the current one (score remains)
215  * * = 0 current result is better than the previous one (score updated)
216  */
217  int
218  validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score) override;
219 
220  /** \brief Final computation of best match out of vector of matches. To avoid cross thread dependencies
221  * during parallel running, a best match for each try was calculated.
222  * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
223  * \param[in] candidates vector of candidate matches
224  */
225  void
226  finalCompute (const std::vector <MatchingCandidates > &candidates) override;
227 
228 
229  /** \brief Lower boundary for translation costs calculation.
230  * \note If not set by the user, the translation costs are not used during evaluation.
231  */
233 
234  /** \brief Upper boundary for translation costs calculation.
235  * \note If not set by the user, it is calculated from the estimated overlap and the diameter
236  * of the point cloud.
237  */
239 
240  /** \brief Weighting factor for translation costs (standard = 0.5). */
241  float lambda_;
242 
243 
244  /** \brief Container for resulting vector of registration candidates. */
246 
247  /** \brief Flag if translation score should be used in validation (internal calculation). */
249 
250  /** \brief Subset of input indices on which we evaluate candidates.
251  * To speed up the evaluation, we only use a fix number of indices defined during initialization.
252  */
254 
255  };
256  }; // namespace registration
257 }; // namespace pcl
258 
259 #include <pcl/registration/impl/ia_kfpcs.hpp>
pcl::registration::KFPCSInitialAlignment::lower_trl_boundary_
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition: ia_kfpcs.h:232
pcl::Registration< PointSource, PointTarget, float >::Ptr
shared_ptr< Registration< PointSource, PointTarget, float > > Ptr
Definition: registration.h:70
pcl::registration::KFPCSInitialAlignment::KFPCSInitialAlignment
KFPCSInitialAlignment()
Constructor.
Definition: ia_kfpcs.hpp:42
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::registration::KFPCSInitialAlignment::upper_trl_boundary_
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition: ia_kfpcs.h:238
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:60
pcl::registration::FPCSInitialAlignment
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:76
pcl::registration::KFPCSInitialAlignment::initCompute
bool initCompute() override
Internal computation initialization.
Definition: ia_kfpcs.hpp:55
pcl::registration::KFPCSInitialAlignment::~KFPCSInitialAlignment
~KFPCSInitialAlignment()
Destructor.
Definition: ia_kfpcs.h:78
pcl::registration::KFPCSInitialAlignment::use_trl_score_
bool use_trl_score_
Flag if translation score should be used in validation (internal calculation).
Definition: ia_kfpcs.h:248
pcl::registration::KFPCSInitialAlignment::getLowerTranslationThreshold
float getLowerTranslationThreshold() const
Definition: ia_kfpcs.h:110
pcl::registration::KFPCSInitialAlignment::getNBestCandidates
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:215
pcl::registration::KFPCSInitialAlignment::setUpperTranslationThreshold
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition: ia_kfpcs.h:86
pcl::registration::KFPCSInitialAlignment::getTBestCandidates
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:254
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::PointCloud< PointSource >
pcl::registration::KFPCSInitialAlignment::getUpperTranslationThreshold
float getUpperTranslationThreshold() const
Definition: ia_kfpcs.h:93
pcl::Registration< PointSource, PointTarget, float >::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:81
pcl::registration::MatchingCandidates
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Definition: matching_candidate.h:87
pcl::registration::KFPCSInitialAlignment::getLambda
float getLambda() const
Definition: ia_kfpcs.h:127
pcl::registration::KFPCSInitialAlignment
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition: ia_kfpcs.h:54
pcl::Registration< PointSource, PointTarget, float >::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: registration.h:85
pcl::registration::KFPCSInitialAlignment::setLambda
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition: ia_kfpcs.h:120
pcl::registration::KFPCSInitialAlignment::lambda_
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition: ia_kfpcs.h:241
pcl::registration::KFPCSInitialAlignment::finalCompute
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition: ia_kfpcs.hpp:178
pcl::registration::KFPCSInitialAlignment::candidates_
MatchingCandidates candidates_
Container for resulting vector of registration candidates.
Definition: ia_kfpcs.h:245
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:415
pcl::registration::KFPCSInitialAlignment::indices_validation_
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition: ia_kfpcs.h:253
pcl::registration::KFPCSInitialAlignment::setLowerTranslationThreshold
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition: ia_kfpcs.h:103
pcl::registration::KFPCSInitialAlignment::validateTransformation
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:131
pcl::Registration< PointSource, PointTarget, float >::ConstPtr
shared_ptr< const Registration< PointSource, PointTarget, float > > ConstPtr
Definition: registration.h:71
pcl::registration::MatchingCandidate
Container for matching candidate consisting of.
Definition: matching_candidate.h:55
pcl::PointCloud< PointSource >::iterator
typename VectorType::iterator iterator
Definition: point_cloud.h:427
pcl::registration::KFPCSInitialAlignment::handleMatches
void handleMatches(const std::vector< int > &base_indices, std::vector< std::vector< int > > &matches, MatchingCandidates &candidates) override
Method to handle current candidate matches.
Definition: ia_kfpcs.hpp:97
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90