Point Cloud Library (PCL)  1.8.0
ia_kfpcs.hpp
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_IMPL_IA_KFPCS_H_
39 #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
40 
41 ///////////////////////////////////////////////////////////////////////////////////////////
42 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
44  lower_trl_boundary_ (-1.f),
45  upper_trl_boundary_ (-1.f),
46  lambda_ (0.5f),
47  candidates_ (),
48  use_trl_score_ (false),
49  indices_validation_ (new std::vector <int>)
50 {
51  reg_name_ = "pcl::registration::KFPCSInitialAlignment";
52 }
53 
54 
55 ///////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
58 {
59  // due to sparse keypoint cloud, do not normalize delta with estimated point density
60  if (normalize_delta_)
61  {
62  PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
63  normalize_delta_ = false;
64  }
65 
66  // initialize as in fpcs
68 
69  // set the threshold values with respect to keypoint charactersitics
70  max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
71  coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
72  max_edge_diff_ = delta_ * 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
73  max_mse_ = powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
74  max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function)
75 
76  // check use of translation costs and calculate upper boundary if not set by user
77  if (upper_trl_boundary_ < 0)
78  upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
79 
80  if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
81  use_trl_score_ = true;
82  else
83  lambda_ = 0.f;
84 
85  // generate a subset of indices of size ransac_iterations_ on which to evaluate candidates on
86  std::size_t nr_indices = indices_->size ();
87  if (nr_indices < ransac_iterations_)
88  indices_validation_ = indices_;
89  else
90  for (int i = 0; i < ransac_iterations_; i++)
91  indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
92 
93  return (true);
94 }
95 
96 
97 ///////////////////////////////////////////////////////////////////////////////////////////
98 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
100  const std::vector <int> &base_indices,
101  std::vector <std::vector <int> > &matches,
102  MatchingCandidates &candidates)
103 {
104  candidates.clear ();
105  float fitness_score = FLT_MAX;
106 
107  // loop over all Candidate matches
108  for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
109  {
110  Eigen::Matrix4f transformation_temp;
111  pcl::Correspondences correspondences_temp;
112  fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best
113 
114  // determine corresondences between base and match according to their distance to centroid
115  linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
116 
117  // check match based on residuals of the corresponding points after transformation
118  if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
119  continue;
120 
121  // check resulting transformation using a sub sample of the source point cloud
122  // all candidates are stored and later sorted according to their fitness score
123  validateTransformation (transformation_temp, fitness_score);
124 
125  // store all valid match as well as associated score and transformation
126  candidates.push_back (MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
127  }
128 }
129 
130 
131 ///////////////////////////////////////////////////////////////////////////////////////////
132 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
134  Eigen::Matrix4f &transformation,
135  float &fitness_score)
136 {
137  // transform sub sampled source cloud
138  PointCloudSource source_transformed;
139  pcl::transformPointCloud (*input_, *indices_validation_, source_transformed, transformation);
140 
141  const std::size_t nr_points = source_transformed.size ();
142  float score_a = 0.f, score_b = 0.f;
143 
144  // residual costs based on mse
145  std::vector <int> ids;
146  std::vector <float> dists_sqr;
147  for (PointCloudSourceIterator it = source_transformed.begin (), it_e = source_transformed.end (); it != it_e; ++it)
148  {
149  // search for nearest point using kd tree search
150  tree_->nearestKSearch (*it, 1, ids, dists_sqr);
151  score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_); // MSAC
152  }
153 
154  score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC
155  //score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative to estimated overlap
156 
157  // translation score (solutions with small translation are down-voted)
158  float scale = 1.f;
159  if (use_trl_score_)
160  {
161  float trl = transformation.rightCols <1> ().head (3).norm ();
162  float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
163 
164  score_b = (trl_ratio < 0.f ? 1.f : (trl_ratio > 1.f ? 0.f : 0.5f * sin (M_PI * trl_ratio + M_PI_2) + 0.5f)); // sinusoidal costs
165  scale += lambda_;
166  }
167 
168  // calculate the fitness and return unsuccessfull if smaller than previous ones
169  float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
170  if (fitness_score_temp > fitness_score)
171  return (-1);
172 
173  fitness_score = fitness_score_temp;
174  return (0);
175 }
176 
177 
178 ///////////////////////////////////////////////////////////////////////////////////////////
179 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
181  const std::vector <MatchingCandidates > &candidates)
182 {
183  // reorganize candidates into single vector
184  size_t total_size = 0;
185  std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
186  for (it = candidates.begin (); it != it_e; it++)
187  total_size += it->size ();
188 
189  candidates_.clear ();
190  candidates_.reserve (total_size);
191 
192  MatchingCandidates::const_iterator it_curr, it_curr_e;
193  for (it = candidates.begin (); it != it_e; it++)
194  for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
195  candidates_.push_back (*it_curr);
196 
197  // sort acoording to score value
198  std::sort (candidates_.begin (), candidates_.end (), by_score ());
199 
200  // return here if no score was valid, i.e. all scores are FLT_MAX
201  if (candidates_[0].fitness_score == FLT_MAX)
202  {
203  converged_ = false;
204  return;
205  }
206 
207  // save best candidate as output result
208  // note, all other candidates are accessible via getNBestCandidates () and getTBestCandidates ()
209  fitness_score_ = candidates_ [0].fitness_score;
210  final_transformation_ = candidates_ [0].transformation;
211  *correspondences_ = candidates_ [0].correspondences;
212 
213  // here we define convergence if resulting score is above threshold
214  converged_ = fitness_score_ < score_threshold_;
215 }
216 
217 ///////////////////////////////////////////////////////////////////////////////////////////
218 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
220  int n,
221  float min_angle3d,
222  float min_translation3d,
223  MatchingCandidates &candidates)
224 {
225  candidates.clear ();
226 
227  // loop over all candidates starting from the best one
228  for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
229  {
230  // stop if current candidate has no valid score
231  if (it_candidate->fitness_score == FLT_MAX)
232  return;
233 
234  // check if current candidate is a unique one compared to previous using the min_diff threshold
235  bool unique = true;
236  MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
237  while (unique && it != it_e2)
238  {
239  Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
240  const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
241  const float translation3d = diff.block <3, 1> (0, 3).norm ();
242  unique = angle3d > min_angle3d && translation3d > min_translation3d;
243  it++;
244  }
245 
246  // add candidate to best candidates
247  if (unique)
248  candidates.push_back (*it_candidate);
249 
250  // stop if n candidates are reached
251  if (candidates.size () == n)
252  return;
253  }
254 }
255 
256 ///////////////////////////////////////////////////////////////////////////////////////////
257 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
259  float t,
260  float min_angle3d,
261  float min_translation3d,
262  MatchingCandidates &candidates)
263 {
264  candidates.clear ();
265 
266  // loop over all candidates starting from the best one
267  for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
268  {
269  // stop if current candidate has score below threshold
270  if (it_candidate->fitness_score > t)
271  return;
272 
273  // check if current candidate is a unique one compared to previous using the min_diff threshold
274  bool unique = true;
275  MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
276  while (unique && it != it_e2)
277  {
278  Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
279  const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
280  const float translation3d = diff.block <3, 1> (0, 3).norm ();
281  unique = angle3d > min_angle3d && translation3d > min_translation3d;
282  it++;
283  }
284 
285  // add candidate to best candidates
286  if (unique)
287  candidates.push_back (*it_candidate);
288  }
289 }
290 
291 ///////////////////////////////////////////////////////////////////////////////////////////
292 
293 #endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Sorting of candidates based on fitness score value.
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:78
iterator end()
Definition: point_cloud.h:435
Container for matching candidate consisting of.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
iterator begin()
Definition: point_cloud.h:434
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition: ia_kfpcs.h:56
size_t size() const
Definition: point_cloud.h:440