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