object_recognition.h
#ifndef OBJECT_RECOGNITION_H_
#define OBJECT_RECOGNITION_H_
#include "typedefs.h"
#include "solution/filters.h"
#include "solution/segmentation.h"
#include "solution/feature_estimation.h"
#include "solution/registration.h"
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
struct ObjectRecognitionParameters
{
// Filter parameters
float min_depth;
float max_depth;
float downsample_leaf_size;
float outlier_rejection_radius;
int outlier_rejection_min_neighbors;
// Segmentation parameters
float plane_inlier_distance_threshold;
int max_ransac_iterations;
float cluster_tolerance;
int min_cluster_size;
int max_cluster_size;
// Feature estimation parameters
float surface_normal_radius;
float keypoints_min_scale;
float keypoints_nr_octaves;
float keypoints_nr_scales_per_octave;
float keypoints_min_contrast;
float local_descriptor_radius;
// Registration parameters
float initial_alignment_min_sample_distance;
float initial_alignment_max_correspondence_distance;
int initial_alignment_nr_iterations;
float icp_max_correspondence_distance;
float icp_outlier_rejection_threshold;
float icp_transformation_epsilon;
int icp_max_iterations;
};
struct ObjectModel
{
PointCloudPtr points;
PointCloudPtr keypoints;
LocalDescriptorsPtr local_descriptors;
GlobalDescriptorsPtr global_descriptor;
};
class ObjectRecognition
{
public:
ObjectRecognition (const ObjectRecognitionParameters & params) : params_ (params)
{}
void
populateDatabase (const std::vector<std::string> & filenames)
{
}
const ObjectModel &
recognizeObject (const PointCloudPtr & query_cloud)
{
int best_match = 0;
return (models_[best_match]);
}
PointCloudPtr
recognizeAndAlignPoints (const PointCloudPtr & query_cloud)
{
PointCloudPtr output;
return (output);
}
/* Construct an object model by filtering, segmenting, and estimating feature descriptors */
void
constructObjectModel (const PointCloudPtr & points, ObjectModel & output) const
{
output.points = applyFiltersAndSegment (points, params_);
SurfaceNormalsPtr normals;
estimateFeatures (output.points, params_, normals, output.keypoints,
output.local_descriptors, output.global_descriptor);
}
protected:
/* Apply a series of filters (threshold depth, downsample, and remove outliers) */
PointCloudPtr
applyFiltersAndSegment (const PointCloudPtr & input, const ObjectRecognitionParameters & params) const
{
PointCloudPtr cloud;
cloud = thresholdDepth (input, params.min_depth, params.max_depth);
cloud = downsample (cloud, params.downsample_leaf_size);
cloud = removeOutliers (cloud, params.outlier_rejection_radius, params.outlier_rejection_min_neighbors);
cloud = findAndSubtractPlane (cloud, params.plane_inlier_distance_threshold, params.max_ransac_iterations);
std::vector<pcl::PointIndices> cluster_indices;
clusterObjects (cloud, params.cluster_tolerance, params.min_cluster_size,
params.max_cluster_size, cluster_indices);
PointCloudPtr largest_cluster (new PointCloud);
pcl::copyPointCloud (*cloud, cluster_indices[0], *largest_cluster);
return (largest_cluster);
}
/* Estimate surface normals, keypoints, and local/global feature descriptors */
void
estimateFeatures (const PointCloudPtr & points, const ObjectRecognitionParameters & params,
SurfaceNormalsPtr & normals_out, PointCloudPtr & keypoints_out,
LocalDescriptorsPtr & local_descriptors_out, GlobalDescriptorsPtr & global_descriptor_out) const
{
normals_out = estimateSurfaceNormals (points, params.surface_normal_radius);
keypoints_out = detectKeypoints (points, normals_out, params.keypoints_min_scale, params.keypoints_nr_octaves,
params.keypoints_nr_scales_per_octave, params.keypoints_min_contrast);
local_descriptors_out = computeLocalDescriptors (points, normals_out, keypoints_out,
params.local_descriptor_radius);
global_descriptor_out = computeGlobalDescriptor (points, normals_out);
}
/* Align the points in the source model to the points in the target model */
PointCloudPtr
alignModelPoints (const ObjectModel & source, const ObjectModel & target,
const ObjectRecognitionParameters & params) const
{
Eigen::Matrix4f tform;
tform = computeInitialAlignment (source.keypoints, source.local_descriptors,
target.keypoints, target.local_descriptors,
params.initial_alignment_min_sample_distance,
params.initial_alignment_max_correspondence_distance,
params.initial_alignment_nr_iterations);
tform = refineAlignment (source.points, target.points, tform,
params.icp_max_correspondence_distance, params.icp_outlier_rejection_threshold,
params.icp_transformation_epsilon, params.icp_max_iterations);
PointCloudPtr output (new PointCloud);
pcl::transformPointCloud (*(source.points), *output, tform);
return (output);
}
ObjectRecognitionParameters params_;
std::vector<ObjectModel> models_;
GlobalDescriptorsPtr descriptors_;
pcl::KdTreeFLANN<GlobalDescriptorT>::Ptr kdtree_;
};
#endif
ObjectRecognitionParameters::local_descriptor_radius
float local_descriptor_radius
Definition: object_recognition.h:37
ObjectRecognitionParameters::plane_inlier_distance_threshold
float plane_inlier_distance_threshold
Definition: object_recognition.h:25
ObjectRecognitionParameters::initial_alignment_min_sample_distance
float initial_alignment_min_sample_distance
Definition: object_recognition.h:40
ObjectRecognitionParameters::outlier_rejection_min_neighbors
int outlier_rejection_min_neighbors
Definition: object_recognition.h:22
ObjectRecognitionParameters::keypoints_nr_scales_per_octave
float keypoints_nr_scales_per_octave
Definition: object_recognition.h:35
ObjectRecognitionParameters::icp_max_correspondence_distance
float icp_max_correspondence_distance
Definition: object_recognition.h:43
ObjectRecognitionParameters::surface_normal_radius
float surface_normal_radius
Definition: object_recognition.h:32
ObjectRecognitionParameters::icp_max_iterations
int icp_max_iterations
Definition: object_recognition.h:46
ObjectModel::global_descriptor
GlobalDescriptorsPtr global_descriptor
Definition: object_recognition.h:54
ObjectRecognitionParameters::initial_alignment_max_correspondence_distance
float initial_alignment_max_correspondence_distance
Definition: object_recognition.h:41
ObjectRecognition::populateDatabase
void populateDatabase(const std::vector< std::string > &filenames)
Definition: object_recognition.h:64
pcl::copyPointCloud
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
ObjectRecognitionParameters::outlier_rejection_radius
float outlier_rejection_radius
Definition: object_recognition.h:21
ObjectRecognitionParameters::downsample_leaf_size
float downsample_leaf_size
Definition: object_recognition.h:20
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
ObjectRecognitionParameters::min_cluster_size
int min_cluster_size
Definition: object_recognition.h:28
ObjectRecognitionParameters::icp_outlier_rejection_threshold
float icp_outlier_rejection_threshold
Definition: object_recognition.h:44
ObjectRecognition::ObjectRecognition
ObjectRecognition(const ObjectRecognitionParameters &params)
Definition: object_recognition.h:60
ObjectRecognitionParameters::max_ransac_iterations
int max_ransac_iterations
Definition: object_recognition.h:26
ObjectRecognition
Definition: object_recognition.h:57
ObjectRecognitionParameters::cluster_tolerance
float cluster_tolerance
Definition: object_recognition.h:27
ObjectModel::local_descriptors
LocalDescriptorsPtr local_descriptors
Definition: object_recognition.h:53
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
ObjectRecognitionParameters
Definition: object_recognition.h:15
ObjectRecognitionParameters::min_depth
float min_depth
Definition: object_recognition.h:18
ObjectRecognitionParameters::icp_transformation_epsilon
float icp_transformation_epsilon
Definition: object_recognition.h:45
ObjectRecognition::alignModelPoints
PointCloudPtr alignModelPoints(const ObjectModel &source, const ObjectModel &target, const ObjectRecognitionParameters &params) const
Definition: object_recognition.h:133
ObjectRecognitionParameters::keypoints_min_scale
float keypoints_min_scale
Definition: object_recognition.h:33
ObjectRecognition::constructObjectModel
void constructObjectModel(const PointCloudPtr &points, ObjectModel &output) const
Definition: object_recognition.h:84
ObjectRecognitionParameters::max_depth
float max_depth
Definition: object_recognition.h:19
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:116
ObjectModel::points
PointCloudPtr points
Definition: object_recognition.h:51
ObjectRecognitionParameters::keypoints_nr_octaves
float keypoints_nr_octaves
Definition: object_recognition.h:34
ObjectModel::keypoints
PointCloudPtr keypoints
Definition: object_recognition.h:52
ObjectRecognition::recognizeAndAlignPoints
PointCloudPtr recognizeAndAlignPoints(const PointCloudPtr &query_cloud)
Definition: object_recognition.h:76
ObjectRecognitionParameters::initial_alignment_nr_iterations
int initial_alignment_nr_iterations
Definition: object_recognition.h:42
ObjectModel
Definition: object_recognition.h:49
ObjectRecognitionParameters::keypoints_min_contrast
float keypoints_min_contrast
Definition: object_recognition.h:36
ObjectRecognitionParameters::max_cluster_size
int max_cluster_size
Definition: object_recognition.h:29
ObjectRecognition::applyFiltersAndSegment
PointCloudPtr applyFiltersAndSegment(const PointCloudPtr &input, const ObjectRecognitionParameters &params) const
Definition: object_recognition.h:96
ObjectRecognition::recognizeObject
const ObjectModel & recognizeObject(const PointCloudPtr &query_cloud)
Definition: object_recognition.h:69
pcl::KdTreeFLANN::Ptr
boost::shared_ptr< KdTreeFLANN< PointT > > Ptr
Definition: kdtree_flann.h:89