Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef OBJECT_RECOGNITION_H_ 00002 #define OBJECT_RECOGNITION_H_ 00003 00004 #include "typedefs.h" 00005 #include "load_clouds.h" 00006 #include "filters.h" 00007 #include "segmentation.h" 00008 #include "feature_estimation.h" 00009 #include "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 size_t n = filenames.size (); 00067 models_.resize (n); 00068 descriptors_ = GlobalDescriptorsPtr (new GlobalDescriptors); 00069 for (size_t i = 0; i < n; ++i) 00070 { 00071 const std::string & filename = filenames[i]; 00072 if (filename.compare (filename.size ()-4, 4, ".pcd") == 0) 00073 { 00074 // If filename ends pcd extension, load the points and process them 00075 PointCloudPtr raw_input (new PointCloud); 00076 pcl::io::loadPCDFile (filenames[i], *raw_input); 00077 00078 constructObjectModel (raw_input, models_[i]); 00079 } 00080 else 00081 { 00082 // If the filename has no extension, load the pre-computed models 00083 models_[i].points = loadPointCloud<PointT> (filename, "_points.pcd"); 00084 models_[i].keypoints = loadPointCloud<PointT> (filename, "_keypoints.pcd"); 00085 models_[i].local_descriptors = loadPointCloud<LocalDescriptorT> (filename, "_localdesc.pcd"); 00086 models_[i].global_descriptor = loadPointCloud<GlobalDescriptorT> (filename, "_globaldesc.pcd"); 00087 } 00088 *descriptors_ += *(models_[i].global_descriptor); 00089 } 00090 kdtree_ = pcl::KdTreeFLANN<GlobalDescriptorT>::Ptr (new pcl::KdTreeFLANN<GlobalDescriptorT>); 00091 kdtree_->setInputCloud (descriptors_); 00092 } 00093 00094 const ObjectModel & 00095 recognizeObject (const PointCloudPtr & query_cloud) 00096 { 00097 ObjectModel query_object; 00098 constructObjectModel (query_cloud, query_object); 00099 const GlobalDescriptorT & query_descriptor = query_object.global_descriptor->points[0]; 00100 00101 std::vector<int> nn_index (1); 00102 std::vector<float> nn_sqr_distance (1); 00103 kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance); 00104 const int & best_match = nn_index[0]; 00105 00106 return (models_[best_match]); 00107 } 00108 00109 PointCloudPtr 00110 recognizeAndAlignPoints (const PointCloudPtr & query_cloud) 00111 { 00112 ObjectModel query_object; 00113 constructObjectModel (query_cloud, query_object); 00114 const GlobalDescriptorT & query_descriptor = query_object.global_descriptor->points[0]; 00115 00116 std::vector<int> nn_index (1); 00117 std::vector<float> nn_sqr_distance (1); 00118 kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance); 00119 const int & best_match = nn_index[0]; 00120 00121 PointCloudPtr output = alignModelPoints (models_[best_match], query_object, params_); 00122 return (output); 00123 } 00124 00125 /* Construct an object model by filtering, segmenting, and estimating feature descriptors */ 00126 void 00127 constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const 00128 { 00129 output.points = applyFiltersAndSegment (points, params_); 00130 00131 SurfaceNormalsPtr normals; 00132 estimateFeatures (output.points, params_, normals, output.keypoints, 00133 output.local_descriptors, output.global_descriptor); 00134 } 00135 00136 protected: 00137 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */ 00138 PointCloudPtr 00139 applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const 00140 { 00141 PointCloudPtr cloud; 00142 cloud = thresholdDepth (input, params.min_depth, params.max_depth); 00143 cloud = downsample (cloud, params.downsample_leaf_size); 00144 cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors); 00145 00146 cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations); 00147 std::vector<pcl::PointIndices> cluster_indices; 00148 clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size, 00149 params.max_cluster_size, cluster_indices); 00150 00151 PointCloudPtr largest_cluster (new PointCloud); 00152 pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster); 00153 00154 return (largest_cluster); 00155 } 00156 00157 /* Estimate surface normals, keypoints, and local/global feature descriptors */ 00158 void 00159 estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params, 00160 SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out, 00161 LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const 00162 { 00163 normals_out = estimateSurfaceNormals (points, params.surface_normal_radius); 00164 00165 keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves, 00166 params.keypoints_nr_scales_per_octave, params.keypoints_min_contrast); 00167 00168 local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out, 00169 params.local_descriptor_radius); 00170 00171 global_descriptor_out = computeGlobalDescriptor (points, normals_out); 00172 } 00173 00174 /* Align the points in the source model to the points in the target model */ 00175 PointCloudPtr 00176 alignModelPoints (const ObjectModel & source, const ObjectModel & target, 00177 const ObjectRecognitionParameters & params) const 00178 { 00179 Eigen::Matrix4f tform; 00180 tform = computeInitialAlignment (source.keypoints, source.local_descriptors, 00181 target.keypoints, target.local_descriptors, 00182 params.initial_alignment_min_sample_distance, 00183 params.initial_alignment_max_correspondence_distance, 00184 params.initial_alignment_nr_iterations); 00185 00186 tform = refineAlignment (source.points, target.points, tform, 00187 params.icp_max_correspondence_distance, params.icp_outlier_rejection_threshold, 00188 params.icp_transformation_epsilon, params.icp_max_iterations); 00189 00190 PointCloudPtr output (new PointCloud); 00191 pcl::transformPointCloud (*(source.points), *output, tform); 00192 00193 return (output); 00194 } 00195 00196 ObjectRecognitionParameters params_; 00197 std::vector<ObjectModel> models_; 00198 GlobalDescriptorsPtr descriptors_; 00199 pcl::KdTreeFLANN<GlobalDescriptorT>::Ptr kdtree_; 00200 }; 00201 00202 #endif