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