pyramid_surface_matching.cpp
Go to the documentation of this file.
00001 #include <pcl/io/pcd_io.h>
00002 #include <pcl/filters/voxel_grid.h>
00003 #include <pcl/features/normal_3d.h>
00004 #include <pcl/features/ppf.h>
00005 #include <pcl/registration/pyramid_feature_matching.h>
00006 
00007 using namespace pcl;
00008 
00009 #include <iostream>
00010 using namespace std;
00011 
00012 const Eigen::Vector4f subsampling_leaf_size (0.02f, 0.02f, 0.02f, 0.0f);
00013 const float normal_estimation_search_radius = 0.05f;
00014 
00015 //const Eigen::Vector4f subsampling_leaf_size (5, 5, 5, 0.0);
00016 //const float normal_estimation_search_radius = 11;
00017 
00018 void
00019 subsampleAndCalculateNormals (PointCloud<PointXYZ>::Ptr &cloud,
00020                               PointCloud<PointXYZ>::Ptr &cloud_subsampled,
00021                               PointCloud<Normal>::Ptr &cloud_subsampled_normals)
00022 {
00023   cloud_subsampled = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ> ());
00024   VoxelGrid<PointXYZ> subsampling_filter;
00025   subsampling_filter.setInputCloud (cloud);
00026   subsampling_filter.setLeafSize (subsampling_leaf_size);
00027   subsampling_filter.filter (*cloud_subsampled);
00028 
00029   cloud_subsampled_normals = PointCloud<Normal>::Ptr (new PointCloud<Normal> ());
00030   NormalEstimation<PointXYZ, Normal> normal_estimation_filter;
00031   normal_estimation_filter.setInputCloud (cloud_subsampled);
00032   search::KdTree<PointXYZ>::Ptr search_tree (new search::KdTree<PointXYZ>);
00033   normal_estimation_filter.setSearchMethod (search_tree);
00034   normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
00035   normal_estimation_filter.compute (*cloud_subsampled_normals);
00036 
00037   cerr << "Before -> After subsampling: " << cloud->points.size () << " -> " << cloud_subsampled->points.size () << endl;
00038 }
00039 
00040 
00041 int
00042 main (int argc, char **argv)
00043 {
00044   if (argc != 3)
00045   {
00046     PCL_ERROR ("Syntax: ./pyramid_surface_matching model_1 model_2\n");
00047     return -1;
00048   }
00049 
00050 
00052   PCL_INFO ("Reading scene ...\n");
00053   PointCloud<PointXYZ>::Ptr cloud_a (new PointCloud<PointXYZ> ()),
00054       cloud_b (new PointCloud<PointXYZ> ());
00055   PCDReader reader;
00056   reader.read (argv[1], *cloud_a);
00057   reader.read (argv[2], *cloud_b);
00058 
00059   PointCloud<PointXYZ>::Ptr cloud_a_subsampled, cloud_b_subsampled;
00060   PointCloud<Normal>::Ptr cloud_a_subsampled_normals, cloud_b_subsampled_normals;
00061   subsampleAndCalculateNormals (cloud_a, cloud_a_subsampled, cloud_a_subsampled_normals);
00062   subsampleAndCalculateNormals (cloud_b, cloud_b_subsampled, cloud_b_subsampled_normals);
00063 
00064   PCL_INFO ("Finished subsampling the clouds ...\n");
00065 
00066 
00067   PointCloud<PPFSignature>::Ptr ppf_signature_a (new PointCloud<PPFSignature> ()),
00068       ppf_signature_b (new PointCloud<PPFSignature> ());
00069   PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals_a (new PointCloud<PointNormal> ()),
00070       cloud_subsampled_with_normals_b (new PointCloud<PointNormal> ());
00071   concatenateFields (*cloud_a_subsampled, *cloud_a_subsampled_normals, *cloud_subsampled_with_normals_a);
00072   concatenateFields (*cloud_b_subsampled, *cloud_b_subsampled_normals, *cloud_subsampled_with_normals_b);
00073 
00074   PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
00075   ppf_estimator.setInputCloud (cloud_subsampled_with_normals_a);
00076   ppf_estimator.setInputNormals (cloud_subsampled_with_normals_a);
00077   ppf_estimator.compute (*ppf_signature_a);
00078   ppf_estimator.setInputCloud (cloud_subsampled_with_normals_b);
00079   ppf_estimator.setInputNormals (cloud_subsampled_with_normals_b);
00080   ppf_estimator.compute (*ppf_signature_b);
00081 
00082   PCL_INFO ("Feature cloud sizes: %u , %u\n", ppf_signature_a->points.size (), ppf_signature_b->points.size ());
00083 
00084   PCL_INFO ("Finished calculating the features ...\n");
00085   vector<pair<float, float> > dim_range_input, dim_range_target;
00086   for (size_t i = 0; i < 3; ++i) dim_range_input.push_back (pair<float, float> (float (-M_PI), float (M_PI)));
00087   dim_range_input.push_back (pair<float, float> (0.0f, 1.0f));
00088   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));
00089   dim_range_target.push_back (pair<float, float> (0.0f, 50.0f));
00090 
00091 
00092   PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_a (new PyramidFeatureHistogram<PPFSignature> ());
00093   pyramid_a->setInputCloud (ppf_signature_a);
00094   pyramid_a->setInputDimensionRange (dim_range_input);
00095   pyramid_a->setTargetDimensionRange (dim_range_target);
00096   pyramid_a->compute ();
00097   PCL_INFO ("Done with the first pyramid\n");
00098 
00099   PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_b (new PyramidFeatureHistogram<PPFSignature> ());
00100   pyramid_b->setInputCloud (ppf_signature_b);
00101   pyramid_b->setInputDimensionRange (dim_range_input);
00102   pyramid_b->setTargetDimensionRange (dim_range_target);
00103   pyramid_b->compute ();
00104   PCL_INFO ("Done with the second pyramid\n");
00105 
00106   float value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_a, pyramid_b);
00107   PCL_INFO ("Surface comparison value between %s and %s is: %f\n", argv[1], argv[2], value);
00108 
00109 
00110   return 0;
00111 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:28