Point Cloud Library (PCL) 1.12.0
object_recognition.h
1#pragma once
2
3#include "typedefs.h"
4
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
28
29 // Feature estimation parameters
36
37 // Registration parameters
45};
46
47struct ObjectModel
48{
49 PointCloudPtr points;
50 PointCloudPtr keypoints;
51 LocalDescriptorsPtr local_descriptors;
52 GlobalDescriptorsPtr global_descriptor;
53};
54
56{
57public:
59 {}
60
61 void
62 populateDatabase (const std::vector<std::string> & /*filenames*/)
63 {
64 }
65
66 const ObjectModel &
67 recognizeObject (const PointCloudPtr & /*query_cloud*/)
68 {
69 int best_match = 0;
70 return (models_[best_match]);
71 }
72
73 PointCloudPtr
74 recognizeAndAlignPoints (const PointCloudPtr & /*query_cloud*/)
75 {
76 PointCloudPtr output;
77 return (output);
78 }
79
80 /* Construct an object model by filtering, segmenting, and estimating feature descriptors */
81 void
82 constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const
83 {
84 output.points = applyFiltersAndSegment (points, params_);
85
86 SurfaceNormalsPtr normals;
87 estimateFeatures (output.points, params_, normals, output.keypoints,
89 }
90
91protected:
92 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
93 PointCloudPtr
94 applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const
95 {
96 PointCloudPtr cloud;
97 cloud = thresholdDepth (input, params.min_depth, params.max_depth);
98 cloud = downsample (cloud, params.downsample_leaf_size);
99 cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors);
100
101 cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations);
102 std::vector<pcl::PointIndices> cluster_indices;
103 clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size,
104 params.max_cluster_size, cluster_indices);
105
106 PointCloudPtr largest_cluster (new PointCloud);
107 pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster);
108
109 return (largest_cluster);
110 }
111
112 /* Estimate surface normals, keypoints, and local/global feature descriptors */
113 void
114 estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params,
115 SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out,
116 LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const
117 {
118 normals_out = estimateSurfaceNormals (points, params.surface_normal_radius);
119
120 keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves,
122
123 local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out,
125
126 global_descriptor_out = computeGlobalDescriptor (points, normals_out);
127 }
128
129 /* Align the points in the source model to the points in the target model */
130 PointCloudPtr
131 alignModelPoints (const ObjectModel & source, const ObjectModel & target,
132 const ObjectRecognitionParameters & params) const
133 {
134 Eigen::Matrix4f tform;
135 tform = computeInitialAlignment (source.keypoints, source.local_descriptors,
136 target.keypoints, target.local_descriptors,
140
141 tform = refineAlignment (source.points, target.points, tform,
144
145 PointCloudPtr output (new PointCloud);
146 pcl::transformPointCloud (*(source.points), *output, tform);
147
148 return (output);
149 }
150
152 std::vector<ObjectModel> models_;
153 GlobalDescriptorsPtr descriptors_;
155};
const ObjectModel & recognizeObject(const PointCloudPtr &)
pcl::KdTreeFLANN< GlobalDescriptorT >::Ptr kdtree_
PointCloudPtr applyFiltersAndSegment(const PointCloudPtr &input, const ObjectRecognitionParameters &params) const
std::vector< ObjectModel > models_
GlobalDescriptorsPtr descriptors_
ObjectRecognition(const ObjectRecognitionParameters &params)
PointCloudPtr recognizeAndAlignPoints(const PointCloudPtr &)
ObjectRecognitionParameters params_
PointCloudPtr alignModelPoints(const ObjectModel &source, const ObjectModel &target, const ObjectRecognitionParameters &params) const
void populateDatabase(const std::vector< std::string > &)
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
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:144
GlobalDescriptorsPtr global_descriptor
LocalDescriptorsPtr local_descriptors
PointCloudPtr keypoints
PointCloudPtr points