302 lines
11 KiB
C++
302 lines
11 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2014-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
*/
|
|
|
|
#ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
|
|
#define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
|
|
|
|
#include <limits>
|
|
|
|
namespace pcl {
|
|
|
|
namespace registration {
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
|
|
KFPCSInitialAlignment()
|
|
: indices_validation_(new pcl::Indices)
|
|
{
|
|
reg_name_ = "pcl::registration::KFPCSInitialAlignment";
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
bool
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::initCompute()
|
|
{
|
|
// due to sparse keypoint cloud, do not normalize delta with estimated point density
|
|
if (normalize_delta_) {
|
|
PCL_WARN("[%s::initCompute] Delta should be set according to keypoint precision! "
|
|
"Normalization according to point cloud density is ignored.\n",
|
|
reg_name_.c_str());
|
|
normalize_delta_ = false;
|
|
}
|
|
|
|
// initialize as in fpcs
|
|
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
|
|
initCompute();
|
|
|
|
// set the threshold values with respect to keypoint characteristics
|
|
max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
|
|
coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
|
|
max_edge_diff_ =
|
|
delta_ *
|
|
3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
|
|
max_mse_ =
|
|
powf(delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
|
|
max_inlier_dist_sqr_ =
|
|
powf(delta_ * 8.f,
|
|
2.f); // set rel. high, because MSAC is used (residual based score function)
|
|
|
|
// check use of translation costs and calculate upper boundary if not set by user
|
|
if (upper_trl_boundary_ < 0)
|
|
upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
|
|
|
|
if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
|
|
use_trl_score_ = true;
|
|
else
|
|
lambda_ = 0.f;
|
|
|
|
// generate a subset of indices of size ransac_iterations_ on which to evaluate
|
|
// candidates on
|
|
std::size_t nr_indices = indices_->size();
|
|
if (nr_indices < static_cast<std::size_t>(ransac_iterations_))
|
|
indices_validation_ = indices_;
|
|
else
|
|
for (int i = 0; i < ransac_iterations_; i++)
|
|
indices_validation_->push_back((*indices_)[rand() % nr_indices]);
|
|
|
|
return (true);
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
void
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::handleMatches(
|
|
const pcl::Indices& base_indices,
|
|
std::vector<pcl::Indices>& matches,
|
|
MatchingCandidates& candidates)
|
|
{
|
|
candidates.clear();
|
|
|
|
// loop over all Candidate matches
|
|
for (auto& match : matches) {
|
|
Eigen::Matrix4f transformation_temp;
|
|
pcl::Correspondences correspondences_temp;
|
|
float fitness_score =
|
|
std::numeric_limits<float>::max(); // reset to std::numeric_limits<float>::max()
|
|
// to accept all candidates and not only best
|
|
|
|
// determine corresondences between base and match according to their distance to
|
|
// centroid
|
|
linkMatchWithBase(base_indices, match, correspondences_temp);
|
|
|
|
// check match based on residuals of the corresponding points after transformation
|
|
if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
|
|
0)
|
|
continue;
|
|
|
|
// check resulting transformation using a sub sample of the source point cloud
|
|
// all candidates are stored and later sorted according to their fitness score
|
|
validateTransformation(transformation_temp, fitness_score);
|
|
|
|
// store all valid match as well as associated score and transformation
|
|
candidates.push_back(
|
|
MatchingCandidate(fitness_score, correspondences_temp, transformation_temp));
|
|
}
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
int
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
|
|
validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
|
|
{
|
|
// transform sub sampled source cloud
|
|
PointCloudSource source_transformed;
|
|
pcl::transformPointCloud(
|
|
*input_, *indices_validation_, source_transformed, transformation);
|
|
|
|
const std::size_t nr_points = source_transformed.size();
|
|
float score_a = 0.f, score_b = 0.f;
|
|
|
|
// residual costs based on mse
|
|
pcl::Indices ids;
|
|
std::vector<float> dists_sqr;
|
|
for (const auto& source : source_transformed) {
|
|
// search for nearest point using kd tree search
|
|
tree_->nearestKSearch(source, 1, ids, dists_sqr);
|
|
score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0]
|
|
: max_inlier_dist_sqr_); // MSAC
|
|
}
|
|
|
|
score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC
|
|
// score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative
|
|
// to estimated overlap
|
|
|
|
// translation score (solutions with small translation are down-voted)
|
|
float scale = 1.f;
|
|
if (use_trl_score_) {
|
|
float trl = transformation.rightCols<1>().head(3).norm();
|
|
float trl_ratio =
|
|
(trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
|
|
|
|
score_b =
|
|
(trl_ratio < 0.f ? 1.f
|
|
: (trl_ratio > 1.f ? 0.f
|
|
: 0.5f * sin(M_PI * trl_ratio + M_PI_2) +
|
|
0.5f)); // sinusoidal costs
|
|
scale += lambda_;
|
|
}
|
|
|
|
// calculate the fitness and return unsuccessful if smaller than previous ones
|
|
float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
|
|
if (fitness_score_temp > fitness_score)
|
|
return (-1);
|
|
|
|
fitness_score = fitness_score_temp;
|
|
return (0);
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
void
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::finalCompute(
|
|
const std::vector<MatchingCandidates>& candidates)
|
|
{
|
|
// reorganize candidates into single vector
|
|
std::size_t total_size = 0;
|
|
for (const auto& candidate : candidates)
|
|
total_size += candidate.size();
|
|
|
|
candidates_.clear();
|
|
candidates_.reserve(total_size);
|
|
|
|
for (const auto& candidate : candidates)
|
|
for (const auto& match : candidate)
|
|
candidates_.push_back(match);
|
|
|
|
// sort according to score value
|
|
std::sort(candidates_.begin(), candidates_.end(), by_score());
|
|
|
|
// return here if no score was valid, i.e. all scores are
|
|
// std::numeric_limits<float>::max()
|
|
if (candidates_[0].fitness_score == std::numeric_limits<float>::max()) {
|
|
converged_ = false;
|
|
return;
|
|
}
|
|
|
|
// save best candidate as output result
|
|
// note, all other candidates are accessible via getNBestCandidates () and
|
|
// getTBestCandidates ()
|
|
fitness_score_ = candidates_[0].fitness_score;
|
|
final_transformation_ = candidates_[0].transformation;
|
|
*correspondences_ = candidates_[0].correspondences;
|
|
|
|
// here we define convergence if resulting score is above threshold
|
|
converged_ = fitness_score_ < score_threshold_;
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
void
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates(
|
|
int n, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
|
|
{
|
|
candidates.clear();
|
|
|
|
// loop over all candidates starting from the best one
|
|
for (const auto& candidate : candidates_) {
|
|
// stop if current candidate has no valid score
|
|
if (candidate.fitness_score == std::numeric_limits<float>::max())
|
|
return;
|
|
|
|
// check if current candidate is a unique one compared to previous using the
|
|
// min_diff threshold
|
|
bool unique = true;
|
|
for (const auto& c2 : candidates) {
|
|
Eigen::Matrix4f diff =
|
|
candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
|
|
const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
|
|
const float translation3d = diff.block<3, 1>(0, 3).norm();
|
|
unique = angle3d > min_angle3d && translation3d > min_translation3d;
|
|
if (!unique) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// add candidate to best candidates
|
|
if (unique)
|
|
candidates.push_back(candidate);
|
|
|
|
// stop if n candidates are reached
|
|
if (candidates.size() == n)
|
|
return;
|
|
}
|
|
}
|
|
|
|
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
|
|
void
|
|
KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates(
|
|
float t, float min_angle3d, float min_translation3d, MatchingCandidates& candidates)
|
|
{
|
|
candidates.clear();
|
|
|
|
// loop over all candidates starting from the best one
|
|
for (const auto& candidate : candidates_) {
|
|
// stop if current candidate has score below threshold
|
|
if (candidate.fitness_score > t)
|
|
return;
|
|
|
|
// check if current candidate is a unique one compared to previous using the
|
|
// min_diff threshold
|
|
bool unique = true;
|
|
for (const auto& c2 : candidates) {
|
|
Eigen::Matrix4f diff =
|
|
candidate.transformation.colPivHouseholderQr().solve(c2.transformation);
|
|
const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle();
|
|
const float translation3d = diff.block<3, 1>(0, 3).norm();
|
|
unique = angle3d > min_angle3d && translation3d > min_translation3d;
|
|
if (!unique) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// add candidate to best candidates
|
|
if (unique)
|
|
candidates.push_back(candidate);
|
|
}
|
|
}
|
|
|
|
} // namespace registration
|
|
} // namespace pcl
|
|
|
|
#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
|