From: Jochen Sprickerhof Date: Tue, 16 Aug 2016 11:45:10 +0000 (+0200) Subject: Imported Upstream version 1.8.0+dfsg1 X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~11 X-Git-Url: https://dgit.raspbian.org/?a=commitdiff_plain;h=ef3a772ac664fd10fbf34ea6382101a7599a6c65;p=pcl.git Imported Upstream version 1.8.0+dfsg1 --- diff --git a/registration/include/pcl/registration/ia_kfpcs.h b/registration/include/pcl/registration/ia_kfpcs.h deleted file mode 100644 index 49f7cfbe..00000000 --- a/registration/include/pcl/registration/ia_kfpcs.h +++ /dev/null @@ -1,263 +0,0 @@ -/* - * 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 - -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 - class KFPCSInitialAlignment : public virtual FPCSInitialAlignment - { - public: - /** \cond */ - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; - - typedef pcl::PointCloud PointCloudSource; - typedef typename PointCloudSource::Ptr PointCloudSourcePtr; - typedef typename PointCloudSource::iterator PointCloudSourceIterator; - - typedef pcl::PointCloud 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 ::deinitCompute; - using PCLBase ::input_; - using PCLBase ::indices_; - - using Registration ::reg_name_; - using Registration ::tree_; - using Registration ::final_transformation_; - using Registration ::ransac_iterations_; - using Registration ::correspondences_; - using Registration ::converged_; - - using FPCSInitialAlignment ::delta_; - using FPCSInitialAlignment ::approx_overlap_; - using FPCSInitialAlignment ::max_pair_diff_; - using FPCSInitialAlignment ::max_edge_diff_; - using FPCSInitialAlignment ::coincidation_limit_; - using FPCSInitialAlignment ::max_mse_; - using FPCSInitialAlignment ::max_inlier_dist_sqr_; - using FPCSInitialAlignment ::diameter_; - using FPCSInitialAlignment ::normalize_delta_; - using FPCSInitialAlignment ::fitness_score_; - using FPCSInitialAlignment ::score_threshold_; - using FPCSInitialAlignment ::linkMatchWithBase; - using FPCSInitialAlignment ::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 &base_indices, - std::vector > &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 &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 - -#endif // PCL_REGISTRATION_IA_KFPCS_H_ diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp deleted file mode 100644 index 4b4ca3cc..00000000 --- a/registration/include/pcl/registration/impl/ia_kfpcs.hpp +++ /dev/null @@ -1,293 +0,0 @@ -/* - * 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 -pcl::registration::KFPCSInitialAlignment ::KFPCSInitialAlignment () : - lower_trl_boundary_ (-1.f), - upper_trl_boundary_ (-1.f), - lambda_ (0.5f), - candidates_ (), - use_trl_score_ (false), - indices_validation_ (new std::vector ) -{ - reg_name_ = "pcl::registration::KFPCSInitialAlignment"; -} - - -/////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::registration::KFPCSInitialAlignment ::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 ::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 void -pcl::registration::KFPCSInitialAlignment ::handleMatches ( - const std::vector &base_indices, - std::vector > &matches, - MatchingCandidates &candidates) -{ - candidates.clear (); - float fitness_score = FLT_MAX; - - // loop over all Candidate matches - for (std::vector >::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 int -pcl::registration::KFPCSInitialAlignment ::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 ids; - std::vector 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 void -pcl::registration::KFPCSInitialAlignment ::finalCompute ( - const std::vector &candidates) -{ - // reorganize candidates into single vector - size_t total_size = 0; - std::vector ::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 void -pcl::registration::KFPCSInitialAlignment ::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 void -pcl::registration::KFPCSInitialAlignment ::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_ diff --git a/test/2d/lena-grayscale.png b/test/2d/lena-grayscale.png deleted file mode 100644 index 9ac73d82..00000000 Binary files a/test/2d/lena-grayscale.png and /dev/null differ