From 391e353e2aae618a9b12cb9f7c01ac3ec87432e1 Mon Sep 17 00:00:00 2001 From: theilerp Date: Tue, 13 Jan 2015 14:23:51 +0100 Subject: [PATCH] Added 4PCS for initial pairwise point cloud registration --- .../include/pcl/registration/ia_fpcs.h | 571 +++++++++++ .../include/pcl/registration/impl/ia_fpcs.hpp | 910 ++++++++++++++++++ .../impl/transformation_estimation_3point.hpp | 181 ++++ .../pcl/registration/matching_candidate.h | 103 ++ .../transformation_estimation_3point.h | 145 +++ test/registration/CMakeLists.txt | 4 + test/registration/test_fpcs_ia.cpp | 119 +++ test/registration/test_fpcs_ia_data.h | 16 + 8 files changed, 2049 insertions(+) create mode 100644 registration/include/pcl/registration/ia_fpcs.h create mode 100644 registration/include/pcl/registration/impl/ia_fpcs.hpp create mode 100644 registration/include/pcl/registration/impl/transformation_estimation_3point.hpp create mode 100644 registration/include/pcl/registration/matching_candidate.h create mode 100644 registration/include/pcl/registration/transformation_estimation_3point.h create mode 100644 test/registration/test_fpcs_ia.cpp create mode 100644 test/registration/test_fpcs_ia_data.h diff --git a/registration/include/pcl/registration/ia_fpcs.h b/registration/include/pcl/registration/ia_fpcs.h new file mode 100644 index 00000000000..c4fcfc2b18d --- /dev/null +++ b/registration/include/pcl/registration/ia_fpcs.h @@ -0,0 +1,571 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * 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_FPCS_H_ +#define PCL_REGISTRATION_IA_FPCS_H_ + +#include +#include +#include + +namespace pcl +{ + /** \brief Compute the mean point density of a given point cloud. + * \param[in] cloud pointer to the input point cloud + * \param[in] max_dist maximum distance of a point to be considered as a neighbor + * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set) + * \return the mean point density of a given point cloud + */ + template inline float + getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, float max_dist, int nr_threads = 1); + + /** \brief Compute the mean point density of a given point cloud. + * \param[in] cloud pointer to the input point cloud + * \param[in] indices the vector of point indices to use from \a cloud + * \param[in] max_dist maximum distance of a point to be considered as a neighbor + * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set) + * \return the mean point density of a given point cloud + */ + template inline float + getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, const std::vector &indices, + float max_dist, int nr_threads = 1); + + + namespace registration + { + /** \brief FPCSInitialAlignment computes corresponding four point congruent sets as described in: + * "4-points congruent sets for robust pairwise surface registration", Dror Aiger, Niloy Mitra, Daniel Cohen-Or. + * ACM Transactions on Graphics, vol. 27(3), 2008 + * \author P.W.Theiler + * \ingroup registration + */ + template + class FPCSInitialAlignment : public Registration + { + public: + /** \cond */ + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + typedef pcl::search::KdTree KdTreeReciprocal; + typedef typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr; + + typedef pcl::PointCloud PointCloudTarget; + typedef pcl::PointCloud PointCloudSource; + typedef typename PointCloudSource::Ptr PointCloudSourcePtr; + typedef typename PointCloudSource::iterator PointCloudSourceIterator; + + typedef pcl::PointCloud Normals; + typedef typename Normals::ConstPtr NormalsConstPtr; + + typedef pcl::registration::MatchingCandidate MatchingCandidate; + typedef std::vector MatchingCandidates; + /** \endcond */ + + + /** \brief Constructor. + * Resets the maximum number of iterations to 0 thus forcing an internal computation if not set by the user. + * Sets the number of RANSAC iterations to 1000 and the standard transformation estimation to TransformationEstimation3Point. + */ + FPCSInitialAlignment (); + + /** \brief Destructor. */ + virtual ~FPCSInitialAlignment () + {}; + + + /** \brief Provide a pointer to the vector of target indices. + * \param[in] target_indices a pointer to the target indices + */ + inline void + setTargetIndices (const IndicesPtr &target_indices) + { + target_indices_ = target_indices; + }; + + /** \return a pointer to the vector of target indices. */ + inline IndicesPtr + getTargetIndices () const + { + return (target_indices_); + }; + + + /** \brief Provide a pointer to the normals of the source point cloud. + * \param[in] source_normals pointer to the normals of the source pointer cloud. + */ + inline void + setSourceNormals (const NormalsConstPtr &source_normals) + { + source_normals_ = source_normals; + }; + + /** \return the normals of the source point cloud. */ + inline NormalsConstPtr + getSourceNormals () const + { + return (source_normals_); + }; + + + /** \brief Provide a pointer to the normals of the target point cloud. + * \param[in] target_normals point to the normals of the target point cloud. + */ + inline void + setTargetNormals (const NormalsConstPtr &target_normals) + { + target_normals_ = target_normals; + }; + + /** \return the normals of the target point cloud. */ + inline NormalsConstPtr + getTargetNormals () const + { + return (target_normals_); + }; + + + /** \brief Set the number of used threads if OpenMP is activated. + * \param[in] nr_threads the number of used threads + */ + inline void + setNumberOfThreads (int nr_threads) + { + nr_threads_ = nr_threads; + }; + + /** \return the number of threads used if OpenMP is activated. */ + inline int + getNumberOfThreads () const + { + return (nr_threads_); + }; + + + /** \brief Set the constant factor delta which weights the internally calculated parameters. + * \param[in] delta the weight factor delta + * \param[in] normalize flag if delta should be normalized according to point cloud density + */ + inline void + setDelta (float delta, bool normalize = false) + { + delta_ = delta; + normalize_delta_ = normalize; + }; + + /** \return the constant factor delta which weights the internally calculated parameters. */ + inline float + getDelta () const + { + return (delta_); + }; + + + /** \brief Set the approximate overlap between source and target. + * \param[in] approx_overlap the estimated overlap + */ + inline void + setApproxOverlap (float approx_overlap) + { + approx_overlap_ = approx_overlap; + }; + + /** \return the approximated overlap between source and target. */ + inline float + getApproxOverlap () const + { + return (approx_overlap_); + }; + + + /** \brief Set the scoring threshold used for early finishing the method. + * \param[in] score_threshold early terminating score criteria + */ + inline void + setScoreThreshold (float score_threshold) + { + score_threshold_ = score_threshold; + }; + + /** \return the scoring threshold used for early finishing the method. */ + inline float + getScoreThreshold () const + { + return (score_threshold_); + }; + + + /** \brief Set the number of source samples to use during alignment. + * \param[in] nr_samples the number of source samples + */ + inline void + setNumberOfSamples (int nr_samples) + { + nr_samples_ = nr_samples; + }; + + /** \return the number of source samples to use during alignment. */ + inline int + getNumberOfSamples () const + { + return (nr_samples_); + }; + + + /** \brief Set the maximum normal difference between valid point correspondences in degree. + * \param[in] max_norm_diff the maximum difference in degree + */ + inline void + setMaxNormalDifference (float max_norm_diff) + { + max_norm_diff_ = max_norm_diff; + }; + + /** \return the maximum normal difference between valid point correspondences in degree. */ + inline float + getMaxNormalDifference () const + { + return (max_norm_diff_); + }; + + + /** \brief Set the maximum computation time in seconds. + * \param[in] max_runtime the maximum runtime of the method in seconds + */ + inline void + setMaxComputationTime (int max_runtime) + { + max_runtime_ = max_runtime; + }; + + /** \return the maximum computation time in seconds. */ + inline int + getMaxComputationTime () const + { + return (max_runtime_); + }; + + + /** \return the fitness score of the best scored four-point match. */ + inline float + getFitnessScore () const + { + return (fitness_score_); + }; + + protected: + + using PCLBase ::deinitCompute; + using PCLBase ::input_; + using PCLBase ::indices_; + + using Registration ::reg_name_; + using Registration ::target_; + using Registration ::tree_; + using Registration ::correspondences_; + using Registration ::target_cloud_updated_; + using Registration ::final_transformation_; + using Registration ::max_iterations_; + using Registration ::ransac_iterations_; + using Registration ::transformation_estimation_; + using Registration ::converged_; + + + /** \brief Rigid transformation computation method. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transforamtion + */ + virtual void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess); + + + /** \brief Internal computation initialization. */ + virtual bool + initCompute (); + + /** \brief Select an approximately coplanar set of four points from the source cloud. + * \param[out] base_indices selected source cloud indices, further used as base (B) + * \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points + * \return + * * < 0 no coplanar four point sets with large enough sampling distance was found + * * = 0 a set of four congruent points was selected + */ + int + selectBase (std::vector &base_indices, float (&ratio)[2]); + + /** \brief Select randomly a triplet of points with large point-to-point distances. The minimum point + * sampling distance is calculated based on the estimated point cloud overlap during initialization. + * + * \param[out] base_indices indices of base B + * \return + * * < 0 no triangle with large enough base lines could be selected + * * = 0 base triangle succesully selected + */ + int + selectBaseTriangle (std::vector &base_indices); + + /** \brief Setup the base (four coplanar points) by ordering the points and computing intersection + * ratios and segment to segment distances of base diagonal. + * + * \param[in,out] base_indices indices of base B (will be reordered) + * \param[out] ratio diagonal intersection ratios of base points + */ + void + setupBase (std::vector &base_indices, float (&ratio)[2]); + + /** \brief Calculate intersection ratios and segment to segment distances of base diagonals. + * \param[in] base_indices indices of base B + * \param[out] ratio diagonal intersection ratios of base points + * \return quality value of diagonal intersection + */ + float + segmentToSegmentDist (const std::vector &base_indices, float (&ratio)[2]); + + /** \brief Search for corresponding point pairs given the distance between two base points. + * + * \param[in] idx1 first index of current base segment (in source cloud) + * \param[in] idx2 second index of current base segment (in source cloud) + * \param[out] pairs resulting point pairs with point-to-point distance close to ref_dist + * \return + * * < 0 no corresponding point pair was found + * * = 0 at least one point pair candidate was found + */ + virtual int + bruteForceCorrespondences (int idx1, int idx2, pcl::Correspondences &pairs); + + /** \brief Determine base matches by combining the point pair candidate and search for coinciding + * intersection points using the diagonal segment ratios of base B. The coincidation threshold is + * calculated during initialization (coincidation_limit_). + * + * \param[in] base_indices indices of base B + * \param[out] matches vector of candidate matches w.r.t the base B + * \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B + * \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B + * \param[in] ratio diagonal intersection ratios of base points + * \return + * * < 0 no base match could be found + * * = 0 at least one base match was found + */ + virtual int + determineBaseMatches ( + const std::vector &base_indices, + std::vector > &matches, + const pcl::Correspondences &pairs_a, + const pcl::Correspondences &pairs_b, + const float (&ratio)[2]); + + /** \brief Check if outer rectangle distance of matched points fit with the base rectangle. + * + * \param[in] match_indices indices of match M + * \param[in] ds edge lengths of base B + * \return + * * < 0 at least one edge of the match M has no corresponding one in the base B + * * = 0 edges of match M fits to the ones of base B + */ + int + checkBaseMatch (const std::vector &match_indices, const float (&ds)[4]); + + /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the + * base and store the best fitting match (together with its score and estimated transformation). + * \note For forwards compatibility the results are stored in 'vectors of size 1'. + * + * \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 Sets the correspondences between the base B and the match M by using the distance of each point + * to the centroid of the rectangle. + * + * \param[in] base_indices indices of base B + * \param[in] match_indices indices of match M + * \param[out] correspondences resulting correspondences + */ + virtual void + linkMatchWithBase ( + const std::vector &base_indices, + std::vector &match_indices, + pcl::Correspondences &correspondences); + + /** \brief Validate the matching by computing the transformation between the source and target based on the + * four matched points and by comparing the mean square error (MSE) to a threshold. The MSE limit was + * calculated during initialization (max_mse_). + * + * \param[in] base_indices indices of base B + * \param[in] match_indices indices of match M + * \param[in] correspondences corresondences between source and target + * \param[out] transformation resulting transformation matrix + * \return + * * < 0 MSE bigger than max_mse_ + * * = 0 MSE smaller than max_mse_ + */ + virtual int + validateMatch ( + const std::vector &base_indices, + const std::vector &match_indices, + const pcl::Correspondences &correspondences, + Eigen::Matrix4f &transformation); + + /** \brief Validate the transformation by calculating the number of inliers after transforming the source cloud. + * The resulting fitness 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 fitness_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 best 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 Normals of source point cloud. */ + NormalsConstPtr source_normals_; + + /** \brief Normals of target point cloud. */ + NormalsConstPtr target_normals_; + + + /** \brief Number of threads for parallelization (standard = 1). + * \note Only used if run compiled with OpenMP. + */ + int nr_threads_; + + /** \brief Estimated overlap between source and target (standard = 0.5). */ + float approx_overlap_; + + /** \brief Delta value of 4pcs algorithm (standard = 1.0). + * It can be used as: + * * absolute value (normalization = false), value should represent the point accuracy to ensure finding neighbors between source <-> target + * * relative value (normalization = true), to adjust the internally calculated point accuracy (= point density) + */ + float delta_; + + /** \brief Score threshold to stop calculation with success. + * If not set by the user it is equal to the approximated overlap + */ + float score_threshold_; + + /** \brief The number of points to uniformly sample the source point cloud. (standard = 0 => full cloud). */ + int nr_samples_; + + /** \brief Maximum normal difference of corresponding point pairs in degrees (standard = 90). */ + float max_norm_diff_; + + /** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited). */ + int max_runtime_; + + + /** \brief Resulting fitness score of the best match. */ + float fitness_score_; + + + /** \brief Estimated diamter of the target point cloud. */ + float diameter_; + + /** \brief Estimated squared metric overlap between source and target. + * \note Internally calculated using the estimated overlap and the extent of the source cloud. + * It is used to derive the minimum sampling distance of the base points as well as to calculated + * the number of trys to reliable find a correct mach. + */ + float max_base_diameter_sqr_; + + /** \brief Use normals flag. */ + bool use_normals_; + + /** \brief Normalize delta flag. */ + bool normalize_delta_; + + + /** \brief A pointer to the vector of source point indices to use after sampling. */ + pcl::IndicesPtr source_indices_; + + /** \brief A pointer to the vector of target point indices to use after sampling. */ + pcl::IndicesPtr target_indices_; + + /** \brief Maximal difference between corresponding point pairs in source and target. + * \note Internally calculated using an estimation of the point density. + */ + float max_pair_diff_; + + /** \brief Maximal difference between the length of the base edges and valid match edges. + * \note Internally calculated using an estimation of the point density. + */ + float max_edge_diff_; + + /** \brief Maximal distance between coinciding intersection points to find valid matches. + * \note Internally calculated using an estimation of the point density. + */ + float coincidation_limit_; + + /** \brief Maximal mean squared errors of a transformation calculated from a candidate match. + * \note Internally calculated using an estimation of the point density. + */ + float max_mse_; + + /** \brief Maximal squared point distance between source and target points to count as inlier. + * \note Internally calculated using an estimation of the point density. + */ + float max_inlier_dist_sqr_; + + + /** \brief Definition of a small error. */ + const float small_error_; + + }; + }; // namespace registration +}; // namespace pcl + +#include + +#endif // PCL_REGISTRATION_IA_FPCS_H_ diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp new file mode 100644 index 00000000000..735e75aa2de --- /dev/null +++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp @@ -0,0 +1,910 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * 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_FPCS_H_ +#define PCL_REGISTRATION_IMPL_IA_FPCS_H_ + +#include +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, float max_dist, int nr_threads) +{ + const float max_dist_sqr = max_dist * max_dist; + const std::size_t s = cloud.size (); + + pcl::search::KdTree tree; + tree.setInputCloud (cloud); + + float mean_dist = 0.f; + int num = 0; + std::vector ids (2); + std::vector dists_sqr (2); + +#ifdef _OPENMP +#pragma omp parallel for \ + reduction (+:mean_dist, num) \ + private (ids, dists_sqr) shared (s, tree, cloud, indices, max_dist_sqr) \ + default (none)num_threads (nr_threads) +#endif + + for (int i = 0; i < 1000; i++) + { + tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr); + if (dists_sqr[1] < max_dist_sqr) + { + mean_dist += std::sqrt (dists_sqr[1]); + num++; + } + } + + return (mean_dist / num); +}; + + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, const std::vector &indices, + float max_dist, int nr_threads) +{ + const float max_dist_sqr = max_dist * max_dist; + const std::size_t s = indices.size (); + + pcl::search::KdTree tree; + tree.setInputCloud (cloud); + + float mean_dist = 0.f; + int num = 0; + std::vector ids (2); + std::vector dists_sqr (2); + +#ifdef _OPENMP +#pragma omp parallel for \ + reduction (+:mean_dist, num) \ + private (ids, dists_sqr) shared (s, tree, cloud, indices, max_dist_sqr) \ + default (none)num_threads (nr_threads) +#endif + + for (int i = 0; i < 1000; i++) + { + tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr); + if (dists_sqr[1] < max_dist_sqr) + { + mean_dist += std::sqrt (dists_sqr[1]); + num++; + } + } + + return (mean_dist / num); +}; + + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::registration::FPCSInitialAlignment ::FPCSInitialAlignment () : + source_normals_ (), + target_normals_ (), + nr_threads_ (1), + approx_overlap_ (0.5f), + delta_ (1.f), + score_threshold_ (FLT_MAX), + nr_samples_ (0), + max_norm_diff_ (90.f), + max_runtime_ (0), + fitness_score_ (FLT_MAX), + diameter_ (), + max_base_diameter_sqr_ (), + use_normals_ (false), + normalize_delta_ (true), + max_pair_diff_ (), + max_edge_diff_ (), + coincidation_limit_ (), + max_mse_ (), + max_inlier_dist_sqr_ (), + small_error_ (0.00001f) +{ + reg_name_ = "pcl::registration::FPCSInitialAlignment"; + max_iterations_ = 0; + ransac_iterations_ = 1000; + transformation_estimation_.reset (new pcl::registration::TransformationEstimation3Point ); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::computeTransformation ( + PointCloudSource &output, + const Eigen::Matrix4f &guess) +{ + if (!initCompute ()) + return; + + final_transformation_ = guess; + bool abort = false; + std::vector all_candidates (max_iterations_); + pcl::StopWatch timer; + + #ifdef _OPENMP + #pragma omp parallel for schedule (dynamic) num_threads (nr_threads_) + #endif + + for (int i = 0; i < max_iterations_; i++) + { + + #ifdef _OPENMP + #pragma omp flush (abort) + #endif + + MatchingCandidates candidates (1); + std::vector base_indices (4); + float ratio[2]; + all_candidates [i] = candidates; + + if (!abort) + { + // select four coplanar point base + if (selectBase (base_indices, ratio) == 0) + { + // calculate candidate pair correspondences using diagonal lenghts of base + pcl::Correspondences pairs_a, pairs_b; + if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 && + bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0) + { + // determine candidate matches by combining pair correspondences based on segment distances + std::vector > matches; + if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0) + { + // check and evaluate candidate matches and store them + handleMatches (base_indices, matches, candidates); + if (candidates.size () != 0) + all_candidates [i] = candidates; + } + } + } + + // check terminate early (time or fitness_score threshold reached) + abort = (candidates.size () > 0 ? candidates[0].fitness_score < score_threshold_ : abort); + abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_); + + + #ifdef _OPENMP + #pragma omp flush (abort) + #endif + } + } + + // determine best match over all trys + finalCompute (all_candidates); + + // apply the final transformation + pcl::transformPointCloud (*input_, output, final_transformation_); + + deinitCompute (); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::FPCSInitialAlignment ::initCompute () +{ + std::srand (static_cast (std::time (NULL))); + + // basic pcl initialization + if (!pcl::PCLBase ::initCompute ()) + return (false); + + // check if source and target are given + if (!input_ || !target_) + { + PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ()); + return (false); + } + + if (!target_indices_ || target_indices_->size () == 0) + { + target_indices_.reset (new std::vector (static_cast (target_->size ()))); + int index = 0; + for (std::vector ::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++) + *it = index++; + target_cloud_updated_ = true; + } + + // if a sample size for the point clouds is given; prefarably no sampling of target cloud + if (nr_samples_ != 0) + { + const int ss = static_cast (indices_->size ()); + const int sample_fraction_src = std::max (1, static_cast (ss / nr_samples_)); + + source_indices_ = pcl::IndicesPtr (new std::vector ); + for (int i = 0; i < ss; i++) + if (rand () % sample_fraction_src == 0) + source_indices_->push_back ((*indices_) [i]); + } + else + source_indices_ = indices_; + + // check usage of normals + if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ()) + use_normals_ = true; + + // set up tree structures + if (target_cloud_updated_) + { + tree_->setInputCloud (target_, target_indices_); + target_cloud_updated_ = false; + } + + // set predefined variables + const int min_iterations = 4; + const float diameter_fraction = 0.3f; + + // get diameter of input cloud (distance between farthest points) + Eigen::Vector4f pt_min, pt_max; + pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max); + diameter_ = (pt_max - pt_min).norm (); + + // derive the limits for the random base selection + float max_base_diameter = diameter_* approx_overlap_ * 2.f; + max_base_diameter_sqr_ = max_base_diameter * max_base_diameter; + + // normalize the delta + if (normalize_delta_) + { + float mean_dist = getMeanPointDensity (target_, *target_indices_, 0.05f * diameter_, nr_threads_); + delta_ *= mean_dist; + } + + // heuristic determination of number of trials to have high probabilty of finding a good solution + if (max_iterations_ == 0) + { + float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations)); + max_iterations_ = static_cast (first_est / (diameter_fraction * approx_overlap_ * 2.f)); + } + + // set further parameter + if (score_threshold_ < FLT_MAX) + score_threshold_ = 1.f - approx_overlap_; + + if (max_iterations_ < 4) + max_iterations_ = 4; + + if (max_runtime_ < 1) + max_runtime_ = INT_MAX; + + // calculate internal parameters based on the the estimated point density + max_pair_diff_ = delta_ * 2.f; + max_edge_diff_ = delta_ * 4.f; + coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f) + max_mse_ = powf (delta_* 2.f, 2.f); + max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f); + + // reset fitness_score + fitness_score_ = FLT_MAX; + + return (true); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::selectBase ( + std::vector &base_indices, + float (&ratio)[2]) +{ + const float too_close_sqr = max_base_diameter_sqr_*0.01; + + Eigen::VectorXf coefficients (4); + pcl::SampleConsensusModelPlane plane (target_, target_indices_); + Eigen::Vector4f centre_pt; + float nearest_to_plane = FLT_MAX; + + // repeat base search until valid quadruple was found or ransac_iterations_ number of trys were unsuccessfull + for (int i = 0; i < ransac_iterations_; i++) + { + // random select an appropriate point triple + if (selectBaseTriangle (base_indices) < 0) + continue; + + std::vector base_triple (base_indices.begin (), base_indices.end () - 1); + plane.computeModelCoefficients (base_triple, coefficients); + pcl::compute3DCentroid (*target_, base_triple, centre_pt); + + // loop over all points in source cloud to find most suitable fourth point + const PointTarget *pt1 = &(target_->points[base_indices[0]]); + const PointTarget *pt2 = &(target_->points[base_indices[1]]); + const PointTarget *pt3 = &(target_->points[base_indices[2]]); + + for (std::vector ::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++) + { + const PointTarget *pt4 = &(target_->points[*it]); + + float d1 = pcl::squaredEuclideanDistance (*pt4, *pt1); + float d2 = pcl::squaredEuclideanDistance (*pt4, *pt2); + float d3 = pcl::squaredEuclideanDistance (*pt4, *pt3); + float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm (); + + // check distance between points w.r.t minimum sampling distance; EDITED -> 4th point now also limited by max base line + if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr || + d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_) + continue; + + // check distance to plane to get point closest to plane + float dist_to_plane = pcl::pointToPlaneDistance (*pt4, coefficients); + if (dist_to_plane < nearest_to_plane) + { + base_indices[3] = *it; + nearest_to_plane = dist_to_plane; + } + } + + // check if at least one point fullfilled the conditions + if (nearest_to_plane != FLT_MAX) + { + // order points to build largest quadrangle and calcuate intersection ratios of diagonals + setupBase (base_indices, ratio); + return (0); + } + } + + // return unsuccessfull if no quadruple was selected + return (-1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::selectBaseTriangle (std::vector &base_indices) +{ + int nr_points = static_cast (target_indices_->size ()); + float best_t = 0.f; + + // choose random first point + base_indices[0] = (*target_indices_)[rand () % nr_points]; + int *index1 = &base_indices[0]; + + // random search for 2 other points (as far away as overlap allows) + for (int i = 0; i < ransac_iterations_; i++) + { + int *index2 = &(*target_indices_)[rand () % nr_points]; + int *index3 = &(*target_indices_)[rand () % nr_points]; + + Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap (); + Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap (); + float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal + + // check for most suitable point triple + if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_) + { + best_t = t; + base_indices[1] = *index2; + base_indices[2] = *index3; + } + } + + // return if a triplet could be selected + return (best_t == 0.f ? -1 : 0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::setupBase ( + std::vector &base_indices, + float (&ratio)[2]) +{ + float best_t = FLT_MAX; + const std::vector copy (base_indices.begin (), base_indices.end ()); + std::vector temp (base_indices.begin (), base_indices.end ()); + + // loop over all combinations of base points + for (std::vector ::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++) + for (std::vector ::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++) + { + if (i == j) + continue; + + for (std::vector ::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++) + { + if (k == j || k == i) + continue; + + std::vector ::const_iterator l = copy.begin (); + while (l == i || l == j || l == k) + l++; + + temp[0] = *i; + temp[1] = *j; + temp[2] = *k; + temp[3] = *l; + + // calculate diagonal intersection ratios and check for suitable segment to segment distances + float ratio_temp[2]; + float t = segmentToSegmentDist (temp, ratio_temp); + if (t < best_t) + { + best_t = t; + ratio[0] = ratio_temp[0]; + ratio[1] = ratio_temp[1]; + base_indices = temp; + } + } + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::registration::FPCSInitialAlignment ::segmentToSegmentDist ( + const std::vector &base_indices, + float (&ratio)[2]) +{ + // get point vectors + Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap (); + Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap (); + Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap (); + + // calculate segment distances + float a = u.dot (u); + float b = u.dot (v); + float c = v.dot (v); + float d = u.dot (w); + float e = v.dot (w); + float D = a * c - b * b; + float sN = 0.f, sD = D; + float tN = 0.f, tD = D; + + // check segments + if (D < small_error_) + { + sN = 0.f; + sD = 1.f; + tN = e; + tD = c; + } + else + { + sN = (b * e - c * d); + tN = (a * e - b * d); + + if (sN < 0.f) + { + sN = 0.f; + tN = e; + tD = c; + } + else if (sN > sD) + { + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.f) + { + tN = 0.f; + + if (-d < 0.f) + sN = 0.f; + + else if (-d > a) + sN = sD; + + else + { + sN = -d; + sD = a; + } + } + + else if (tN > tD) + { + tN = tD; + + if ((-d + b) < 0.f) + sN = 0.f; + + else if ((-d + b) > a) + sN = sD; + + else + { + sN = (-d + b); + sD = a; + } + } + + // set intersection ratios + ratio[0] = (std::fabsf (sN) < small_error_) ? 0.f : sN / sD; + ratio[1] = (std::fabsf (tN) < small_error_) ? 0.f : tN / tD; + + Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v); + return (x.norm ()); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::bruteForceCorrespondences ( + int idx1, + int idx2, + pcl::Correspondences &pairs) +{ + const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f; + + // calculate reference segment distance and normal angle + float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]); + float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () - + target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f); + + // loop over all pairs of points in source point cloud + std::vector ::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1; + std::vector ::iterator it_in, it_in_e = source_indices_->end (); + for ( ; it_out != it_out_e; it_out++) + { + it_in = it_out + 1; + const PointSource *pt1 = &(*input_)[*it_out]; + for ( ; it_in != it_in_e; it_in++) + { + const PointSource *pt2 = &(*input_)[*it_in]; + + // check point distance compared to reference dist (from base) + float dist = pcl::euclideanDistance (*pt1, *pt2); + if (std::abs(dist - ref_dist) < max_pair_diff_) + { + // add here normal evaluation if normals are given + if (use_normals_) + { + const NormalT *pt1_n = &(source_normals_->points[*it_out]); + const NormalT *pt2_n = &(source_normals_->points[*it_in]); + + float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm (); + float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm (); + + float norm_diff = std::min (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle)); + if (norm_diff > max_norm_diff) + continue; + } + + pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist)); + pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist)); + } + } + } + + // return success if at least one correspondence was found + return (pairs.size () == 0 ? -1 : 0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::determineBaseMatches ( + const std::vector &base_indices, + std::vector > &matches, + const pcl::Correspondences &pairs_a, + const pcl::Correspondences &pairs_b, + const float (&ratio)[2]) +{ + // calculate edge lengths of base + float dist_base[4]; + dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]); + dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]); + dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]); + dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]); + + // loop over first point pair correspondences and store intermediate points 'e' in new point cloud + PointCloudSourcePtr cloud_e (new PointCloudSource); + cloud_e->resize (pairs_a.size () * 2); + PointCloudSourceIterator it_pt = cloud_e->begin (); + for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++) + { + const PointSource *pt1 = &(input_->points[it_pair->index_match]); + const PointSource *pt2 = &(input_->points[it_pair->index_query]); + + // calculate intermediate points using both ratios from base (r1,r2) + for (int i = 0; i < 2; i++, it_pt++) + { + it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x); + it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y); + it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z); + } + } + + // initialize new kd tree of intermediate points from first point pair correspondences + KdTreeReciprocalPtr tree_e (new KdTreeReciprocal); + tree_e->setInputCloud (cloud_e); + + std::vector ids; + std::vector dists_sqr; + + // loop over second point pair correspondences + for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++) + { + const PointTarget *pt1 = &(input_->points[it_pair->index_match]); + const PointTarget *pt2 = &(input_->points[it_pair->index_query]); + + // calculate intermediate points using both ratios from base (r1,r2) + for (int i = 0; i < 2; i++) + { + PointTarget pt_e; + pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x); + pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y); + pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z); + + // search for corresponding intermediate points + tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr); + for (std::vector ::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++) + { + std::vector match_indices (4); + + match_indices[0] = pairs_a[static_cast (std::floor ((float)(*it/2.f)))].index_match; + match_indices[1] = pairs_a[static_cast (std::floor ((float)(*it/2.f)))].index_query; + match_indices[2] = it_pair->index_match; + match_indices[3] = it_pair->index_query; + + // EDITED: added coarse check of match based on edge length (due to rigid-body ) + if (checkBaseMatch (match_indices, dist_base) < 0) + continue; + + matches.push_back (match_indices); + } + } + } + + // return unsuccessfull if no match was found + return (matches.size () > 0 ? 0 : -1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::checkBaseMatch ( + const std::vector &match_indices, + const float (&dist_ref)[4]) +{ + float d0 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[2]]); + float d1 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[3]]); + float d2 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[2]]); + float d3 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[3]]); + + // check edge distances of match w.r.t the base + return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ && + std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1; +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::handleMatches ( + const std::vector &base_indices, + std::vector > &matches, + MatchingCandidates &candidates) +{ + candidates.resize (1); + 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; + + // 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 + if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0) + continue; + + // check resulting using a sub sample of the source point cloud and compare to previous matches + if (validateTransformation (transformation_temp, fitness_score) < 0) + continue; + + // store best match as well as associated fitness_score and transformation + candidates[0].fitness_score = fitness_score; + candidates [0].transformation = transformation_temp; + correspondences_temp.erase (correspondences_temp.end () - 1); + candidates[0].correspondences = correspondences_temp; + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::linkMatchWithBase ( + const std::vector &base_indices, + std::vector &match_indices, + pcl::Correspondences &correspondences) +{ + // calculate centroid of base and target + Eigen::Vector4f centre_base, centre_match; + pcl::compute3DCentroid (*target_, base_indices, centre_base); + pcl::compute3DCentroid (*input_, match_indices, centre_match); + + PointTarget centre_pt_base; + centre_pt_base.x = centre_base[0]; + centre_pt_base.y = centre_base[1]; + centre_pt_base.z = centre_base[2]; + + PointSource centre_pt_match; + centre_pt_match.x = centre_match[0]; + centre_pt_match.y = centre_match[1]; + centre_pt_match.z = centre_match[2]; + + // find corresponding points according to their distance to the centroid + std::vector copy = match_indices; + + std::vector ::const_iterator it_base = base_indices.begin (), it_base_e = base_indices.end (); + std::vector ::iterator it_match, it_match_e = copy.end (); + std::vector ::iterator it_match_orig = match_indices.begin (); + for (; it_base != it_base_e; it_base++, it_match_orig++) + { + float dist_sqr_1 = pcl::squaredEuclideanDistance (target_->points[*it_base], centre_pt_base); + float best_diff_sqr = FLT_MAX; + int best_index; + + for (it_match = copy.begin (); it_match != it_match_e; it_match++) + { + // calculate difference of distances to centre point + float dist_sqr_2 = pcl::squaredEuclideanDistance (input_->points[*it_match], centre_pt_match); + float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2); + + if (diff_sqr < best_diff_sqr) + { + best_diff_sqr = diff_sqr; + best_index = *it_match; + } + } + + // assign new correspondence and update indices of matched targets + correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr)); + *it_match_orig = best_index; + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::validateMatch ( + const std::vector &base_indices, + const std::vector &match_indices, + const pcl::Correspondences &correspondences, + Eigen::Matrix4f &transformation) +{ + // only use triplet of points to simlify process (possible due to planar case) + pcl::Correspondences correspondences_temp = correspondences; + correspondences_temp.erase (correspondences_temp.end () - 1); + + // estimate transformation between correspondence set + transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation); + + // transform base points + PointCloudSource match_transformed; + pcl::transformPointCloud (*input_, match_indices, match_transformed, transformation); + + // calculate residuals of transformation and check against maximum threshold + std::size_t nr_points = correspondences_temp.size (); + float mse = 0.f; + for (std::size_t i = 0; i < nr_points; i++) + mse += pcl::squaredEuclideanDistance (match_transformed.points [i], target_->points [base_indices[i]]); + + mse /= nr_points; + return (mse < max_mse_ ? 0 : -1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::validateTransformation ( + Eigen::Matrix4f &transformation, + float &fitness_score) +{ + // transform source point cloud + PointCloudSource source_transformed; + pcl::transformPointCloud (*input_, *source_indices_, source_transformed, transformation); + + std::size_t nr_points = source_transformed.size (); + std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast ((1.f - fitness_score) * nr_points); + + float inlier_score_temp = 0; + std::vector ids; + std::vector dists_sqr; + PointCloudSourceIterator it = source_transformed.begin (); + + for (std::size_t i = 0; i < nr_points; it++, i++) + { + // search for nearest point using kd tree search + tree_->nearestKSearch (*it, 1, ids, dists_sqr); + inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0); + + // early terminating + if (nr_points - i + inlier_score_temp < terminate_value) + break; + } + + // check current costs and return unsuccessfull if larger than previous ones + inlier_score_temp /= static_cast (nr_points); + float fitness_score_temp = 1.f - inlier_score_temp; + + if (fitness_score_temp > fitness_score) + return (-1); + + fitness_score = fitness_score_temp; + return (0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::finalCompute ( + const std::vector &candidates) +{ + // get best fitness_score over all trys + int nr_candidates = static_cast (candidates.size ()); + int best_index = -1; + float best_score = FLT_MAX; + for (int i = 0; i < nr_candidates; i++) + { + const float &fitness_score = candidates [i][0].fitness_score; + if (fitness_score < best_score) + { + best_score = fitness_score; + best_index = i; + } + } + + // check if a valid candidate was available + if (!(best_index < 0)) + { + fitness_score_ = candidates [best_index][0].fitness_score; + final_transformation_ = candidates [best_index][0].transformation; + *correspondences_ = candidates [best_index][0].correspondences; + + // here we define convergence if resulting fitness_score is below 1-threshold + converged_ = fitness_score_ < score_threshold_; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// + +#endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_ diff --git a/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp new file mode 100644 index 00000000000..614b6f2bee9 --- /dev/null +++ b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp @@ -0,0 +1,181 @@ +/* + * 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. + * + */ +#ifndef PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ +#define PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (cloud_src.points.size () != 3 || cloud_tgt.points.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of points in source (%lu) and target (%lu) must be 3!\n", + cloud_src.points.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != 3 || cloud_tgt.points.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and points in target (%lu) must be 3!\n", + indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != 3 || indices_tgt.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and target (%lu) must be 3!\n", + indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + if (correspondences.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of correspondences (%lu) must be 3!\n", + correspondences.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const +{ + transformation_matrix.setIdentity (); + source_it.reset (); + target_it.reset (); + + Eigen::Matrix source_mean, target_mean; + pcl::compute3DCentroid (source_it, source_mean); + pcl::compute3DCentroid (target_it, target_mean); + + source_it.reset (); + target_it.reset (); + + Eigen::Matrix source_demean, target_demean; + pcl::demeanPointCloud (source_it, source_mean, source_demean, 3); + pcl::demeanPointCloud (target_it, target_mean, target_demean, 3); + + source_it.reset (); + target_it.reset (); + + Eigen::Matrix s1 = source_demean.col (1).head (3) - source_demean.col (0).head (3); + s1.normalize (); + + Eigen::Matrix s2 = source_demean.col (2).head (3) - source_demean.col (0).head (3); + s2 -= s2.dot (s1) * s1; + s2.normalize (); + + Eigen::Matrix source_rot; + source_rot.col (0) = s1; + source_rot.col (1) = s2; + source_rot.col (2) = s1.cross (s2); + + Eigen::Matrix t1 = target_demean.col (1).head (3) - target_demean.col (0).head (3); + t1.normalize (); + + Eigen::Matrix t2 = target_demean.col (2).head (3) - target_demean.col (0).head (3); + t2 -= t2.dot (t1) * t1; + t2.normalize (); + + Eigen::Matrix target_rot; + target_rot.col (0) = t1; + target_rot.col (1) = t2; + target_rot.col (2) = t1.cross (t2); + + //Eigen::Matrix R = source_rot * target_rot.transpose (); + Eigen::Matrix R = target_rot * source_rot.transpose (); + transformation_matrix.topLeftCorner (3, 3) = R; + //transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R * target_mean.head (3); + transformation_matrix.block (0, 3, 3, 1) = target_mean.head (3) - R * source_mean.head (3); +} + +//#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimation3Point; + +#endif // PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ diff --git a/registration/include/pcl/registration/matching_candidate.h b/registration/include/pcl/registration/matching_candidate.h new file mode 100644 index 00000000000..206e93310d3 --- /dev/null +++ b/registration/include/pcl/registration/matching_candidate.h @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * 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_MATCHING_CANDIDATE_H_ +#define PCL_REGISTRATION_MATCHING_CANDIDATE_H_ + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief Container for matching candidate consisting of + * + * * fitness score value as a result of the matching algorithm + * * correspondences between source and target data set + * * transformation matrix calculated based on the correspondences + * + */ + struct MatchingCandidate + { + /** \brief Constructor. */ + MatchingCandidate () : + fitness_score (FLT_MAX), + correspondences (), + transformation (Eigen::Matrix4f::Identity ()) + {}; + + /** \brief Value constructor. */ + MatchingCandidate (float s, const pcl::Correspondences &c, const Eigen::Matrix4f &m) : + fitness_score (s), + correspondences (c), + transformation (m) + {}; + + /** \brief Destructor. */ + ~MatchingCandidate () + {}; + + + /** \brief Fitness score of current candidate resulting from matching algorithm. */ + float fitness_score; + + /** \brief Correspondences between source <-> target. */ + pcl::Correspondences correspondences; + + /** \brief Corresponding transformation matrix retrieved using \a corrs. */ + Eigen::Matrix4f transformation; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Sorting of candidates based on fitness score value. */ + struct by_score + { + /** \brief Operator used to sort candidates based on fitness score. */ + bool operator () (MatchingCandidate const &left, MatchingCandidate const &right) + { + return (left.fitness_score < right.fitness_score); + } + }; + + }; // namespace registration +}; // namespace pcl + + +#endif // PCL_REGISTRATION_MATCHING_CANDIDATE_H_ diff --git a/registration/include/pcl/registration/transformation_estimation_3point.h b/registration/include/pcl/registration/transformation_estimation_3point.h new file mode 100644 index 00000000000..d124ef181c6 --- /dev/null +++ b/registration/include/pcl/registration/transformation_estimation_3point.h @@ -0,0 +1,145 @@ +/* + * 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. + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_ + +#include + +namespace pcl +{ + namespace registration + { + /** \brief TransformationEstimation3Points represents the class for transformation estimation based on: + * - correspondence vectors of 3 pairs (planar case) + * - two point clouds (source and target) of size 3 + * - a point cloud with a set of 3 indices (source), and another point cloud (target) + * - two point clouds with two sets of indices (source and target) of the size 3 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix + * (i.e., float or double). Default: float. + * + * \author P.W.Theiler + * \ingroup registration + */ + template + class TransformationEstimation3Point : public TransformationEstimation + { + public: + /** \cond */ + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + typedef typename TransformationEstimation::Matrix4 Matrix4; + /** \endcond */ + + /** \brief Constructor */ + TransformationEstimation3Point () + {}; + + /** \brief Destructor */ + virtual ~TransformationEstimation3Point () + {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + }; + }; // namespace registration +}; // namespace registration + +#include + +#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_ diff --git a/test/registration/CMakeLists.txt b/test/registration/CMakeLists.txt index bb4832bc004..dd6e85f7f95 100644 --- a/test/registration/CMakeLists.txt +++ b/test/registration/CMakeLists.txt @@ -21,3 +21,7 @@ PCL_ADD_TEST(correspondence_rejectors test_correspondence_rejectors LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features ARGUMENTS "${PCL_SOURCE_DIR}/test/bunny.pcd") +PCL_ADD_TEST(fpcs_ia test_fpcs_ia + FILES test_fpcs_ia.cpp test_fpcs_ia_data.h + LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree pcl_kdtree + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd") \ No newline at end of file diff --git a/test/registration/test_fpcs_ia.cpp b/test/registration/test_fpcs_ia.cpp new file mode 100644 index 00000000000..3c20c5f37fd --- /dev/null +++ b/test/registration/test_fpcs_ia.cpp @@ -0,0 +1,119 @@ +/* +* 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. +* +* $Id$ +* +*/ + +#include + +#include +#include +#include + +#include "test_fpcs_ia_data.h" + +using namespace pcl; +using namespace pcl::io; +using namespace pcl::registration; +using namespace std; + +PointCloud cloud_source, cloud_target; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, FPCSInitialAlignment) +{ + // transform the source cloud by a large amount + Eigen::Vector3f initial_offset (1.f, 0.f, 0.f); + float angle = static_cast (M_PI) / 2.f; + Eigen::Quaternionf initial_rotation (cos (angle / 2.f), 0, 0, sin (angle / 2.f)); + PointCloud cloud_source_transformed; + transformPointCloud (cloud_source, cloud_source_transformed, initial_offset, initial_rotation); + + // create shared pointers + PointCloud::Ptr cloud_source_ptr, cloud_target_ptr; + cloud_source_ptr = cloud_source_transformed.makeShared (); + cloud_target_ptr = cloud_target.makeShared (); + + // initialize fpcs + PointCloud source_aligned; + FPCSInitialAlignment fpcs_ia; + fpcs_ia.setInputSource (cloud_source_ptr); + fpcs_ia.setInputTarget (cloud_target_ptr); + + fpcs_ia.setNumberOfThreads (nr_threads); + fpcs_ia.setApproxOverlap (approx_overlap); + fpcs_ia.setDelta (delta, true); + fpcs_ia.setNumberOfSamples (nr_samples); + + // align + fpcs_ia.align (source_aligned); + EXPECT_EQ (static_cast (source_aligned.points.size ()), static_cast (cloud_source.points.size ())); + + // check for correct coarse transformation marix + //Eigen::Matrix4f transform_res_from_fpcs = fpcs_ia.getFinalTransformation (); + //for (int i = 0; i < 4; ++i) + // for (int j = 0; j < 4; ++j) + // EXPECT_NEAR (transform_res_from_fpcs (i,j), transform_from_fpcs[i][j], 0.5); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +int +main (int argc, char** argv) +{ + if (argc < 3) + { + std::cerr << "No test files given. Please download `bun0.pcd` and `bun4.pcd` pass their path to the test." << std::endl; + return (-1); + } + + // Input + if (loadPCDFile (argv[1], cloud_source) < 0) + { + std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; + return (-1); + } + if (loadPCDFile (argv[2], cloud_target) < 0) + { + std::cerr << "Failed to read test file. Please download `bun4.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/registration/test_fpcs_ia_data.h b/test/registration/test_fpcs_ia_data.h new file mode 100644 index 00000000000..8d7a85e78f9 --- /dev/null +++ b/test/registration/test_fpcs_ia_data.h @@ -0,0 +1,16 @@ +#ifndef TEST_FPCS_DATA_H_ +#define TEST_FPCS_DATA_H_ + +const int nr_threads = 1; +const float approx_overlap = 0.9f; +const float delta = 1.f; +const int nr_samples = 100; + +const float transform_from_fpcs [4][4] = { + { -0.0019f, 0.8266f, -0.5628f, -0.0378f }, + { -0.9999f, -0.0094f, -0.0104f, 0.9997f }, + { -0.0139f, 0.5627f, 0.8265f, 0.0521f }, + { 0.f, 0.f, 0.f, 1.f } +}; + +#endif // TEST_FPCS_DATA_H_