ia_kfpcs.hpp
/*
* 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_
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
lower_trl_boundary_ (-1.f),
upper_trl_boundary_ (-1.f),
lambda_ (0.5f),
use_trl_score_ (false),
indices_validation_ (new std::vector <int>)
{
reg_name_ = "pcl::registration::KFPCSInitialAlignment";
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
pcl::registration::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.", 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 charactersitics
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 < 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
pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
const std::vector <int> &base_indices,
std::vector <std::vector <int> > &matches,
MatchingCandidates &candidates)
{
candidates.clear ();
float fitness_score = FLT_MAX;
// loop over all Candidate matches
for (auto &match : matches)
{
Eigen::Matrix4f transformation_temp;
pcl::Correspondences correspondences_temp;
fitness_score = FLT_MAX; // reset to FLT_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
pcl::registration::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
std::vector <int> ids;
std::vector <float> dists_sqr;
for (PointCloudSourceIterator it = source_transformed.begin (), it_e = source_transformed.end (); it != it_e; ++it)
{
// search for nearest point using kd tree search
tree_->nearestKSearch (*it, 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
pcl::registration::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 FLT_MAX
if (candidates_[0].fitness_score == FLT_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
pcl::registration::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 (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
{
// stop if current candidate has no valid score
if (it_candidate->fitness_score == FLT_MAX)
return;
// check if current candidate is a unique one compared to previous using the min_diff threshold
bool unique = true;
MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
while (unique && it != it_e2)
{
Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->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;
it++;
}
// add candidate to best candidates
if (unique)
candidates.push_back (*it_candidate);
// stop if n candidates are reached
if (candidates.size () == n)
return;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
pcl::registration::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 (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
{
// stop if current candidate has score below threshold
if (it_candidate->fitness_score > t)
return;
// check if current candidate is a unique one compared to previous using the min_diff threshold
bool unique = true;
MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
while (unique && it != it_e2)
{
Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->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;
it++;
}
// add candidate to best candidates
if (unique)
candidates.push_back (*it_candidate);
}
}
///////////////////////////////////////////////////////////////////////////////////////////
#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
pcl::PointCloud::end
iterator end()
Definition: point_cloud.h:430
pcl::registration::FPCSInitialAlignment
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:76
pcl::registration::by_score
Sorting of candidates based on fitness score value.
Definition: matching_candidate.h:90
pcl::PointCloud< PointSource >
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
pcl::registration::MatchingCandidates
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Definition: matching_candidate.h:87
pcl::registration::KFPCSInitialAlignment
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition: ia_kfpcs.h:54
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:435
pcl::PointCloud::begin
iterator begin()
Definition: point_cloud.h:429
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88
pcl::registration::MatchingCandidate
Container for matching candidate consisting of.
Definition: matching_candidate.h:55