Point Cloud Library (PCL) 1.14.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 std::size_t nr_indices = indices_->size();
94 if (nr_indices < static_cast<std::size_t>(ransac_iterations_))
95 indices_validation_ = indices_;
96 else
97 for (int i = 0; i < ransac_iterations_; i++)
98 indices_validation_->push_back((*indices_)[rand() % nr_indices]);
99
100 return (true);
101}
102
103template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
104void
106 const pcl::Indices& base_indices,
107 std::vector<pcl::Indices>& matches,
108 MatchingCandidates& candidates)
109{
110 candidates.clear();
111
112 // loop over all Candidate matches
113 for (auto& match : matches) {
114 Eigen::Matrix4f transformation_temp;
115 pcl::Correspondences correspondences_temp;
116 float fitness_score =
117 std::numeric_limits<float>::max(); // reset to std::numeric_limits<float>::max()
118 // to accept all candidates and not only best
119
120 // determine corresondences between base and match according to their distance to
121 // centroid
122 linkMatchWithBase(base_indices, match, correspondences_temp);
123
124 // check match based on residuals of the corresponding points after transformation
125 if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
126 0)
127 continue;
128
129 // check resulting transformation using a sub sample of the source point cloud
130 // all candidates are stored and later sorted according to their fitness score
131 validateTransformation(transformation_temp, fitness_score);
132
133 // store all valid match as well as associated score and transformation
134 candidates.push_back(
135 MatchingCandidate(fitness_score, correspondences_temp, transformation_temp));
136 }
137}
138
139template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
140int
142 validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
143{
144 // transform sub sampled source cloud
145 PointCloudSource source_transformed;
147 *input_, *indices_validation_, source_transformed, transformation);
148
149 const std::size_t nr_points = source_transformed.size();
150 float score_a = 0.f, score_b = 0.f;
151
152 // residual costs based on mse
153 pcl::Indices ids;
154 std::vector<float> dists_sqr;
155 for (const auto& source : source_transformed) {
156 // search for nearest point using kd tree search
157 tree_->nearestKSearch(source, 1, ids, dists_sqr);
158 score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
159 : max_inlier_dist_sqr_); // MSAC
160 }
161
162 score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC
163 // score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative
164 // to estimated overlap
165
166 // translation score (solutions with small translation are down-voted)
167 float scale = 1.f;
168 if (use_trl_score_) {
169 float trl = transformation.rightCols<1>().head(3).norm();
170 float trl_ratio =
171 (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
172
173 score_b =
174 (trl_ratio < 0.f ? 1.f
175 : (trl_ratio > 1.f ? 0.f
176 : 0.5f * sin(M_PI * trl_ratio + M_PI_2) +
177 0.5f)); // sinusoidal costs
178 scale += lambda_;
179 }
180
181 // calculate the fitness and return unsuccessful if smaller than previous ones
182 float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
183 if (fitness_score_temp > fitness_score)
184 return (-1);
185
186 fitness_score = fitness_score_temp;
187 return (0);
188}
189
190template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
191void
193 const std::vector<MatchingCandidates>& candidates)
194{
195 // reorganize candidates into single vector
196 std::size_t total_size = 0;
197 for (const auto& candidate : candidates)
198 total_size += candidate.size();
199
200 candidates_.clear();
201 candidates_.reserve(total_size);
202
203 for (const auto& candidate : candidates)
204 for (const auto& match : candidate)
205 candidates_.push_back(match);
206
207 // sort according to score value
208 std::sort(candidates_.begin(), candidates_.end(), by_score());
209
210 // return here if no score was valid, i.e. all scores are
211 // std::numeric_limits<float>::max()
212 if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
213 converged_ = false;
214 return;
215 }
216
217 // save best candidate as output result
218 // note, all other candidates are accessible via getNBestCandidates () and
219 // getTBestCandidates ()
220 fitness_score_ = candidates_[0].fitness_score;
221 final_transformation_ = candidates_[0].transformation;
222 *correspondences_ = candidates_[0].correspondences;
223
224 // here we define convergence if resulting score is above threshold
225 converged_ = fitness_score_ < score_threshold_;
226}
227
228template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
229void
231 int n, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
232{
233 candidates.clear();
234
235 // loop over all candidates starting from the best one
236 for (const auto& candidate : candidates_) {
237 // stop if current candidate has no valid score
238 if (candidate.fitness_score == std::numeric_limits<float>::max())
239 return;
240
241 // check if current candidate is a unique one compared to previous using the
242 // min_diff threshold
243 bool unique = true;
244 for (const auto& c2 : candidates) {
245 Eigen::Matrix4f diff =
246 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
247 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
248 const float translation3d = diff.block<3, 1>(0, 3).norm();
249 unique = angle3d > min_angle3d && translation3d > min_translation3d;
250 if (!unique) {
251 break;
252 }
253 }
254
255 // add candidate to best candidates
256 if (unique)
257 candidates.push_back(candidate);
258
259 // stop if n candidates are reached
260 if (candidates.size() == n)
261 return;
262 }
263}
264
265template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
266void
268 float t, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
269{
270 candidates.clear();
271
272 // loop over all candidates starting from the best one
273 for (const auto& candidate : candidates_) {
274 // stop if current candidate has score below threshold
275 if (candidate.fitness_score > t)
276 return;
277
278 // check if current candidate is a unique one compared to previous using the
279 // min_diff threshold
280 bool unique = true;
281 for (const auto& c2 : candidates) {
282 Eigen::Matrix4f diff =
283 candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
284 const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
285 const float translation3d = diff.block<3, 1>(0, 3).norm();
286 unique = angle3d > min_angle3d && translation3d > min_translation3d;
287 if (!unique) {
288 break;
289 }
290 }
291
292 // add candidate to best candidates
293 if (unique)
294 candidates.push_back(candidate);
295 }
296}
297
298} // namespace registration
299} // namespace pcl
300
301#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:214
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:267
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition ia_kfpcs.hpp:192
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:105
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:230
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:142
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:202
#define M_PI
Definition pcl_macros.h:201
Container for matching candidate consisting of.
Sorting of candidates based on fitness score value.