void build_feature_cloud(const std::vector<cv::KeyPoint>& keypoints, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, sensor_msgs::PointCloud2::Ptr& feature_cloud_msg)
{
// Work with PCL compatible variables
pcl::PointIndices::Ptr keypoint_indices(new pcl::PointIndices());
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr pcl_output(new pcl::PCLPointCloud2);
// Convert keypoints to PCL format
for(int i=0; i < keypoints.size(); i++)
{
int x = keypoints[i].pt.x;//1-640
int y = keypoints[i].pt.y;//1-480
keypoint_indices->indices.push_back(cloud_msg->width*y+x);
}
// Convert cloud_msg to PCL format
pcl_conversions::toPCL(*cloud_msg, *pcl_input);
// Use ExtractIndices filter to build feature cloud
pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
extract.setInputCloud(pcl_input);
extract.setIndices(keypoint_indices);
extract.setNegative(false);
extract.filter(*pcl_output);
// Convert pcl_output to ROS format
pcl_conversions::fromPCL(*pcl_output, *feature_cloud_msg);
// Print out number of features for debugging
if(display_features)
ROS_INFO("# feature cloud points: %d", pcl_output->width * pcl_output->height);
}
void
pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
const IndicesPtr &indices,
PointCloud2 &output)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices);
pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
}
void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
ROS_INFO("# feature cloud points: %d", cloud_msg->width * cloud_msg->height);
// Save feature cloud
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*cloud_msg, *pcl_input);
char buff[100];
sprintf(buff, "pointcloud_%d.pcd", count);
std::string filename(buff);
count++;
pcl::PCDWriter pcd_writer;
pcd_writer.writeASCII(filename, *pcl_input);
}
