void SegmentPlane::detectPlaneCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc) { pcl::PointCloud<pcl::PointXYZ> pcl_source; pcl::fromROSMsg(*source_pc, pcl_source); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source)); pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>); pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter; approximate_voxel_filter.setLeafSize (leaf_size_x_, leaf_size_y_, leaf_size_z_); approximate_voxel_filter.setInputCloud (pcl_source_ptr); approximate_voxel_filter.filter (*resized_pc_ptr); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients(true); // Mandatory seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.1); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZ> extract; seg.setInputCloud(resized_pc_ptr); seg.segment(*inliers, *coefficients); if(inliers->indices.size() == 0) { ROS_WARN("Could not estimate a planar model for the given dataset"); return; } extract.setInputCloud(resized_pc_ptr); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_p); sensor_msgs::PointCloud2 inliers_pc; pcl::toROSMsg(*cloud_p, inliers_pc); inliers_pc.header.stamp = ros::Time::now(); inliers_pc.header.frame_id = frame_id_; inliers_pc_pub_.publish(inliers_pc); }
void KinectV2Downsampler::resizeCallback(const sensor_msgs::PointCloud2::ConstPtr& source_pc) { pcl::PointCloud<pcl::PointXYZ> pcl_source; pcl::fromROSMsg(*source_pc, pcl_source); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_source_ptr(new pcl::PointCloud<pcl::PointXYZ>(pcl_source)); pcl::PointCloud<pcl::PointXYZ>::Ptr resized_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter; approximate_voxel_filter.setLeafSize(leaf_size_x_, leaf_size_y_, leaf_size_z_); approximate_voxel_filter.setInputCloud(pcl_source_ptr); approximate_voxel_filter.filter(*resized_pc_ptr); sensor_msgs::PointCloud2 resized_pc; pcl::toROSMsg(*resized_pc_ptr, resized_pc); resized_pc.header.stamp = ros::Time::now(); resized_pc.header.frame_id = frame_id_; resized_pc_pub_.publish(resized_pc); }