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