diff --git a/registration/include/pcl/registration/ia_kfpcs.h b/registration/include/pcl/registration/ia_kfpcs.h new file mode 100644 index 00000000000..39ac79ddb52 --- /dev/null +++ b/registration/include/pcl/registration/ia_kfpcs.h @@ -0,0 +1,263 @@ +/* + * 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 std::vector 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 new file mode 100644 index 00000000000..4b4ca3cce99 --- /dev/null +++ b/registration/include/pcl/registration/impl/ia_kfpcs.hpp @@ -0,0 +1,293 @@ +/* + * 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/registration/CMakeLists.txt b/test/registration/CMakeLists.txt index dd6e85f7f95..7a4dd2c7733 100644 --- a/test/registration/CMakeLists.txt +++ b/test/registration/CMakeLists.txt @@ -23,5 +23,10 @@ PCL_ADD_TEST(correspondence_rejectors test_correspondence_rejectors 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 + LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd") + +PCL_ADD_TEST(kfpcs_ia test_kfpcs_ia + FILES test_kfpcs_ia.cpp test_kfpcs_ia_data.h + LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree + ARGUMENTS "${PCL_SOURCE_DIR}/test/office1_keypoints.pcd" "${PCL_SOURCE_DIR}/test/office2_keypoints.pcd") \ No newline at end of file diff --git a/test/registration/test_kfpcs_ia.cpp b/test/registration/test_kfpcs_ia.cpp new file mode 100644 index 00000000000..ada65d29179 --- /dev/null +++ b/test/registration/test_kfpcs_ia.cpp @@ -0,0 +1,118 @@ +/* +* 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. +* +*/ + +#include + +#include +#include +#include + +#include "test_kfpcs_ia_data.h" + +using namespace pcl; +using namespace pcl::io; +using namespace pcl::registration; +using namespace std; + +PointCloud cloud_source, cloud_target; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, KFPCSInitialAlignment) +{ + // create shared pointers + PointCloud::Ptr cloud_source_ptr, cloud_target_ptr; + cloud_source_ptr = cloud_source.makeShared (); + cloud_target_ptr = cloud_target.makeShared (); + + // initialize k-fpcs + PointCloud source_aligned; + KFPCSInitialAlignment kfpcs_ia; + kfpcs_ia.setInputSource (cloud_source_ptr); + kfpcs_ia.setInputTarget (cloud_target_ptr); + + kfpcs_ia.setNumberOfThreads (nr_threads); + kfpcs_ia.setApproxOverlap (approx_overlap); + kfpcs_ia.setDelta (voxel_size, false); + kfpcs_ia.setScoreThreshold (abort_score); + + // align + kfpcs_ia.align (source_aligned); + EXPECT_EQ (static_cast (source_aligned.points.size ()), static_cast (cloud_source.points.size ())); + + // copy initial matrix + Eigen::Matrix4f transform_groundtruth; + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + transform_groundtruth (i, j) = transformation_office1_office2[i][j]; + + // check for correct transformation + Eigen::Matrix4f transform_rest = kfpcs_ia.getFinalTransformation ().colPivHouseholderQr ().solve (transform_groundtruth); + const float angle3d = Eigen::AngleAxisf (transform_rest.block <3, 3> (0, 0)).angle (); + const float translation3d = transform_rest.block <3, 1> (0, 3).norm (); + + EXPECT_NEAR (angle3d, 0.f, 0.1745f); // 10° + EXPECT_NEAR (translation3d, 0.f, 1.f); // 1m +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +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_kfpcs_ia_data.h b/test/registration/test_kfpcs_ia_data.h new file mode 100644 index 00000000000..e3f6e102eb9 --- /dev/null +++ b/test/registration/test_kfpcs_ia_data.h @@ -0,0 +1,16 @@ +#ifndef TEST_KFPCS_DATA_H_ +#define TEST_KFPCS_DATA_H_ + +const int nr_threads = 1; +const float voxel_size = 0.1f; +const float approx_overlap = 0.9f; +const float abort_score = 1.0f; + +const float transformation_office1_office2 [4][4] = { + { -0.6946f, -0.7194f, -0.0051f, -3.6352f }, + { 0.7194f, -0.6945f, -0.0100f, -2.3865f }, + { 0.0037f, -0.0106f, -0.9999f, 0.7778f }, + { 0.0000f, 0.0000f, 0.0000f, 1.0000f } +}; + +#endif // TEST_KFPCS_DATA_H_