Point Cloud Library (PCL)  1.8.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  * * The use for research only (no for any commercial application).
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_REGISTRATION_IA_KFPCS_H_
39 #define PCL_REGISTRATION_IA_KFPCS_H_
40 
41 #include <pcl/registration/ia_fpcs.h>
42 
43 namespace pcl
44 {
45  namespace registration
46  {
47  /** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints
48  * as described in: "Markerless point cloud registration with keypoint-based 4-points congruent sets",
49  * Pascal Theiler, Jan Dirk Wegner, Konrad Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop
50  * Laser Scanning, Antalya, Turkey, 2013.
51  * \note Method has since been improved and some variations to the paper exist.
52  * \author P.W.Theiler
53  * \ingroup registration
54  */
55  template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
56  class KFPCSInitialAlignment : public virtual FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>
57  {
58  public:
59  /** \cond */
60  typedef boost::shared_ptr <KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > Ptr;
61  typedef boost::shared_ptr <const KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > ConstPtr;
62 
65  typedef typename PointCloudSource::iterator PointCloudSourceIterator;
66 
69  typedef typename PointCloudTarget::iterator PointCloudTargetIterator;
70 
72  typedef std::vector <MatchingCandidate> MatchingCandidates;
73  /** \endcond */
74 
75 
76  /** \brief Constructor. */
78 
79  /** \brief Destructor. */
81  {};
82 
83 
84  /** \brief Set the upper translation threshold used for score evaluation.
85  * \param[in] upper_trl_boundary upper translation threshold
86  */
87  inline void
88  setUpperTranslationThreshold (float upper_trl_boundary)
89  {
90  upper_trl_boundary_ = upper_trl_boundary;
91  };
92 
93  /** \return the upper translation threshold used for score evaluation. */
94  inline float
96  {
97  return (upper_trl_boundary_);
98  };
99 
100 
101  /** \brief Set the lower translation threshold used for score evaluation.
102  * \param[in] lower_trl_boundary lower translation threshold
103  */
104  inline void
105  setLowerTranslationThreshold (float lower_trl_boundary)
106  {
107  lower_trl_boundary_ = lower_trl_boundary;
108  };
109 
110  /** \return the lower translation threshold used for score evaluation. */
111  inline float
113  {
114  return (lower_trl_boundary_);
115  };
116 
117 
118  /** \brief Set the weighting factor of the translation cost term.
119  * \param[in] lambda the weighting factor of the translation cost term
120  */
121  inline void
122  setLambda (float lambda)
123  {
124  lambda_ = lambda;
125  };
126 
127  /** \return the weighting factor of the translation cost term. */
128  inline float
129  getLambda () const
130  {
131  return (lambda_);
132  };
133 
134 
135  /** \brief Get the N best unique candidate matches according to their fitness score.
136  * The method only returns unique transformations comparing the translation
137  * and the 3D rotation to already returned transformations.
138  *
139  * \note The method may return less than N candidates, if the number of unique candidates
140  * is smaller than N
141  *
142  * \param[in] n number of best candidates to return
143  * \param[in] min_angle3d minimum 3D angle difference in radian
144  * \param[in] min_translation3d minimum 3D translation difference
145  * \param[out] candidates vector of unique candidates
146  */
147  void
148  getNBestCandidates (int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
149 
150  /** \brief Get all unique candidate matches with fitness scores above a threshold t.
151  * The method only returns unique transformations comparing the translation
152  * and the 3D rotation to already returned transformations.
153  *
154  * \param[in] t fitness score threshold
155  * \param[in] min_angle3d minimum 3D angle difference in radian
156  * \param[in] min_translation3d minimum 3D translation difference
157  * \param[out] candidates vector of unique candidates
158  */
159  void
160  getTBestCandidates (float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
161 
162 
163  protected:
164 
168 
175 
189 
190 
191  /** \brief Internal computation initialization. */
192  virtual bool
193  initCompute ();
194 
195  /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
196  * base and store the sorted matches (together with score values and estimated transformations).
197  *
198  * \param[in] base_indices indices of base B
199  * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are
200  * reordered during this step.
201  * \param[out] candidates vector which contains the candidates matches M
202  */
203  virtual void
204  handleMatches (
205  const std::vector <int> &base_indices,
206  std::vector <std::vector <int> > &matches,
207  MatchingCandidates &candidates);
208 
209  /** \brief Validate the transformation by calculating the score value after transforming the input source cloud.
210  * The resulting score is later used as the decision criteria of the best fitting match.
211  *
212  * \param[out] transformation updated orientation matrix using all inliers
213  * \param[out] fitness_score current best score
214  * \note fitness score is only updated if the score of the current transformation exceeds the input one.
215  * \return
216  * * < 0 if previous result is better than the current one (score remains)
217  * * = 0 current result is better than the previous one (score updated)
218  */
219  virtual int
220  validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score);
221 
222  /** \brief Final computation of best match out of vector of matches. To avoid cross thread dependencies
223  * during parallel running, a best match for each try was calculated.
224  * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
225  * \param[in] candidates vector of candidate matches
226  */
227  virtual void
228  finalCompute (const std::vector <MatchingCandidates > &candidates);
229 
230 
231  /** \brief Lower boundary for translation costs calculation.
232  * \note If not set by the user, the translation costs are not used during evaluation.
233  */
235 
236  /** \brief Upper boundary for translation costs calculation.
237  * \note If not set by the user, it is calculated from the estimated overlap and the diameter
238  * of the point cloud.
239  */
241 
242  /** \brief Weighting factor for translation costs (standard = 0.5). */
243  float lambda_;
244 
245 
246  /** \brief Container for resulting vector of registration candidates. */
247  MatchingCandidates candidates_;
248 
249  /** \brief Flag if translation score should be used in validation (internal calculation). */
251 
252  /** \brief Subset of input indices on which we evaluate candidates.
253  * To speed up the evaluation, we only use a fix number of indices defined during initialization.
254  */
256 
257  };
258  }; // namespace registration
259 }; // namespace pcl
260 
261 #include <pcl/registration/impl/ia_kfpcs.hpp>
262 
263 #endif // PCL_REGISTRATION_IA_KFPCS_H_
virtual bool initCompute()
Internal computation initialization.
Definition: ia_kfpcs.hpp:57
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition: ia_kfpcs.h:234
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:78
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
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition: ia_kfpcs.h:243
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:219
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:258
VectorType::iterator iterator
Definition: point_cloud.h:432
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition: ia_kfpcs.h:105
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:180
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:99
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:133
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition: ia_kfpcs.h:255
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:247
bool use_trl_score_
Flag if translation score should be used in validation (internal calculation).
Definition: ia_kfpcs.h:250
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition: ia_kfpcs.h:122
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition: ia_kfpcs.h:88
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition: ia_kfpcs.h:240
pcl::PointCloud< PointSource > PointCloudSource
Definition: registration.h:82
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition: ia_kfpcs.h:56
virtual ~KFPCSInitialAlignment()
Destructor.
Definition: ia_kfpcs.h:80