Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef FEATURE_ESTIMATION_H 00002 #define FEATURE_ESTIMATION_H 00003 00004 #include "typedefs.h" 00005 00006 #include <pcl/io/io.h> 00007 #include <pcl/features/normal_3d.h> 00008 #include <pcl/keypoints/sift_keypoint.h> 00009 #include <pcl/features/fpfh.h> 00010 #include <pcl/features/vfh.h> 00011 #include <pcl/search/kdtree.h> 00012 00013 /* Use NormalEstimation to estimate a cloud's surface normals 00014 * Inputs: 00015 * input 00016 * The input point cloud 00017 * radius 00018 * The size of the local neighborhood used to estimate the surface 00019 * Return: A pointer to a SurfaceNormals point cloud 00020 */ 00021 SurfaceNormalsPtr 00022 estimateSurfaceNormals (const PointCloudPtr & input, float radius) 00023 { 00024 pcl::NormalEstimation<PointT, NormalT> normal_estimation; 00025 normal_estimation.setSearchMethod (pcl::search::KdTree<PointT>::Ptr (new pcl::search::KdTree<PointT>)); 00026 normal_estimation.setRadiusSearch (radius); 00027 normal_estimation.setInputCloud (input); 00028 SurfaceNormalsPtr normals (new SurfaceNormals); 00029 normal_estimation.compute (*normals); 00030 00031 return (normals); 00032 } 00033 00034 /* Use SIFTKeypoint to detect a set of keypoints 00035 * Inputs: 00036 * points 00037 * The input point cloud 00038 * normals 00039 * The input surface normals 00040 * min_scale 00041 * The smallest scale in the difference-of-Gaussians (DoG) scale-space 00042 * nr_octaves 00043 * The number of times the scale doubles in the DoG scale-space 00044 * nr_scales_per_octave 00045 * The number of scales computed for each doubling 00046 * min_contrast 00047 * The minimum local contrast that must be present for a keypoint to be detected 00048 * Return: A pointer to a point cloud of keypoints 00049 */ 00050 PointCloudPtr 00051 detectKeypoints (const PointCloudPtr & points, const SurfaceNormalsPtr & normals, 00052 float min_scale, int nr_octaves, int nr_scales_per_octave, float min_contrast) 00053 { 00054 pcl::SIFTKeypoint<PointT, pcl::PointWithScale> sift_detect; 00055 sift_detect.setSearchMethod (pcl::search::KdTree<PointT>::Ptr (new pcl::search::KdTree<PointT>)); 00056 sift_detect.setScales (min_scale, nr_octaves, nr_scales_per_octave); 00057 sift_detect.setMinimumContrast (min_contrast); 00058 sift_detect.setInputCloud (points); 00059 pcl::PointCloud<pcl::PointWithScale> keypoints_temp; 00060 sift_detect.compute (keypoints_temp); 00061 PointCloudPtr keypoints (new PointCloud); 00062 pcl::copyPointCloud (keypoints_temp, *keypoints); 00063 00064 return (keypoints); 00065 } 00066 00067 /* Use FPFHEstimation to compute local feature descriptors around each keypoint 00068 * Inputs: 00069 * points 00070 * The input point cloud 00071 * normals 00072 * The input surface normals 00073 * keypoints 00074 * A cloud of keypoints specifying the positions at which the descriptors should be computed 00075 * feature_radius 00076 * The size of the neighborhood from which the local descriptors will be computed 00077 * Return: A pointer to a LocalDescriptors (a cloud of LocalDescriptorT points) 00078 */ 00079 LocalDescriptorsPtr 00080 computeLocalDescriptors (const PointCloudPtr & points, const SurfaceNormalsPtr & normals, 00081 const PointCloudPtr & keypoints, float feature_radius) 00082 { 00083 pcl::FPFHEstimation<PointT, NormalT, LocalDescriptorT> fpfh_estimation; 00084 fpfh_estimation.setSearchMethod (pcl::search::KdTree<PointT>::Ptr (new pcl::search::KdTree<PointT>)); 00085 fpfh_estimation.setRadiusSearch (feature_radius); 00086 fpfh_estimation.setSearchSurface (points); 00087 fpfh_estimation.setInputNormals (normals); 00088 fpfh_estimation.setInputCloud (keypoints); 00089 LocalDescriptorsPtr local_descriptors (new LocalDescriptors); 00090 fpfh_estimation.compute (*local_descriptors); 00091 00092 return (local_descriptors); 00093 } 00094 00095 /* Use VFHEstimation to compute a single global descriptor for the entire input cloud 00096 * Inputs: 00097 * points 00098 * The input point cloud 00099 * normals 00100 * The input surface normals 00101 * Return: A pointer to a GlobalDescriptors point cloud (a cloud containing a single GlobalDescriptorT point) 00102 */ 00103 GlobalDescriptorsPtr 00104 computeGlobalDescriptor (const PointCloudPtr & points, const SurfaceNormalsPtr & normals) 00105 { 00106 pcl::VFHEstimation<PointT, NormalT, GlobalDescriptorT> vfh_estimation; 00107 vfh_estimation.setSearchMethod (pcl::search::KdTree<PointT>::Ptr (new pcl::search::KdTree<PointT>)); 00108 vfh_estimation.setInputCloud (points); 00109 vfh_estimation.setInputNormals (normals); 00110 GlobalDescriptorsPtr global_descriptor (new GlobalDescriptors); 00111 vfh_estimation.compute (*global_descriptor); 00112 00113 return (global_descriptor); 00114 } 00115 00116 /* A simple structure for storing all of a cloud's features */ 00117 struct ObjectFeatures 00118 { 00119 PointCloudPtr points; 00120 SurfaceNormalsPtr normals; 00121 PointCloudPtr keypoints; 00122 LocalDescriptorsPtr local_descriptors; 00123 GlobalDescriptorsPtr global_descriptor; 00124 }; 00125 00126 /* Estimate normals, detect keypoints, and compute local and global descriptors 00127 * Return: An ObjectFeatures struct containing all the features 00128 */ 00129 ObjectFeatures 00130 computeFeatures (const PointCloudPtr & input) 00131 { 00132 ObjectFeatures features; 00133 features.points = input; 00134 features.normals = estimateSurfaceNormals (input, 0.05); 00135 features.keypoints = detectKeypoints (input, features.normals, 0.005, 10, 8, 1.5); 00136 features.local_descriptors = computeLocalDescriptors (input, features.normals, features.keypoints, 0.1); 00137 features.global_descriptor = computeGlobalDescriptor (input, features.normals); 00138 00139 return (features); 00140 } 00141 00142 #endif