+++ /dev/null
-/*
- * 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
- *
- * * The use for research only (no for any commercial application).
- * * 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_IA_KFPCS_H_
-#define PCL_REGISTRATION_IA_KFPCS_H_
-
-#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 */
- typedef boost::shared_ptr <KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > Ptr;
- typedef boost::shared_ptr <const KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > ConstPtr;
-
- typedef pcl::PointCloud <PointSource> PointCloudSource;
- typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
- typedef typename PointCloudSource::iterator PointCloudSourceIterator;
-
- typedef pcl::PointCloud <PointTarget> PointCloudTarget;
- typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
- typedef typename PointCloudTarget::iterator PointCloudTargetIterator;
-
- typedef pcl::registration::MatchingCandidate MatchingCandidate;
- typedef pcl::registration::MatchingCandidates MatchingCandidates;
- /** \endcond */
-
-
- /** \brief Constructor. */
- KFPCSInitialAlignment ();
-
- /** \brief Destructor. */
- virtual ~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. */
- virtual bool
- initCompute ();
-
- /** \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
- */
- virtual void
- handleMatches (
- const std::vector <int> &base_indices,
- std::vector <std::vector <int> > &matches,
- MatchingCandidates &candidates);
-
- /** \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)
- */
- virtual int
- validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score);
-
- /** \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
- */
- virtual void
- finalCompute (const std::vector <MatchingCandidates > &candidates);
-
-
- /** \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>
-
-#endif // PCL_REGISTRATION_IA_KFPCS_H_
+++ /dev/null
-/*
- * 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
- *
- * * The use for research only (no for any commercial application).
- * * 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),
- candidates_ (),
- 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 < 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 (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
- {
- 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_indices, correspondences_temp);
-
- // check match based on residuals of the corresponding points after transformation
- if (validateMatch (base_indices, *match_indices, 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 unsuccessfull 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
- size_t total_size = 0;
- std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
- for (it = candidates.begin (); it != it_e; it++)
- total_size += it->size ();
-
- candidates_.clear ();
- candidates_.reserve (total_size);
-
- MatchingCandidates::const_iterator it_curr, it_curr_e;
- for (it = candidates.begin (); it != it_e; it++)
- for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
- candidates_.push_back (*it_curr);
-
- // sort acoording 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_
projects / pcl.git / commitdiff
