Point Cloud Library (PCL)  1.14.1-dev
object_recognition.h
1 #pragma once
2 
3 #include "typedefs.h"
4 #include "load_clouds.h"
5 #include "solution/filters.h"
6 #include "solution/segmentation.h"
7 #include "solution/feature_estimation.h"
8 #include "solution/registration.h"
9 
10 #include <pcl/io/pcd_io.h>
11 #include <pcl/kdtree/kdtree_flann.h>
12 
14 {
15  // Filter parameters
16  float min_depth;
17  float max_depth;
21 
22  // Segmentation parameters
25  float cluster_tolerance;
26  int min_cluster_size;
27  int max_cluster_size;
28 
29  // Feature estimation parameters
31  float keypoints_min_scale;
36 
37  // Registration parameters
45 };
46 
47 struct ObjectModel
48 {
49  PointCloudPtr points;
50  PointCloudPtr keypoints;
51  LocalDescriptorsPtr local_descriptors;
52  GlobalDescriptorsPtr global_descriptor;
53 };
54 
56 {
57  public:
59  {}
60 
61  void
62  populateDatabase (const std::vector<std::string> & filenames)
63  {
64  std::size_t n = filenames.size ();
65  models_.resize (n);
66  descriptors_ = GlobalDescriptorsPtr (new GlobalDescriptors);
67  for (std::size_t i = 0; i < n; ++i)
68  {
69  const std::string & filename = filenames[i];
70  if (filename.compare (filename.size ()-4, 4, ".pcd") == 0)
71  {
72  // If filename ends pcd extension, load the points and process them
73  PointCloudPtr raw_input (new PointCloud);
74  pcl::io::loadPCDFile (filenames[i], *raw_input);
75 
76  constructObjectModel (raw_input, models_[i]);
77  }
78  else
79  {
80  // If the filename has no extension, load the pre-computed models
81  models_[i].points = loadPointCloud<PointT> (filename, "_points.pcd");
82  models_[i].keypoints = loadPointCloud<PointT> (filename, "_keypoints.pcd");
83  models_[i].local_descriptors = loadPointCloud<LocalDescriptorT> (filename, "_localdesc.pcd");
84  models_[i].global_descriptor = loadPointCloud<GlobalDescriptorT> (filename, "_globaldesc.pcd");
85  }
86  *descriptors_ += *(models_[i].global_descriptor);
87  }
89  kdtree_->setInputCloud (descriptors_);
90  }
91 
92  const ObjectModel &
93  recognizeObject (const PointCloudPtr & query_cloud)
94  {
95  ObjectModel query_object;
96  constructObjectModel (query_cloud, query_object);
97  const GlobalDescriptorT & query_descriptor = (*query_object.global_descriptor)[0];
98 
99  std::vector<int> nn_index (1);
100  std::vector<float> nn_sqr_distance (1);
101  kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance);
102  const int & best_match = nn_index[0];
103 
104  return (models_[best_match]);
105  }
106 
107  PointCloudPtr
108  recognizeAndAlignPoints (const PointCloudPtr & query_cloud)
109  {
110  ObjectModel query_object;
111  constructObjectModel (query_cloud, query_object);
112  const GlobalDescriptorT & query_descriptor = (*query_object.global_descriptor)[0];
113 
114  std::vector<int> nn_index (1);
115  std::vector<float> nn_sqr_distance (1);
116  kdtree_->nearestKSearch (query_descriptor, 1, nn_index, nn_sqr_distance);
117  const int & best_match = nn_index[0];
118 
119  PointCloudPtr output = alignModelPoints (models_[best_match], query_object, params_);
120  return (output);
121  }
122 
123  /* Construct an object model by filtering, segmenting, and estimating feature descriptors */
124  void
125  constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const
126  {
127  output.points = applyFiltersAndSegment (points, params_);
128 
129  SurfaceNormalsPtr normals;
130  estimateFeatures (output.points, params_, normals, output.keypoints,
131  output.local_descriptors, output.global_descriptor);
132  }
133 
134  protected:
135  /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
136  PointCloudPtr
137  applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const
138  {
139  PointCloudPtr cloud;
140  cloud = thresholdDepth (input, params.min_depth, params.max_depth);
141  cloud = downsample (cloud, params.downsample_leaf_size);
142  cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors);
143 
144  cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations);
145  std::vector<pcl::PointIndices> cluster_indices;
146  clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size,
147  params.max_cluster_size, cluster_indices);
148 
149  PointCloudPtr largest_cluster (new PointCloud);
150  pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster);
151 
152  return (largest_cluster);
153  }
154 
155  /* Estimate surface normals, keypoints, and local/global feature descriptors */
156  void
157  estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params,
158  SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out,
159  LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const
160  {
161  normals_out = estimateSurfaceNormals (points, params.surface_normal_radius);
162 
163  keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves,
165 
166  local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out,
167  params.local_descriptor_radius);
168 
169  global_descriptor_out = computeGlobalDescriptor (points, normals_out);
170  }
171 
172  /* Align the points in the source model to the points in the target model */
173  PointCloudPtr
174  alignModelPoints (const ObjectModel & source, const ObjectModel & target,
175  const ObjectRecognitionParameters & params) const
176  {
177  Eigen::Matrix4f tform;
178  tform = computeInitialAlignment (source.keypoints, source.local_descriptors,
179  target.keypoints, target.local_descriptors,
183 
184  tform = refineAlignment (source.points, target.points, tform,
187 
188  PointCloudPtr output (new PointCloud);
189  pcl::transformPointCloud (*(source.points), *output, tform);
190 
191  return (output);
192  }
193 
195  std::vector<ObjectModel> models_;
196  GlobalDescriptorsPtr descriptors_;
198 };
pcl::KdTreeFLANN< GlobalDescriptorT >::Ptr kdtree_
PointCloudPtr applyFiltersAndSegment(const PointCloudPtr &input, const ObjectRecognitionParameters &params) const
std::vector< ObjectModel > models_
const ObjectModel & recognizeObject(const PointCloudPtr &query_cloud)
GlobalDescriptorsPtr descriptors_
PointCloudPtr recognizeAndAlignPoints(const PointCloudPtr &query_cloud)
void populateDatabase(const std::vector< std::string > &filenames)
ObjectRecognition(const ObjectRecognitionParameters &params)
ObjectRecognitionParameters params_
PointCloudPtr alignModelPoints(const ObjectModel &source, const ObjectModel &target, const ObjectRecognitionParameters &params) const
void constructObjectModel(const PointCloudPtr &points, ObjectModel &output) const
void estimateFeatures(const PointCloudPtr &points, const ObjectRecognitionParameters &params, SurfaceNormalsPtr &normals_out, PointCloudPtr &keypoints_out, LocalDescriptorsPtr &local_descriptors_out, GlobalDescriptorsPtr &global_descriptor_out) const
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:142
int loadPCDFile(const std::string &file_name, pcl::PCLPointCloud2 &cloud)
Load a PCD v.6 file into a templated PointCloud type.
Definition: pcd_io.h:633
GlobalDescriptorsPtr global_descriptor
LocalDescriptorsPtr local_descriptors
PointCloudPtr keypoints
PointCloudPtr points
A point structure representing the Viewpoint Feature Histogram (VFH).