Point Cloud Library (PCL)  1.7.1
object_recognition.h
1 #ifndef OBJECT_RECOGNITION_H_
2 #define OBJECT_RECOGNITION_H_
3 
4 #include "typedefs.h"
5 #include "load_clouds.h"
6 #include "solution/filters.h"
7 #include "solution/segmentation.h"
8 #include "solution/feature_estimation.h"
9 #include "solution/registration.h"
10 
11 #include <pcl/io/pcd_io.h>
12 #include <pcl/kdtree/kdtree_flann.h>
13 
14 
16 {
17  // Filter parameters
18  float min_depth;
19  float max_depth;
23 
24  // Segmentation parameters
27  float cluster_tolerance;
28  int min_cluster_size;
29  int max_cluster_size;
30 
31  // Feature estimation parameters
33  float keypoints_min_scale;
38 
39  // Registration parameters
47 };
48 
49 struct ObjectModel
50 {
51  PointCloudPtr points;
52  PointCloudPtr keypoints;
53  LocalDescriptorsPtr local_descriptors;
54  GlobalDescriptorsPtr global_descriptor;
55 };
56 
58 {
59  public:
61  {}
62 
63  void
64  populateDatabase (const std::vector<std::string> & filenames)
65  {
66  size_t n = filenames.size ();
67  models_.resize (n);
68  descriptors_ = GlobalDescriptorsPtr (new GlobalDescriptors);
69  for (size_t i = 0; i < n; ++i)
70  {
71  const std::string & filename = filenames[i];
72  if (filename.compare (filename.size ()-4, 4, ".pcd") == 0)
73  {
74  // If filename ends pcd extension, load the points and process them
75  PointCloudPtr raw_input (new PointCloud);
76  pcl::io::loadPCDFile (filenames[i], *raw_input);
77 
78  constructObjectModel (raw_input, models_[i]);
79  }
80  else
81  {
82  // If the filename has no extension, load the pre-computed models
83  models_[i].points = loadPointCloud<PointT> (filename, "_points.pcd");
84  models_[i].keypoints = loadPointCloud<PointT> (filename, "_keypoints.pcd");
85  models_[i].local_descriptors = loadPointCloud<LocalDescriptorT> (filename, "_localdesc.pcd");
86  models_[i].global_descriptor = loadPointCloud<GlobalDescriptorT> (filename, "_globaldesc.pcd");
87  }
88  *descriptors_ += *(models_[i].global_descriptor);
89  }
91  kdtree_->setInputCloud (descriptors_);
92  }
93 
94  const ObjectModel &
95  recognizeObject (const PointCloudPtr & query_cloud)
96  {
97  ObjectModel query_object;
98  constructObjectModel (query_cloud, query_object);
99  const GlobalDescriptorT & query_descriptor = query_object.global_descriptor->points[0];
100 
101  std::vector<int> nn_index (1);
102  std::vector<float> nn_sqr_distance (1);
103  kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance);
104  const int & best_match = nn_index[0];
105 
106  return (models_[best_match]);
107  }
108 
109  PointCloudPtr
110  recognizeAndAlignPoints (const PointCloudPtr & query_cloud)
111  {
112  ObjectModel query_object;
113  constructObjectModel (query_cloud, query_object);
114  const GlobalDescriptorT & query_descriptor = query_object.global_descriptor->points[0];
115 
116  std::vector<int> nn_index (1);
117  std::vector<float> nn_sqr_distance (1);
118  kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance);
119  const int & best_match = nn_index[0];
120 
121  PointCloudPtr output = alignModelPoints (models_[best_match], query_object, params_);
122  return (output);
123  }
124 
125  /* Construct an object model by filtering, segmenting, and estimating feature descriptors */
126  void
127  constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const
128  {
129  output.points = applyFiltersAndSegment (points, params_);
130 
131  SurfaceNormalsPtr normals;
132  estimateFeatures (output.points, params_, normals, output.keypoints,
133  output.local_descriptors, output.global_descriptor);
134  }
135 
136  protected:
137  /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
138  PointCloudPtr
139  applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const
140  {
141  PointCloudPtr cloud;
142  cloud = thresholdDepth (input, params.min_depth, params.max_depth);
143  cloud = downsample (cloud, params.downsample_leaf_size);
144  cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors);
145 
146  cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations);
147  std::vector<pcl::PointIndices> cluster_indices;
148  clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size,
149  params.max_cluster_size, cluster_indices);
150 
151  PointCloudPtr largest_cluster (new PointCloud);
152  pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster);
153 
154  return (largest_cluster);
155  }
156 
157  /* Estimate surface normals, keypoints, and local/global feature descriptors */
158  void
159  estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params,
160  SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out,
161  LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const
162  {
163  normals_out = estimateSurfaceNormals (points, params.surface_normal_radius);
164 
165  keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves,
167 
168  local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out,
169  params.local_descriptor_radius);
170 
171  global_descriptor_out = computeGlobalDescriptor (points, normals_out);
172  }
173 
174  /* Align the points in the source model to the points in the target model */
175  PointCloudPtr
176  alignModelPoints (const ObjectModel & source, const ObjectModel & target,
177  const ObjectRecognitionParameters & params) const
178  {
179  Eigen::Matrix4f tform;
180  tform = computeInitialAlignment (source.keypoints, source.local_descriptors,
181  target.keypoints, target.local_descriptors,
185 
186  tform = refineAlignment (source.points, target.points, tform,
189 
190  PointCloudPtr output (new PointCloud);
191  pcl::transformPointCloud (*(source.points), *output, tform);
192 
193  return (output);
194  }
195 
197  std::vector<ObjectModel> models_;
198  GlobalDescriptorsPtr descriptors_;
200 };
201 
202 #endif