object_recognition.h
Go to the documentation of this file.
00001 #ifndef OBJECT_RECOGNITION_H_
00002 #define OBJECT_RECOGNITION_H_
00003 
00004 #include "typedefs.h"
00005 #include "load_clouds.h"
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       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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:12