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.
* \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 ()
{};
/** \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>::linkMatchWithBase;
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 std::vector <int> &base_indices,
std::vector <std::vector <int> > &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_;
/** \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_;
/** \brief Weighting factor for translation costs (standard = 0.5). */
float lambda_;
/** \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_;
/** \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::registration::KFPCSInitialAlignment::lower_trl_boundary_
float lower_trl_boundary_
Lower boundary for translation costs calculation.
Definition: ia_kfpcs.h:232
pcl::Registration::Ptr
shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
Definition: registration.h:70
pcl::registration::KFPCSInitialAlignment::KFPCSInitialAlignment
KFPCSInitialAlignment()
Constructor.
Definition: ia_kfpcs.hpp:42
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::registration::KFPCSInitialAlignment::upper_trl_boundary_
float upper_trl_boundary_
Upper boundary for translation costs calculation.
Definition: ia_kfpcs.h:238
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:60
pcl::registration::FPCSInitialAlignment
FPCSInitialAlignment computes corresponding four point congruent sets as described in: "4-points cong...
Definition: ia_fpcs.h:76
pcl::registration::KFPCSInitialAlignment::initCompute
bool initCompute() override
Internal computation initialization.
Definition: ia_kfpcs.hpp:55
pcl::registration::KFPCSInitialAlignment::~KFPCSInitialAlignment
~KFPCSInitialAlignment()
Destructor.
Definition: ia_kfpcs.h:78
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:248
pcl::registration::KFPCSInitialAlignment::getLowerTranslationThreshold
float getLowerTranslationThreshold() const
Definition: ia_kfpcs.h:110
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:215
pcl::registration::KFPCSInitialAlignment::setUpperTranslationThreshold
void setUpperTranslationThreshold(float upper_trl_boundary)
Set the upper translation threshold used for score evaluation.
Definition: ia_kfpcs.h:86
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:254
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::PointCloud< PointSource >
pcl::registration::KFPCSInitialAlignment::getUpperTranslationThreshold
float getUpperTranslationThreshold() const
Definition: ia_kfpcs.h:93
pcl::Registration::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:81
pcl::registration::MatchingCandidates
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Definition: matching_candidate.h:87
pcl::registration::KFPCSInitialAlignment::getLambda
float getLambda() const
Definition: ia_kfpcs.h:127
pcl::registration::KFPCSInitialAlignment
KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints as describe...
Definition: ia_kfpcs.h:54
pcl::Registration::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: registration.h:85
pcl::registration::KFPCSInitialAlignment::setLambda
void setLambda(float lambda)
Set the weighting factor of the translation cost term.
Definition: ia_kfpcs.h:120
pcl::registration::KFPCSInitialAlignment::lambda_
float lambda_
Weighting factor for translation costs (standard = 0.5).
Definition: ia_kfpcs.h:241
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:178
pcl::registration::KFPCSInitialAlignment::candidates_
MatchingCandidates candidates_
Container for resulting vector of registration candidates.
Definition: ia_kfpcs.h:245
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:415
pcl::registration::KFPCSInitialAlignment::indices_validation_
pcl::IndicesPtr indices_validation_
Subset of input indices on which we evaluate candidates.
Definition: ia_kfpcs.h:253
pcl::registration::KFPCSInitialAlignment::setLowerTranslationThreshold
void setLowerTranslationThreshold(float lower_trl_boundary)
Set the lower translation threshold used for score evaluation.
Definition: ia_kfpcs.h:103
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:131
pcl::Registration::ConstPtr
shared_ptr< const Registration< PointSource, PointTarget, Scalar > > ConstPtr
Definition: registration.h:71
pcl::registration::MatchingCandidate
Container for matching candidate consisting of.
Definition: matching_candidate.h:55
pcl::PointCloud< PointSource >::iterator
typename VectorType::iterator iterator
Definition: point_cloud.h:427
pcl::registration::KFPCSInitialAlignment::handleMatches
void handleMatches(const std::vector< int > &base_indices, std::vector< std::vector< int > > &matches, MatchingCandidates &candidates) override
Method to handle current candidate matches.
Definition: ia_kfpcs.hpp:97
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90