Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
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,
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};
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
Iterator class for point clouds with or without given indices.
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
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.
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:144
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:621
GlobalDescriptorsPtr global_descriptor
LocalDescriptorsPtr local_descriptors
PointCloudPtr keypoints
PointCloudPtr points
A point structure representing the Viewpoint Feature Histogram (VFH).