From 3e3a61ebf47195b2ebcd6f7066ef465700cdb70a Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Wed, 19 Dec 2018 15:43:04 +0100 Subject: [PATCH 1/2] Extract NormalDistributionTransform test into a separate file --- test/registration/CMakeLists.txt | 5 + test/registration/test_ndt.cpp | 119 ++++++++++++++++++++++++ test/registration/test_registration.cpp | 47 +--------- 3 files changed, 125 insertions(+), 46 deletions(-) create mode 100644 test/registration/test_ndt.cpp diff --git a/test/registration/CMakeLists.txt b/test/registration/CMakeLists.txt index ca268be1a58..a340c9c363d 100644 --- a/test/registration/CMakeLists.txt +++ b/test/registration/CMakeLists.txt @@ -23,6 +23,11 @@ if(build) 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" "${PCL_SOURCE_DIR}/test/milk_color.pcd") + PCL_ADD_TEST(ndt test_ndt + FILES test_ndt.cpp + LINK_WITH pcl_gtest pcl_io pcl_registration + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd") + PCL_ADD_TEST(registration_api test_registration_api FILES test_registration_api.cpp test_registration_api_data.h LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_kdtree diff --git a/test/registration/test_ndt.cpp b/test/registration/test_ndt.cpp new file mode 100644 index 00000000000..ad98174b664 --- /dev/null +++ b/test/registration/test_ndt.cpp @@ -0,0 +1,119 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2011, Willow Garage, Inc. +* Copyright (c) 2018-, 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 + +using namespace pcl; +using namespace pcl::io; + +PointCloud cloud_source, cloud_target; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, NormalDistributionsTransform) +{ + typedef PointNormal PointT; + PointCloud::Ptr src (new PointCloud); + copyPointCloud (cloud_source, *src); + PointCloud::Ptr tgt (new PointCloud); + copyPointCloud (cloud_target, *tgt); + PointCloud output; + + NormalDistributionsTransform reg; + reg.setStepSize (0.05); + reg.setResolution (0.025f); + reg.setInputSource (src); + reg.setInputTarget (tgt); + reg.setMaximumIterations (50); + reg.setTransformationEpsilon (1e-8); + // Register + reg.align (output); + EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); + EXPECT_LT (reg.getFitnessScore (), 0.001); + + // Check again, for all possible caching schemes + for (int iter = 0; iter < 4; iter++) + { + bool force_cache = (bool) iter/2; + bool force_cache_reciprocal = (bool) iter%2; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // Ensure that, when force_cache is not set, we are robust to the wrong input + if (force_cache) + tree->setInputCloud (tgt); + reg.setSearchMethodTarget (tree, force_cache); + + pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); + if (force_cache_reciprocal) + tree_recip->setInputCloud (src); + reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); + + // Register + reg.align (output); + EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); + EXPECT_LT (reg.getFitnessScore (), 0.001); + } +} + +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); + } + + 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_registration.cpp b/test/registration/test_registration.cpp index 44a9081825e..1cbbf68ef5c 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -59,8 +59,8 @@ #include #include #include -#include #include +#include // We need Histogram<2> to function, so we'll explicitly add kdtree_flann.hpp here #include //(pcl::Histogram<2>) @@ -592,51 +592,6 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, NormalDistributionsTransform) -{ - typedef PointNormal PointT; - PointCloud::Ptr src (new PointCloud); - copyPointCloud (cloud_source, *src); - PointCloud::Ptr tgt (new PointCloud); - copyPointCloud (cloud_target, *tgt); - PointCloud output; - - NormalDistributionsTransform reg; - reg.setStepSize (0.05); - reg.setResolution (0.025f); - reg.setInputSource (src); - reg.setInputTarget (tgt); - reg.setMaximumIterations (50); - reg.setTransformationEpsilon (1e-8); - // Register - reg.align (output); - EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); - EXPECT_LT (reg.getFitnessScore (), 0.001); - - // Check again, for all possible caching schemes - for (int iter = 0; iter < 4; iter++) - { - bool force_cache = (bool) iter/2; - bool force_cache_reciprocal = (bool) iter%2; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - // Ensure that, when force_cache is not set, we are robust to the wrong input - if (force_cache) - tree->setInputCloud (tgt); - reg.setSearchMethodTarget (tree, force_cache); - - pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); - if (force_cache_reciprocal) - tree_recip->setInputCloud (src); - reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); - - // Register - reg.align (output); - EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); - EXPECT_LT (reg.getFitnessScore (), 0.001); - } -} - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, SampleConsensusInitialAlignment) From 214775f8eddd1a622dce4f6d49f6200805f719f3 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Wed, 19 Dec 2018 15:55:16 +0100 Subject: [PATCH 2/2] Extract SampleConsensusInitialAlignment and SampleConsensusPrerejective tests into a separate file --- test/registration/CMakeLists.txt | 7 +- test/registration/test_registration.cpp | 186 ----------------- test/registration/test_sac_ia.cpp | 261 ++++++++++++++++++++++++ 3 files changed, 267 insertions(+), 187 deletions(-) create mode 100644 test/registration/test_sac_ia.cpp diff --git a/test/registration/CMakeLists.txt b/test/registration/CMakeLists.txt index a340c9c363d..cec59df07fc 100644 --- a/test/registration/CMakeLists.txt +++ b/test/registration/CMakeLists.txt @@ -20,7 +20,7 @@ if(build) if(BUILD_io) PCL_ADD_TEST(a_registration_test test_registration FILES test_registration.cpp - LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree pcl_kdtree + 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_SOURCE_DIR}/test/milk_color.pcd") PCL_ADD_TEST(ndt test_ndt @@ -28,6 +28,11 @@ if(build) LINK_WITH pcl_gtest pcl_io pcl_registration ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd") + PCL_ADD_TEST(sac_ia test_sac_ia + FILES test_sac_ia.cpp + LINK_WITH pcl_gtest pcl_io pcl_registration + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd") + PCL_ADD_TEST(registration_api test_registration_api FILES test_registration_api.cpp test_registration_api_data.h LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_kdtree diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 1cbbf68ef5c..be3359f3c8d 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -55,11 +54,9 @@ #include #include #include -#include #include #include #include -#include #include // We need Histogram<2> to function, so we'll explicitly add kdtree_flann.hpp here #include @@ -593,189 +590,6 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, SampleConsensusInitialAlignment) -{ - // Transform the source cloud by a large amount - Eigen::Vector3f initial_offset (100, 0, 0); - float angle = static_cast (M_PI) / 2.0f; - Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2)); - 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 estimators for surface normals and FPFH features - search::KdTree::Ptr tree (new search::KdTree); - - NormalEstimation norm_est; - norm_est.setSearchMethod (tree); - norm_est.setRadiusSearch (0.05); - PointCloud normals; - - FPFHEstimation fpfh_est; - fpfh_est.setSearchMethod (tree); - fpfh_est.setRadiusSearch (0.05); - PointCloud features_source, features_target; - - // Estimate the FPFH features for the source cloud - norm_est.setInputCloud (cloud_source_ptr); - norm_est.compute (normals); - fpfh_est.setInputCloud (cloud_source_ptr); - fpfh_est.setInputNormals (normals.makeShared ()); - fpfh_est.compute (features_source); - - // Estimate the FPFH features for the target cloud - norm_est.setInputCloud (cloud_target_ptr); - norm_est.compute (normals); - fpfh_est.setInputCloud (cloud_target_ptr); - fpfh_est.setInputNormals (normals.makeShared ()); - fpfh_est.compute (features_target); - - // Initialize Sample Consensus Initial Alignment (SAC-IA) - SampleConsensusInitialAlignment reg; - reg.setMinSampleDistance (0.05f); - reg.setMaxCorrespondenceDistance (0.1); - reg.setMaximumIterations (1000); - - reg.setInputSource (cloud_source_ptr); - reg.setInputTarget (cloud_target_ptr); - reg.setSourceFeatures (features_source.makeShared ()); - reg.setTargetFeatures (features_target.makeShared ()); - - // Register - reg.align (cloud_reg); - EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); - EXPECT_LT (reg.getFitnessScore (), 0.0005); - - // Check again, for all possible caching schemes - typedef pcl::PointXYZ PointT; - for (int iter = 0; iter < 4; iter++) - { - bool force_cache = (bool) iter/2; - bool force_cache_reciprocal = (bool) iter%2; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - // Ensure that, when force_cache is not set, we are robust to the wrong input - if (force_cache) - tree->setInputCloud (cloud_target_ptr); - reg.setSearchMethodTarget (tree, force_cache); - - pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); - if (force_cache_reciprocal) - tree_recip->setInputCloud (cloud_source_ptr); - reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); - - // Register - reg.align (cloud_reg); - EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); - EXPECT_LT (reg.getFitnessScore (), 0.0005); - } -} - - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, SampleConsensusPrerejective) -{ - /* - * This test is a near-exact copy of the SampleConsensusInitialAlignment test, - * with the only modifications that: - * 1) the number of iterations is increased 1000 --> 5000 - * 2) the feature correspondence randomness (the number of kNNs) is decreased 10 --> 2 - */ - - // Transform the source cloud by a large amount - Eigen::Vector3f initial_offset (100, 0, 0); - float angle = static_cast (M_PI) / 2.0f; - Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2)); - 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 estimators for surface normals and FPFH features - search::KdTree::Ptr tree (new search::KdTree); - - // Normal estimator - NormalEstimation norm_est; - norm_est.setSearchMethod (tree); - norm_est.setRadiusSearch (0.005); - PointCloud normals; - - // FPFH estimator - FPFHEstimation fpfh_est; - fpfh_est.setSearchMethod (tree); - fpfh_est.setRadiusSearch (0.05); - PointCloud features_source, features_target; - - // Estimate the normals and the FPFH features for the source cloud - norm_est.setInputCloud (cloud_source_ptr); - norm_est.compute (normals); - fpfh_est.setInputCloud (cloud_source_ptr); - fpfh_est.setInputNormals (normals.makeShared ()); - fpfh_est.compute (features_source); - - // Estimate the normals and the FPFH features for the target cloud - norm_est.setInputCloud (cloud_target_ptr); - norm_est.compute (normals); - fpfh_est.setInputCloud (cloud_target_ptr); - fpfh_est.setInputNormals (normals.makeShared ()); - fpfh_est.compute (features_target); - - // Initialize Sample Consensus Prerejective with 5x the number of iterations and 1/5 feature kNNs as SAC-IA - SampleConsensusPrerejective reg; - reg.setMaxCorrespondenceDistance (0.1); - reg.setMaximumIterations (5000); - reg.setSimilarityThreshold (0.6f); - reg.setCorrespondenceRandomness (2); - - // Set source and target cloud/features - reg.setInputSource (cloud_source_ptr); - reg.setInputTarget (cloud_target_ptr); - reg.setSourceFeatures (features_source.makeShared ()); - reg.setTargetFeatures (features_target.makeShared ()); - - // Register - reg.align (cloud_reg); - - // Check output consistency and quality of alignment - EXPECT_EQ (static_cast (cloud_reg.points.size ()), static_cast (cloud_source.points.size ())); - float inlier_fraction = static_cast (reg.getInliers ().size ()) / static_cast (cloud_source.points.size ()); - EXPECT_GT (inlier_fraction, 0.95f); - - // Check again, for all possible caching schemes - typedef pcl::PointXYZ PointT; - for (int iter = 0; iter < 4; iter++) - { - bool force_cache = (bool) iter/2; - bool force_cache_reciprocal = (bool) iter%2; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - // Ensure that, when force_cache is not set, we are robust to the wrong input - if (force_cache) - tree->setInputCloud (cloud_target_ptr); - reg.setSearchMethodTarget (tree, force_cache); - - pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); - if (force_cache_reciprocal) - tree_recip->setInputCloud (cloud_source_ptr); - reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); - - // Register - reg.align (cloud_reg); - - // Check output consistency and quality of alignment - EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); - inlier_fraction = static_cast (reg.getInliers ().size ()) / static_cast (cloud_source.points.size ()); - EXPECT_GT (inlier_fraction, 0.95f); - } -} - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, PyramidFeatureHistogram) { diff --git a/test/registration/test_sac_ia.cpp b/test/registration/test_sac_ia.cpp new file mode 100644 index 00000000000..12c205ee08c --- /dev/null +++ b/test/registration/test_sac_ia.cpp @@ -0,0 +1,261 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2011, Willow Garage, Inc. +* Copyright (c) 2018-, 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 +#include +#include + +using namespace pcl; +using namespace pcl::io; + +PointCloud cloud_source, cloud_target, cloud_reg; + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, SampleConsensusInitialAlignment) +{ + // Transform the source cloud by a large amount + Eigen::Vector3f initial_offset (100, 0, 0); + float angle = static_cast (M_PI) / 2.0f; + Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2)); + 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 estimators for surface normals and FPFH features + search::KdTree::Ptr tree (new search::KdTree); + + NormalEstimation norm_est; + norm_est.setSearchMethod (tree); + norm_est.setRadiusSearch (0.05); + PointCloud normals; + + FPFHEstimation fpfh_est; + fpfh_est.setSearchMethod (tree); + fpfh_est.setRadiusSearch (0.05); + PointCloud features_source, features_target; + + // Estimate the FPFH features for the source cloud + norm_est.setInputCloud (cloud_source_ptr); + norm_est.compute (normals); + fpfh_est.setInputCloud (cloud_source_ptr); + fpfh_est.setInputNormals (normals.makeShared ()); + fpfh_est.compute (features_source); + + // Estimate the FPFH features for the target cloud + norm_est.setInputCloud (cloud_target_ptr); + norm_est.compute (normals); + fpfh_est.setInputCloud (cloud_target_ptr); + fpfh_est.setInputNormals (normals.makeShared ()); + fpfh_est.compute (features_target); + + // Initialize Sample Consensus Initial Alignment (SAC-IA) + SampleConsensusInitialAlignment reg; + reg.setMinSampleDistance (0.05f); + reg.setMaxCorrespondenceDistance (0.1); + reg.setMaximumIterations (1000); + + reg.setInputSource (cloud_source_ptr); + reg.setInputTarget (cloud_target_ptr); + reg.setSourceFeatures (features_source.makeShared ()); + reg.setTargetFeatures (features_target.makeShared ()); + + // Register + reg.align (cloud_reg); + EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); + EXPECT_LT (reg.getFitnessScore (), 0.0005); + + // Check again, for all possible caching schemes + typedef pcl::PointXYZ PointT; + for (int iter = 0; iter < 4; iter++) + { + bool force_cache = (bool) iter/2; + bool force_cache_reciprocal = (bool) iter%2; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // Ensure that, when force_cache is not set, we are robust to the wrong input + if (force_cache) + tree->setInputCloud (cloud_target_ptr); + reg.setSearchMethodTarget (tree, force_cache); + + pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); + if (force_cache_reciprocal) + tree_recip->setInputCloud (cloud_source_ptr); + reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); + + // Register + reg.align (cloud_reg); + EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); + EXPECT_LT (reg.getFitnessScore (), 0.0005); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, SampleConsensusPrerejective) +{ + /* + * This test is a near-exact copy of the SampleConsensusInitialAlignment test, + * with the only modifications that: + * 1) the number of iterations is increased 1000 --> 5000 + * 2) the feature correspondence randomness (the number of kNNs) is decreased 10 --> 2 + */ + + // Transform the source cloud by a large amount + Eigen::Vector3f initial_offset (100, 0, 0); + float angle = static_cast (M_PI) / 2.0f; + Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2)); + 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 estimators for surface normals and FPFH features + search::KdTree::Ptr tree (new search::KdTree); + + // Normal estimator + NormalEstimation norm_est; + norm_est.setSearchMethod (tree); + norm_est.setRadiusSearch (0.005); + PointCloud normals; + + // FPFH estimator + FPFHEstimation fpfh_est; + fpfh_est.setSearchMethod (tree); + fpfh_est.setRadiusSearch (0.05); + PointCloud features_source, features_target; + + // Estimate the normals and the FPFH features for the source cloud + norm_est.setInputCloud (cloud_source_ptr); + norm_est.compute (normals); + fpfh_est.setInputCloud (cloud_source_ptr); + fpfh_est.setInputNormals (normals.makeShared ()); + fpfh_est.compute (features_source); + + // Estimate the normals and the FPFH features for the target cloud + norm_est.setInputCloud (cloud_target_ptr); + norm_est.compute (normals); + fpfh_est.setInputCloud (cloud_target_ptr); + fpfh_est.setInputNormals (normals.makeShared ()); + fpfh_est.compute (features_target); + + // Initialize Sample Consensus Prerejective with 5x the number of iterations and 1/5 feature kNNs as SAC-IA + SampleConsensusPrerejective reg; + reg.setMaxCorrespondenceDistance (0.1); + reg.setMaximumIterations (5000); + reg.setSimilarityThreshold (0.6f); + reg.setCorrespondenceRandomness (2); + + // Set source and target cloud/features + reg.setInputSource (cloud_source_ptr); + reg.setInputTarget (cloud_target_ptr); + reg.setSourceFeatures (features_source.makeShared ()); + reg.setTargetFeatures (features_target.makeShared ()); + + // Register + reg.align (cloud_reg); + + // Check output consistency and quality of alignment + EXPECT_EQ (static_cast (cloud_reg.points.size ()), static_cast (cloud_source.points.size ())); + float inlier_fraction = static_cast (reg.getInliers ().size ()) / static_cast (cloud_source.points.size ()); + EXPECT_GT (inlier_fraction, 0.95f); + + // Check again, for all possible caching schemes + typedef pcl::PointXYZ PointT; + for (int iter = 0; iter < 4; iter++) + { + bool force_cache = (bool) iter/2; + bool force_cache_reciprocal = (bool) iter%2; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // Ensure that, when force_cache is not set, we are robust to the wrong input + if (force_cache) + tree->setInputCloud (cloud_target_ptr); + reg.setSearchMethodTarget (tree, force_cache); + + pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); + if (force_cache_reciprocal) + tree_recip->setInputCloud (cloud_source_ptr); + reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); + + // Register + reg.align (cloud_reg); + + // Check output consistency and quality of alignment + EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); + inlier_fraction = static_cast (reg.getInliers ().size ()) / static_cast (cloud_source.points.size ()); + EXPECT_GT (inlier_fraction, 0.95f); + } +} + +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 ()); +}