void geonDetector::initialize() { totalCount = 0; pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>()); pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>()); pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>()); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>()); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>()); pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients()); pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices()); pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>()); this->cloud = cloud1; this->cloud_filtered = cloud_filtered1; this->cloud_normals = cloud_normals1; this->cloud_filtered2 = cloud_filtered21; this->cloud_normals2 = cloud_normals21; this->coefficients = coefficients1; this->geonIndices = geonIndices1; this->tree = tree2; }
void pcl_process::plane_fitting(const PointCloudRGB::ConstPtr& pcl_in) { PointCloudRGB cloud_input = *pcl_in; ROS_INFO("total points: %d", cloud_input.points.size()); //perform downsampling for the input pointcloud; pcl::VoxelGrid<pcl::PointXYZRGB> sor; PointCloudRGB::Ptr input_msg_filtered (new PointCloudRGB ()); float downsample_size_ = 0.05; sor.setInputCloud(cloud_input.makeShared()); sor.setLeafSize (downsample_size_, downsample_size_, downsample_size_); sor.filter (*input_msg_filtered); cloud_input = * input_msg_filtered; ROS_INFO("remained points: %d", cloud_input.points.size()); //at most 3 planes in the pool; pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers1 (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients2 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers2 (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients3 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers3 (new pcl::PointIndices); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane1 (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane2 (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane3 (new pcl::PointCloud<pcl::PointXYZRGB> ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.1); pcl::ExtractIndices<pcl::PointXYZRGB> extract; seg.setInputCloud (cloud_input.makeShared ()); seg.segment (*inliers1, *coefficients1); extract.setInputCloud (cloud_input.makeShared()); extract.setIndices (inliers1); extract.setNegative (false); extract.filter (*surface_plane1); pcl::PointCloud<pcl::PointXYZRGB>::Ptr remained_points (new pcl::PointCloud<pcl::PointXYZRGB> ()); extract.setInputCloud (cloud_input.makeShared()); extract.setIndices (inliers1); extract.setNegative (true); extract.filter (*remained_points); double remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size()); if(remained_ratio > 0.2) { seg.setInputCloud (remained_points->makeShared ()); seg.segment (*inliers2, *coefficients2); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers2); extract.setNegative (false); extract.filter (*surface_plane2); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers2); extract.setNegative (true); extract.filter (*remained_points); remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size()); if(remained_ratio > 0.2) { seg.setInputCloud (remained_points->makeShared ()); seg.segment (*inliers3, *coefficients3); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers3); extract.setNegative (false); extract.filter (*surface_plane3); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers3); extract.setNegative (true); extract.filter (*remained_points); } } int plane1_type, plane2_type, plane3_type; plane1_type = plane_classication(*inliers1, *coefficients1, cloud_input); plane2_type = plane_classication(*inliers2, *coefficients2, cloud_input); plane3_type = plane_classication(*inliers3, *coefficients3, cloud_input); if(plane1_type ==1) pcl_vis_pub_.publish(*surface_plane1); else if(plane2_type ==1) pcl_vis_pub_.publish(*surface_plane2); else if(plane3_type ==1) pcl_vis_pub_.publish(*surface_plane3); }