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