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); }