266 lines
11 KiB
C
266 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.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#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 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_;
|
||
|
|
|
||
|
|
/** \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>
|