TEST (PCL, SampleConsensusInitialAlignment)
{
// Transform the source cloud by a large amount
Eigen::Vector3f initial_offset (100, 0, 0);
float angle = static_cast<float> (M_PI) / 2.0f;
Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2));
PointCloud<PointXYZ> cloud_source_transformed;
transformPointCloud (cloud_source, cloud_source_transformed, initial_offset, initial_rotation);
// Create shared pointers
PointCloud<PointXYZ>::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<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ>);
NormalEstimation<PointXYZ, Normal> norm_est;
norm_est.setSearchMethod (tree);
norm_est.setRadiusSearch (0.05);
PointCloud<Normal> normals;
FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
fpfh_est.setSearchMethod (tree);
fpfh_est.setRadiusSearch (0.05);
PointCloud<FPFHSignature33> 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<PointXYZ, PointXYZ, FPFHSignature33> reg;
reg.setMinSampleDistance (0.05f);
reg.setMaxCorrespondenceDistance (0.2);
reg.setMaximumIterations (1000);
reg.setInputCloud (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_EQ (reg.getFitnessScore () < 0.0005, true);
}
TEST (PCL, SampleConsensusInitialAlignment)
{
// Transform the source cloud by a large amount
Eigen::Vector3f initial_offset (100, 0, 0);
float angle = static_cast<float> (M_PI) / 2.0f;
Eigen::Quaternionf initial_rotation (cos (angle / 2), 0, 0, sin (angle / 2));
PointCloud<PointXYZ> cloud_source_transformed;
transformPointCloud (cloud_source, cloud_source_transformed, initial_offset, initial_rotation);
// Create shared pointers
PointCloud<PointXYZ>::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<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ>);
NormalEstimation<PointXYZ, Normal> norm_est;
norm_est.setSearchMethod (tree);
norm_est.setRadiusSearch (0.05);
PointCloud<Normal> normals;
FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
fpfh_est.setSearchMethod (tree);
fpfh_est.setRadiusSearch (0.05);
PointCloud<FPFHSignature33> 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<PointXYZ, PointXYZ, FPFHSignature33> 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<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
// 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<PointT>::Ptr tree_recip (new pcl::search::KdTree<PointT>);
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);
}
}