Point Cloud Library (PCL) 1.12.1
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
40namespace pcl {
41
42namespace registration {
43
44template <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
56template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
57bool
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
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
105template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
106void
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
140template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
141int
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
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 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
231template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
232void
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
270template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
271void
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_
iterator end() noexcept
Definition: point_cloud.h:430
std::size_t size() const
Definition: point_cloud.h:443
iterator begin() noexcept
Definition: point_cloud.h:429
std::string reg_name_
The registration method name.
Definition: registration.h:558
virtual bool initCompute()
Internal computation initialization.
Definition: ia_fpcs.hpp:240
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
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:107
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
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
bool initCompute() override
Internal computation initialization.
Definition: ia_kfpcs.hpp:58
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
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.