#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/ppf.h>
#include <pcl/registration/pyramid_feature_matching.h>
using namespace pcl;
#include <iostream>
using namespace std;
const Eigen::Vector4f subsampling_leaf_size (0.02f, 0.02f, 0.02f, 0.0f);
const float normal_estimation_search_radius = 0.05f;
//const Eigen::Vector4f subsampling_leaf_size (5, 5, 5, 0.0);
//const float normal_estimation_search_radius = 11;
void
subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
PointCloud<PointXYZ>::Ptr &cloud_subsampled,
PointCloud<Normal>::Ptr &cloud_subsampled_normals)
{
cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
VoxelGrid<PointXYZ> subsampling_filter;
subsampling_filter.setInputCloud (cloud);
subsampling_filter.setLeafSize (subsampling_leaf_size);
subsampling_filter.filter (*cloud_subsampled);
cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
normal_estimation_filter.setInputCloud (cloud_subsampled);
search::KdTree<PointXYZ>::Ptr search_tree (new search::KdTree<PointXYZ>);
normal_estimation_filter.setSearchMethod (search_tree);
normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
normal_estimation_filter.compute (*cloud_subsampled_normals);
cerr << "Before -> After subsampling: " << cloud->points.size () << " -> " << cloud_subsampled->points.size () << endl;
}
int
main (int argc, char **argv)
{
if (argc != 3)
{
PCL_ERROR ("Syntax: ./pyramid_surface_matching model_1 model_2\n");
return -1;
}
/// read point clouds from HDD
PCL_INFO ("Reading scene ...\n");
PointCloud<PointXYZ>::Ptr cloud_a (new PointCloud<PointXYZ> ()),
cloud_b (new PointCloud<PointXYZ> ());
PCDReader reader;
reader.read (argv[1], *cloud_a);
reader.read (argv[2], *cloud_b);
PointCloud<PointXYZ>::Ptr cloud_a_subsampled, cloud_b_subsampled;
PointCloud<Normal>::Ptr cloud_a_subsampled_normals, cloud_b_subsampled_normals;
subsampleAndCalculateNormals (cloud_a, cloud_a_subsampled, cloud_a_subsampled_normals);
subsampleAndCalculateNormals (cloud_b, cloud_b_subsampled, cloud_b_subsampled_normals);
PCL_INFO ("Finished subsampling the clouds ...\n");
PointCloud<PPFSignature>::Ptr ppf_signature_a (new PointCloud<PPFSignature> ()),
ppf_signature_b (new PointCloud<PPFSignature> ());
PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals_a (new PointCloud<PointNormal> ()),
cloud_subsampled_with_normals_b (new PointCloud<PointNormal> ());
concatenateFields (*cloud_a_subsampled, *cloud_a_subsampled_normals, *cloud_subsampled_with_normals_a);
concatenateFields (*cloud_b_subsampled, *cloud_b_subsampled_normals, *cloud_subsampled_with_normals_b);
PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
ppf_estimator.setInputCloud (cloud_subsampled_with_normals_a);
ppf_estimator.setInputNormals (cloud_subsampled_with_normals_a);
ppf_estimator.compute (*ppf_signature_a);
ppf_estimator.setInputCloud (cloud_subsampled_with_normals_b);
ppf_estimator.setInputNormals (cloud_subsampled_with_normals_b);
ppf_estimator.compute (*ppf_signature_b);
PCL_INFO ("Feature cloud sizes: %u , %u\n", ppf_signature_a->points.size (), ppf_signature_b->points.size ());
PCL_INFO ("Finished calculating the features ...\n");
vector<pair<float, float> > dim_range_input, dim_range_target;
for (size_t i = 0; i < 3; ++i) dim_range_input.push_back (pair<float, float> (float (-M_PI), float (M_PI)));
dim_range_input.push_back (pair<float, float> (0.0f, 1.0f));
for (size_t i = 0; i < 3; ++i) dim_range_target.push_back (pair<float, float> (float (-M_PI) * 10.0f, float (M_PI) * 10.0f));
dim_range_target.push_back (pair<float, float> (0.0f, 50.0f));
PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_a (new PyramidFeatureHistogram<PPFSignature> ());
pyramid_a->setInputCloud (ppf_signature_a);
pyramid_a->setInputDimensionRange (dim_range_input);
pyramid_a->setTargetDimensionRange (dim_range_target);
pyramid_a->compute ();
PCL_INFO ("Done with the first pyramid\n");
PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_b (new PyramidFeatureHistogram<PPFSignature> ());
pyramid_b->setInputCloud (ppf_signature_b);
pyramid_b->setInputDimensionRange (dim_range_input);
pyramid_b->setTargetDimensionRange (dim_range_target);
pyramid_b->compute ();
PCL_INFO ("Done with the second pyramid\n");
float value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_a, pyramid_b);
PCL_INFO ("Surface comparison value between %s and %s is: %f\n", argv[1], argv[2], value);
return 0;
}
PCL(45)PPF点对特征
最新推荐文章于 2023-05-31 14:57:07 发布