Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef OBJECT_RECOGNITION_H_ 00002 #define OBJECT_RECOGNITION_H_ 00003 00004 #include "typedefs.h" 00005 00006 #include "solution/filters.h" 00007 #include "solution/segmentation.h" 00008 #include "solution/feature_estimation.h" 00009 #include "solution/registration.h" 00010 00011 #include <pcl/io/pcd_io.h> 00012 #include <pcl/kdtree/kdtree_flann.h> 00013 00014 00015 struct ObjectRecognitionParameters 00016 { 00017 // Filter parameters 00018 float min_depth; 00019 float max_depth; 00020 float downsample_leaf_size; 00021 float outlier_rejection_radius; 00022 int outlier_rejection_min_neighbors; 00023 00024 // Segmentation parameters 00025 float plane_inlier_distance_threshold; 00026 int max_ransac_iterations; 00027 float cluster_tolerance; 00028 int min_cluster_size; 00029 int max_cluster_size; 00030 00031 // Feature estimation parameters 00032 float surface_normal_radius; 00033 float keypoints_min_scale; 00034 float keypoints_nr_octaves; 00035 float keypoints_nr_scales_per_octave; 00036 float keypoints_min_contrast; 00037 float local_descriptor_radius; 00038 00039 // Registration parameters 00040 float initial_alignment_min_sample_distance; 00041 float initial_alignment_max_correspondence_distance; 00042 int initial_alignment_nr_iterations; 00043 float icp_max_correspondence_distance; 00044 float icp_outlier_rejection_threshold; 00045 float icp_transformation_epsilon; 00046 int icp_max_iterations; 00047 }; 00048 00049 struct ObjectModel 00050 { 00051 PointCloudPtr points; 00052 PointCloudPtr keypoints; 00053 LocalDescriptorsPtr local_descriptors; 00054 GlobalDescriptorsPtr global_descriptor; 00055 }; 00056 00057 class ObjectRecognition 00058 { 00059 public: 00060 ObjectRecognition (const ObjectRecognitionParameters & params) : params_ (params) 00061 {} 00062 00063 void 00064 populateDatabase (const std::vector<std::string> & filenames) 00065 { 00066 } 00067 00068 const ObjectModel & 00069 recognizeObject (const PointCloudPtr & query_cloud) 00070 { 00071 int best_match = 0; 00072 return (models_[best_match]); 00073 } 00074 00075 PointCloudPtr 00076 recognizeAndAlignPoints (const PointCloudPtr & query_cloud) 00077 { 00078 PointCloudPtr output; 00079 return (output); 00080 } 00081 00082 /* Construct an object model by filtering, segmenting, and estimating feature descriptors */ 00083 void 00084 constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const 00085 { 00086 output.points = applyFiltersAndSegment (points, params_); 00087 00088 SurfaceNormalsPtr normals; 00089 estimateFeatures (output.points, params_, normals, output.keypoints, 00090 output.local_descriptors, output.global_descriptor); 00091 } 00092 00093 protected: 00094 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */ 00095 PointCloudPtr 00096 applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const 00097 { 00098 PointCloudPtr cloud; 00099 cloud = thresholdDepth (input, params.min_depth, params.max_depth); 00100 cloud = downsample (cloud, params.downsample_leaf_size); 00101 cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors); 00102 00103 cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations); 00104 std::vector<pcl::PointIndices> cluster_indices; 00105 clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size, 00106 params.max_cluster_size, cluster_indices); 00107 00108 PointCloudPtr largest_cluster (new PointCloud); 00109 pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster); 00110 00111 return (largest_cluster); 00112 } 00113 00114 /* Estimate surface normals, keypoints, and local/global feature descriptors */ 00115 void 00116 estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params, 00117 SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out, 00118 LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const 00119 { 00120 normals_out = estimateSurfaceNormals (points, params.surface_normal_radius); 00121 00122 keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves, 00123 params.keypoints_nr_scales_per_octave, params.keypoints_min_contrast); 00124 00125 local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out, 00126 params.local_descriptor_radius); 00127 00128 global_descriptor_out = computeGlobalDescriptor (points, normals_out); 00129 } 00130 00131 /* Align the points in the source model to the points in the target model */ 00132 PointCloudPtr 00133 alignModelPoints (const ObjectModel & source, const ObjectModel & target, 00134 const ObjectRecognitionParameters & params) const 00135 { 00136 Eigen::Matrix4f tform; 00137 tform = computeInitialAlignment (source.keypoints, source.local_descriptors, 00138 target.keypoints, target.local_descriptors, 00139 params.initial_alignment_min_sample_distance, 00140 params.initial_alignment_max_correspondence_distance, 00141 params.initial_alignment_nr_iterations); 00142 00143 tform = refineAlignment (source.points, target.points, tform, 00144 params.icp_max_correspondence_distance, params.icp_outlier_rejection_threshold, 00145 params.icp_transformation_epsilon, params.icp_max_iterations); 00146 00147 PointCloudPtr output (new PointCloud); 00148 pcl::transformPointCloud (*(source.points), *output, tform); 00149 00150 return (output); 00151 } 00152 00153 ObjectRecognitionParameters params_; 00154 std::vector<ObjectModel> models_; 00155 GlobalDescriptorsPtr descriptors_; 00156 pcl::KdTreeFLANN<GlobalDescriptorT>::Ptr kdtree_; 00157 }; 00158 00159 #endif