void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
// Print cloud message
if(print_msgs)
print_cloud_msg(cloud_msg);
// Extract RGB image from cloud
cv::Mat img;
extract_rgb_image(cloud_msg, img);
// Detect keypoints from RGB image
std::vector<cv::KeyPoint> keypoints;
extract_keypoints(img, keypoints);
// Build feature cloud from keypoints
sensor_msgs::PointCloud2::Ptr feature_cloud_msg(new sensor_msgs::PointCloud2);
build_feature_cloud(keypoints, cloud_msg, feature_cloud_msg);
// Publish the data
pub_cloud.publish(feature_cloud_msg);
}
// global variables used: NONE
void
vtree_user::compute_features( pcl::PointCloud<PointT>::Ptr & point_cloud,
pcl::PointCloud<PointT>::Ptr & keypoints,
pcl::PointCloud<FeatureType>::Ptr & features )
{
// Define Parameters
// IMPORTANT: the radius used for features has to be larger than the radius used to estimate the surface normals!!!
#if FEATURE == 1 // FPFH
float keypoint_radius_(0.032f);
float normal_radius_(0.04f);
float feature_radius_(0.063f);
#elif FEATURE == 2 // PFH
float keypoint_radius_(0.02f);
float normal_radius_(0.03f);
float feature_radius_(0.04f);
#elif FEATURE == 3 // VFH
float keypoint_radius_(0.02f);
float normal_radius_(0.03f);
float feature_radius_(0.04f);
#endif
ROS_INFO("[vtree_user] Starting keypoint extraction...");
clock_t tic = clock();
pcl::PointCloud<int>::Ptr keypoint_idx(new pcl::PointCloud<int>);
extract_keypoints( point_cloud, keypoints, keypoint_idx, keypoint_radius_ );
ROS_INFO("[vtree_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_user] Starting normal extraction...");
tic = clock();
pcl::PointCloud<pcl::Normal>::Ptr normals_(new pcl::PointCloud<pcl::Normal>);
SearchMethod::Ptr search_method_xyz_ (new SearchMethod);
pcl::NormalEstimation<PointT, pcl::Normal> norm_est;
norm_est.setInputCloud ( point_cloud );
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
ROS_INFO("[vtree_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_user] Starting feature computation...");
boost::shared_ptr<std::vector<int> > key_idx_ptr( new std::vector<int>);
for( pcl::PointCloud<int>::iterator it = keypoint_idx->begin();
it != keypoint_idx->end(); ++it)
key_idx_ptr->push_back( *it );
tic = clock();
FeatureEst feat_est;
feat_est.setInputCloud ( point_cloud );
feat_est.setInputNormals (normals_);
feat_est.setIndices( key_idx_ptr );
search_method_xyz_.reset(new SearchMethod);
feat_est.setSearchMethod (search_method_xyz_);
feat_est.setRadiusSearch (feature_radius_);
feat_est.compute ( *features );
/*
feat_est.setSearchSurface (point_cloud);
feat_est.setInputNormals (normals_);
feat_est.setInputCloud ( keypoints );
search_method_xyz_.reset(new SearchMethod);
feat_est.setSearchMethod (search_method_xyz_);
feat_est.setRadiusSearch (feature_radius_);
feat_est.compute ( *features );
*/
ROS_INFO("[vtree_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
}