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