Loading...
Searching...
No Matches
ia_kfpcs.h
/*
* 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.
*
*/
#pragma once
#include <pcl/registration/ia_fpcs.h>
namespace pcl {
namespace registration {
/** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based
* on keypoints as described in: "Markerless point cloud registration with
* keypoint-based 4-points congruent sets", Pascal Theiler, Jan Dirk Wegner, Konrad
* Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop Laser Scanning,
* Antalya, Turkey, 2013.
* \note Method has since been improved and some variations to the paper exist.
*
* The main differences to FPCSInitialAlignment are:
* <ol>
* <li> KFPCSInitialAlignment stores all solution candidates instead of only the best
* one
* <li> KFPCSInitialAlignment uses an MSAC approach to score candidates instead of
* counting inliers
* </ol>
* \author P.W.Theiler
* \ingroup registration
*/
template <typename PointSource,
typename PointTarget,
typename NormalT = pcl::Normal,
typename Scalar = float>
class KFPCSInitialAlignment
: public virtual FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar> {
public:
/** \cond */
using Ptr =
shared_ptr<KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
using ConstPtr = shared_ptr<
const KFPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceIterator = typename PointCloudSource::iterator;
using PointCloudTarget = pcl::PointCloud<PointTarget>;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetIterator = typename PointCloudTarget::iterator;
using MatchingCandidate = pcl::registration::MatchingCandidate;
using MatchingCandidates = pcl::registration::MatchingCandidates;
/** \endcond */
/** \brief Constructor. */
KFPCSInitialAlignment();
/** \brief Destructor. */
~KFPCSInitialAlignment() override = default;
/** \brief Set the upper translation threshold used for score evaluation.
* \param[in] upper_trl_boundary upper translation threshold
*/
inline void
setUpperTranslationThreshold(float upper_trl_boundary)
{
upper_trl_boundary_ = upper_trl_boundary;
};
/** \return the upper translation threshold used for score evaluation. */
inline float
getUpperTranslationThreshold() const
{
return (upper_trl_boundary_);
};
/** \brief Set the lower translation threshold used for score evaluation.
* \param[in] lower_trl_boundary lower translation threshold
*/
inline void
setLowerTranslationThreshold(float lower_trl_boundary)
{
lower_trl_boundary_ = lower_trl_boundary;
};
/** \return the lower translation threshold used for score evaluation. */
inline float
getLowerTranslationThreshold() const
{
return (lower_trl_boundary_);
};
/** \brief Set the weighting factor of the translation cost term.
* \param[in] lambda the weighting factor of the translation cost term
*/
inline void
setLambda(float lambda)
{
lambda_ = lambda;
};
/** \return the weighting factor of the translation cost term. */
inline float
getLambda() const
{
return (lambda_);
};
/** \brief Get the N best unique candidate matches according to their fitness score.
* The method only returns unique transformations comparing the translation
* and the 3D rotation to already returned transformations.
*
* \note The method may return less than N candidates, if the number of unique
* candidates is smaller than N
*
* \param[in] n number of best candidates to return
* \param[in] min_angle3d minimum 3D angle difference in radian
* \param[in] min_translation3d minimum 3D translation difference
* \param[out] candidates vector of unique candidates
*/
void
getNBestCandidates(int n,
float min_angle3d,
float min_translation3d,
MatchingCandidates& candidates);
/** \brief Get all unique candidate matches with fitness scores above a threshold t.
* The method only returns unique transformations comparing the translation
* and the 3D rotation to already returned transformations.
*
* \param[in] t fitness score threshold
* \param[in] min_angle3d minimum 3D angle difference in radian
* \param[in] min_translation3d minimum 3D translation difference
* \param[out] candidates vector of unique candidates
*/
void
getTBestCandidates(float t,
float min_angle3d,
float min_translation3d,
MatchingCandidates& candidates);
protected:
using PCLBase<PointSource>::deinitCompute;
using PCLBase<PointSource>::input_;
using PCLBase<PointSource>::indices_;
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
using Registration<PointSource, PointTarget, Scalar>::tree_;
using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
using Registration<PointSource, PointTarget, Scalar>::correspondences_;
using Registration<PointSource, PointTarget, Scalar>::converged_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::delta_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
approx_overlap_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
coincidation_limit_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::max_mse_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
max_inlier_dist_sqr_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::diameter_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
normalize_delta_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
score_threshold_;
using FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::validateMatch;
/** \brief Internal computation initialization. */
bool
initCompute() override;
/** \brief Method to handle current candidate matches. Here we validate and evaluate
* the matches w.r.t the base and store the sorted matches (together with score values
* and estimated transformations).
*
* \param[in] base_indices indices of base B
* \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
* matches are reordered during this step. \param[out] candidates vector which
* contains the candidates matches M
*/
void
handleMatches(const pcl::Indices& base_indices,
std::vector<pcl::Indices>& matches,
MatchingCandidates& candidates) override;
/** \brief Validate the transformation by calculating the score value after
* transforming the input source cloud. The resulting score is later used as the
* decision criteria of the best fitting match.
*
* \param[out] transformation updated orientation matrix using all inliers
* \param[out] fitness_score current best score
* \note fitness score is only updated if the score of the current transformation
* exceeds the input one. \return
* * < 0 if previous result is better than the current one (score remains)
* * = 0 current result is better than the previous one (score updated)
*/
int
validateTransformation(Eigen::Matrix4f& transformation,
float& fitness_score) override;
/** \brief Final computation of best match out of vector of matches. To avoid cross
* thread dependencies during parallel running, a best match for each try was
* calculated. \note For forwards compatibility the candidates are stored in vectors
* of 'vectors of size 1'. \param[in] candidates vector of candidate matches
*/
void
finalCompute(const std::vector<MatchingCandidates>& candidates) override;
/** \brief Lower boundary for translation costs calculation.
* \note If not set by the user, the translation costs are not used during evaluation.
*/
float lower_trl_boundary_{-1.f};
/** \brief Upper boundary for translation costs calculation.
* \note If not set by the user, it is calculated from the estimated overlap and the
* diameter of the point cloud.
*/
float upper_trl_boundary_{-1.f};
/** \brief Weighting factor for translation costs (standard = 0.5). */
float lambda_{0.5f};
/** \brief Container for resulting vector of registration candidates. */
MatchingCandidates candidates_;
/** \brief Flag if translation score should be used in validation (internal
* calculation). */
bool use_trl_score_{false};
/** \brief Subset of input indices on which we evaluate candidates.
* To speed up the evaluation, we only use a fix number of indices defined during
* initialization.
*/
pcl::IndicesPtr indices_validation_;
};
}; // namespace registration
}; // namespace pcl
#include <pcl/registration/impl/ia_kfpcs.hpp>
pcl::PCLBase
PCL base class.
Definition pcl_base.h:70
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::PCLBase< PointSource >::indices_
IndicesPtr indices_
Definition pcl_base.h:150
pcl::PCLBase< PointSource >::deinitCompute
bool deinitCompute()
Definition pcl_base.hpp:175
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::PointCloud< PointSource >::iterator
typename VectorType::iterator iterator
Definition point_cloud.h:426
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition point_cloud.h:414
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition registration.h:57
pcl::Registration< PointSource, PointTarget, float >::final_transformation_
Matrix4 final_transformation_
Definition registration.h:573
pcl::Registration< PointSource, PointTarget, float >::reg_name_
std::string reg_name_
Definition registration.h:548
pcl::Registration< PointSource, PointTarget, float >::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition registration.h:76
pcl::Registration< PointSource, PointTarget, float >::PointCloudTarget
pcl::PointCloud< PointTarget > PointCloudTarget
Definition registration.h:80
pcl::Registration< PointSource, PointTarget, float >::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition registration.h:77
pcl::Registration< PointSource, PointTarget, float >::tree_
KdTreePtr tree_
Definition registration.h:551
pcl::Registration< PointSource, PointTarget, float >::Ptr
shared_ptr< Registration< PointSource, PointTarget, float > > Ptr
Definition registration.h:66
pcl::Registration< PointSource, PointTarget, float >::ConstPtr
shared_ptr< const Registration< PointSource, PointTarget, float > > ConstPtr
Definition registration.h:67
pcl::Registration< PointSource, PointTarget, float >::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition registration.h:81
pcl::Registration< PointSource, PointTarget, float >::converged_
bool converged_
Definition registration.h:613
pcl::Registration< PointSource, PointTarget, float >::ransac_iterations_
int ransac_iterations_
Definition registration.h:566
pcl::Registration< PointSource, PointTarget, float >::correspondences_
CorrespondencesPtr correspondences_
Definition registration.h:621
pcl::registration::FPCSInitialAlignment
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition ia_fpcs.h:81
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_edge_diff_
float max_edge_diff_
Definition ia_fpcs.h:538
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::delta_
float delta_
Definition ia_fpcs.h:486
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_mse_
float max_mse_
Definition ia_fpcs.h:548
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::validateMatch
virtual int validateMatch(const pcl::Indices &base_indices, const pcl::Indices &match_indices, const pcl::Correspondences &correspondences, Eigen::Matrix4f &transformation)
Definition ia_fpcs.hpp:849
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::fitness_score_
float fitness_score_
Definition ia_fpcs.h:506
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::diameter_
float diameter_
Definition ia_fpcs.h:509
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::FPCSInitialAlignment
FPCSInitialAlignment()
Definition ia_fpcs.hpp:125
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_pair_diff_
float max_pair_diff_
Definition ia_fpcs.h:533
pcl::registration::KFPCSInitialAlignment::getTBestCandidates
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:294
pcl::registration::KFPCSInitialAlignment::setUpperTranslationThreshold
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition ia_kfpcs.h:95
pcl::registration::KFPCSInitialAlignment::getLambda
float getLambda() const
Definition ia_kfpcs.h:134
pcl::registration::KFPCSInitialAlignment::getLowerTranslationThreshold
float getLowerTranslationThreshold() const
Definition ia_kfpcs.h:118
pcl::registration::KFPCSInitialAlignment::indices_validation_
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition ia_kfpcs.h:268
pcl::registration::KFPCSInitialAlignment::upper_trl_boundary_
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition ia_kfpcs.h:252
pcl::registration::KFPCSInitialAlignment::finalCompute
void finalCompute(const std::vector< MatchingCandidates > &candidates) override
Final computation of best match out of vector of matches.
Definition ia_kfpcs.hpp:215
pcl::registration::KFPCSInitialAlignment::handleMatches
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:118
pcl::registration::KFPCSInitialAlignment::setLambda
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition ia_kfpcs.h:127
pcl::registration::KFPCSInitialAlignment::setLowerTranslationThreshold
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition ia_kfpcs.h:111
pcl::registration::KFPCSInitialAlignment::lambda_
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition ia_kfpcs.h:255
pcl::registration::KFPCSInitialAlignment::KFPCSInitialAlignment
KFPCSInitialAlignment()
Constructor.
Definition ia_kfpcs.hpp:48
pcl::registration::KFPCSInitialAlignment::getNBestCandidates
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:257
pcl::registration::KFPCSInitialAlignment::candidates_
MatchingCandidates candidates_
Container for resulting vector of registration candidates.
Definition ia_kfpcs.h:258
pcl::registration::KFPCSInitialAlignment::validateTransformation
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:165
pcl::registration::KFPCSInitialAlignment::~KFPCSInitialAlignment
~KFPCSInitialAlignment() override=default
Destructor.
pcl::registration::KFPCSInitialAlignment::getUpperTranslationThreshold
float getUpperTranslationThreshold() const
Definition ia_kfpcs.h:102
pcl::registration::KFPCSInitialAlignment::lower_trl_boundary_
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition ia_kfpcs.h:246
pcl::registration::KFPCSInitialAlignment::use_trl_score_
bool use_trl_score_
Flag if translation score should be used in validation (internal calculation).
Definition ia_kfpcs.h:262
pcl::registration::KFPCSInitialAlignment::initCompute
bool initCompute() override
Internal computation initialization.
Definition ia_kfpcs.hpp:56
pcl::registration
Definition convergence_criteria.h:46
pcl::registration::MatchingCandidates
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Definition matching_candidate.h:81
pcl
Definition convolution.h:46
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::registration::MatchingCandidate
Container for matching candidate consisting of.
Definition matching_candidate.h:56