Point Cloud Library (PCL) 1.13.1
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: lower_trl_boundary_(-1.f)
50, upper_trl_boundary_(-1.f)
51, lambda_(0.5f)
52, use_trl_score_(false)
53, indices_validation_(new pcl::Indices)
54{
55 reg_name_ = "pcl::registration::KFPCSInitialAlignment";
56}
57
58template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
59bool
61{
62 // due to sparse keypoint cloud, do not normalize delta with estimated point density
63 if (normalize_delta_) {
64 PCL_WARN("[%s::initCompute] Delta should be set according to keypoint precision! "
65 "Normalization according to point cloud density is ignored.\n",
66 reg_name_.c_str());
67 normalize_delta_ = false;
68 }
69
70 // initialize as in fpcs
73
74 // set the threshold values with respect to keypoint characteristics
75 max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
76 coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
77 max_edge_diff_ =
78 delta_ *
79 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
80 max_mse_ =
81 powf(delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
82 max_inlier_dist_sqr_ =
83 powf(delta_ * 8.f,
84 2.f); // set rel. high, because MSAC is used (residual based score function)
85
86 // check use of translation costs and calculate upper boundary if not set by user
87 if (upper_trl_boundary_ < 0)
88 upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
89
90 if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
91 use_trl_score_ = true;
92 else
93 lambda_ = 0.f;
94
95 // generate a subset of indices of size ransac_iterations_ on which to evaluate
96 // candidates on
97 std::size_t nr_indices = indices_->size();
98 if (nr_indices < static_cast<std::size_t>(ransac_iterations_))
99 indices_validation_ = indices_;
100 else
101 for (int i = 0; i < ransac_iterations_; i++)
102 indices_validation_->push_back((*indices_)[rand() % nr_indices]);
103
104 return (true);
105}
106
107template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
108void
110 const pcl::Indices& base_indices,
111 std::vector<pcl::Indices>& matches,
112 MatchingCandidates& candidates)
113{
114 candidates.clear();
115
116 // loop over all Candidate matches
117 for (auto& match : matches) {
118 Eigen::Matrix4f transformation_temp;
119 pcl::Correspondences correspondences_temp;
120 float fitness_score =
121 std::numeric_limits<float>::max(); // reset to std::numeric_limits<float>::max()
122 // to accept all candidates and not only best
123
124 // determine corresondences between base and match according to their distance to
125 // centroid
126 linkMatchWithBase(base_indices, match, correspondences_temp);
127
128 // check match based on residuals of the corresponding points after transformation
129 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
130 0)
131 continue;
132
133 // check resulting transformation using a sub sample of the source point cloud
134 // all candidates are stored and later sorted according to their fitness score
135 validateTransformation(transformation_temp, fitness_score);
136
137 // store all valid match as well as associated score and transformation
138 candidates.push_back(
139 MatchingCandidate(fitness_score, correspondences_temp, transformation_temp));
140 }
141}
142
143template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
144int
146 validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
147{
148 // transform sub sampled source cloud
149 PointCloudSource source_transformed;
151 *input_, *indices_validation_, source_transformed, transformation);
152
153 const std::size_t nr_points = source_transformed.size();
154 float score_a = 0.f, score_b = 0.f;
155
156 // residual costs based on mse
157 pcl::Indices ids;
158 std::vector<float> dists_sqr;
159 for (const auto& source : source_transformed) {
160 // search for nearest point using kd tree search
161 tree_->nearestKSearch(source, 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
194template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
195void
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
215 // std::numeric_limits<float>::max()
216 if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
217 converged_ = false;
218 return;
219 }
220
221 // save best candidate as output result
222 // note, all other candidates are accessible via getNBestCandidates () and
223 // getTBestCandidates ()
224 fitness_score_ = candidates_[0].fitness_score;
225 final_transformation_ = candidates_[0].transformation;
226 *correspondences_ = candidates_[0].correspondences;
227
228 // here we define convergence if resulting score is above threshold
229 converged_ = fitness_score_ < score_threshold_;
230}
231
232template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
233void
235 int n, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
236{
237 candidates.clear();
238
239 // loop over all candidates starting from the best one
240 for (const auto& candidate : candidates_) {
241 // stop if current candidate has no valid score
242 if (candidate.fitness_score == std::numeric_limits<float>::max())
243 return;
244
245 // check if current candidate is a unique one compared to previous using the
246 // min_diff threshold
247 bool unique = true;
248 for (const auto& c2 : candidates) {
249 Eigen::Matrix4f diff =
250 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
251 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
252 const float translation3d = diff.block<3, 1>(0, 3).norm();
253 unique = angle3d > min_angle3d && translation3d > min_translation3d;
254 if (!unique) {
255 break;
256 }
257 }
258
259 // add candidate to best candidates
260 if (unique)
261 candidates.push_back(candidate);
262
263 // stop if n candidates are reached
264 if (candidates.size() == n)
265 return;
266 }
267}
268
269template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
270void
272 float t, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
273{
274 candidates.clear();
275
276 // loop over all candidates starting from the best one
277 for (const auto& candidate : candidates_) {
278 // stop if current candidate has score below threshold
279 if (candidate.fitness_score > t)
280 return;
281
282 // check if current candidate is a unique one compared to previous using the
283 // min_diff threshold
284 bool unique = true;
285 for (const auto& c2 : candidates) {
286 Eigen::Matrix4f diff =
287 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
288 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
289 const float translation3d = diff.block<3, 1>(0, 3).norm();
290 unique = angle3d > min_angle3d && translation3d > min_translation3d;
291 if (!unique) {
292 break;
293 }
294 }
295
296 // add candidate to best candidates
297 if (unique)
298 candidates.push_back(candidate);
299 }
300}
301
302} // namespace registration
303} // namespace pcl
304
305#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
std::size_t size() const
std::string reg_name_
The registration method name.
virtual bool initCompute()
Internal computation initialization.
Definition ia_fpcs.hpp:242
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:271
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition ia_kfpcs.hpp:196
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:109
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:234
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:146
bool initCompute() override
Internal computation initialization.
Definition ia_kfpcs.hpp:60
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:202
#define M_PI
Definition pcl_macros.h:201
Container for matching candidate consisting of.
Sorting of candidates based on fitness score value.