pcl::RANSAC 分割，获取云中的所有平面?

编程入门行业动态更新时间:2024-10-23 16:23:40

本文介绍了pcl::RANSAC 分割，获取云中的所有平面?的处理方法，对大家解决问题具有一定的参考价值，需要的朋友们下面随着小编来一起学习吧！问题描述

我有一个点云库函数，可以检测点云中最大的平面.这很好用.现在，我想扩展此功能以分割出云中的每个平面并将这些点复制到新的云中(例如，房间地板上有球体的场景将返回地板和墙壁，但不是球体，因为它不是平面的).如何扩展以下代码以获取所有飞机，而不仅仅是最大的飞机?(运行时间是这里的一个因素，所以我不希望只是在循环中运行相同的代码，每次都剥离新的最大平面)

I have a Point Cloud Library function that detects the largest plane in a point cloud. This works great. Now, I would like to extend this functionality to segment out every planar surface in the cloud and copy those points to a new cloud (for example, a scene with a sphere on the floor of a room would give me back the floor and walls, but not the sphere, as it is not planar). How can I extend the below code to get all the planes, not just the largest one? (runtime is a factor here, so I would prefer not to just run this same code in a loop, stripping out the new largest plane each time)

int main(int argc, char** argv) { pcl::visualization::CloudViewer viewer("viewer1"); pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>); // Fill in the cloud data pcl::PCDReader reader; reader.read("clouds/table.pcd", *cloud_blob); // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud(cloud_blob); sor.setLeafSize(0.01f, 0.01f, 0.01f); sor.filter(*cloud_filtered_blob); // Convert to the templated PointCloud pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered); std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; 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); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.01); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZ> extract; int i = 0, nr_points = (int)cloud_filtered->points.size(); // While 30% of the original cloud is still there while (cloud_filtered->points.size() > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud(cloud_filtered); pcl::ScopeTime scopeTime("Test loop"); { seg.segment(*inliers, *coefficients); } if (inliers->indices.size() == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the inliers extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_p); std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl; } viewer.showCloud(cloud_p, "viewer1"); while (!viewer.wasStopped()) {} return (0); }

推荐答案

一旦你得到第一个平面，删除点并使用算法计算一个新平面，直到估计平面没有剩下的点不是这样事情了.第二种情况是因为使用 RANSAC，只要有足够的点，您总会找到一个平面.我在这里做了类似的事情(这是一个 ros 节点的回调):

Once you get the first plane, remove the points and use the algorithm to compute a new plane until either there are no points left of the estimated plane is no such thing anymore. The second case is because using RANSAC you will always find a plane as long as there are enough points. I have something similar done here (this is a callback for a ros node):

void pointCloudCb(const sensor_msgs::PointCloud2::ConstPtr &msg){ // Convert to pcl point cloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg,*cloud_msg); ROS_DEBUG("%s: new ponitcloud (%i,%i)(%zu)",_name.c_str(),cloud_msg->width,cloud_msg->height,cloud_msg->size()); // Filter cloud pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud(cloud_msg); pass.setFilterFieldName ("z"); pass.setFilterLimits(0.001,10000); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pass.filter (*cloud); // Get segmentation ready pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::ExtractIndices<pcl::PointXYZ> extract; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold(_max_distance); // Create pointcloud to publish inliers pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_pub(new pcl::PointCloud<pcl::PointXYZRGB>); int original_size(cloud->height*cloud->width); int n_planes(0); while (cloud->height*cloud->width>original_size*_min_percentage/100){ // Fit a plane seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); // Check result if (inliers->indices.size() == 0) break; // Iterate inliers double mean_error(0); double max_error(0); double min_error(100000); std::vector<double> err; for (int i=0;i<inliers->indices.size();i++){ // Get Point pcl::PointXYZ pt = cloud->points[inliers->indices[i]]; // Compute distance double d = point2planedistnace(pt,coefficients)*1000;// mm err.push_back(d); // Update statistics mean_error += d; if (d>max_error) max_error = d; if (d<min_error) min_error = d; } mean_error/=inliers->indices.size(); // Compute Standard deviation ColorMap cm(min_error,max_error); double sigma(0); for (int i=0;i<inliers->indices.size();i++){ sigma += pow(err[i] - mean_error,2); // Get Point pcl::PointXYZ pt = cloud->points[inliers->indices[i]]; // Copy point to noew cloud pcl::PointXYZRGB pt_color; pt_color.x = pt.x; pt_color.y = pt.y; pt_color.z = pt.z; uint32_t rgb; if (_color_pc_with_error) rgb = cm.getColor(err[i]); else rgb = colors[n_planes].getColor(); pt_color.rgb = *reinterpret_cast<float*>(&rgb); cloud_pub->points.push_back(pt_color); } sigma = sqrt(sigma/inliers->indices.size()); // Extract inliers extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(true); pcl::PointCloud<pcl::PointXYZ> cloudF; extract.filter(cloudF); cloud->swap(cloudF); // Display infor ROS_INFO("%s: fitted plane %i: %fx%s%fy%s%fz%s%f=0 (inliers: %zu/%i)", _name.c_str(),n_planes, coefficients->values[0],(coefficients->values[1]>=0?"+":""), coefficients->values[1],(coefficients->values[2]>=0?"+":""), coefficients->values[2],(coefficients->values[3]>=0?"+":""), coefficients->values[3], inliers->indices.size(),original_size); ROS_INFO("%s: mean error: %f(mm), standard deviation: %f (mm), max error: %f(mm)",_name.c_str(),mean_error,sigma,max_error); ROS_INFO("%s: poitns left in cloud %i",_name.c_str(),cloud->width*cloud->height); // Nest iteration n_planes++; } // Publish points sensor_msgs::PointCloud2 cloud_publish; pcl::toROSMsg(*cloud_pub,cloud_publish); cloud_publish.header = msg->header; _pub_inliers.publish(cloud_publish); }