PCL提取圓柱係數
阿新 • • 發佈:2018-12-22
網上看了很多教程,沒看到圓柱提取後的係數解釋。
原始碼如下:
#include <pcl/ModelCoefficients.h> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/filters/extract_indices.h> #include <pcl/filters/passthrough.h> #include <pcl/features/normal_3d.h> #include <pcl/sample_consensus/method_types.h> #include<pcl/sample_consensus/model_types.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/visualization/pcl_visualizer.h> #include <boost/thread/thread.hpp> #include <pcl/filters/radius_outlier_removal.h> typedef pcl::PointXYZ PointT; // All the objects needed pcl::PCDReader reader; pcl::PassThrough<PointT> pass; pcl::NormalEstimation<PointT, pcl::Normal> ne; pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; pcl::PCDWriter writer; pcl::ExtractIndices<PointT> extract; pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); // Datasetspcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer")); pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d(new pcl::PointCloud<pcl::PointXYZ>); int num = 0; void pp_callback(const pcl::visualization::AreaPickingEvent& event, void* args) { clicked_points_3d->points.clear(); pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>); std::vector< int > indices; if (event.getPointsIndices(indices) == -1) return; for (int i = 0; i < indices.size(); ++i) { clicked_points_3d->points.push_back(cloud->points.at(indices[i])); } //clicked_points_3d->width = 1; //clicked_points_3d->height = clicked_points_3d->size(); //if (!clicked_points_3d->points.empty()) //{ // writer.write("Selected.pcd", *clicked_points_3d, false); //} // Estimate point normals ne.setSearchMethod(tree); ne.setInputCloud(clicked_points_3d); ne.setKSearch(50); ne.compute(*cloud_normals); // Create the segmentation object for cylinder segmentation and set all the parameters seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_CYLINDER); seg.setMethodType(pcl::SAC_RANSAC); seg.setNormalDistanceWeight(0.1); seg.setMaxIterations(10000); double threshold; std::cout << "threshold: "; std::cin >> threshold; seg.setDistanceThreshold(threshold); //單位米 double radius; std::cout << "radius: "; std::cin >> radius; seg.setRadiusLimits(0, radius); //單位米 seg.setInputCloud(clicked_points_3d); seg.setInputNormals(cloud_normals); // Obtain the cylinder inliers and coefficients seg.segment(*inliers_cylinder, *coefficients_cylinder); std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; // Write the cylinder inliers to disk extract.setInputCloud(clicked_points_3d); extract.setIndices(inliers_cylinder); extract.setNegative(false); pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>()); extract.filter(*cloud_cylinder); if (cloud_cylinder->points.empty()) std::cerr << "Can't find the cylindrical component." << std::endl; else { std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl; cloud_cylinder->width = 1; cloud_cylinder->height = cloud_cylinder->size(); writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); } system("pause"); std::stringstream ss; std::string cloudName; ss << num++; ss >> cloudName; cloudName += "_cloudName"; pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(clicked_points_3d, 255, 0, 0); viewer->addPointCloud(clicked_points_3d, red, cloudName); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, cloudName); //std::stringstream ss; //std::string cloudName; ss << num++; ss >> cloudName; cloudName += "_cloudName"; pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue(cloud_cylinder, 0, 0, 255); viewer->addPointCloud(cloud_cylinder, blue, cloudName); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, cloudName); } int main (int argc, char** argv) { //std::string location; //std::getline(std::cin, location); //if (location[0] == '"') //{ // location = location.substr(1, location.length() - 2); //} //if (pcl::io::loadPCDFile<pcl::PointXYZ>(location, *cloud) == -1) //{ // PCL_ERROR("Couldn't read file \n"); // system("pause"); //} //std::cout << "Reading Success" << std::endl; //生成圓柱點雲 for (float z(-10); z <= 10; z += 0.5) { for (float angle(0.0); angle <= 360.0; angle += 5.0) { pcl::PointXYZ basic_point; basic_point.x = 10+3.5*cos(angle / 180 * M_PI); basic_point.y = 200+3.5*sin(angle / 180 * M_PI); basic_point.z = z; cloud->points.push_back(basic_point); } } //// Read in the cloud data //reader.read ("table_scene_mug_stereo_textured.pcd", *cloud); //std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl; // Build a passthrough filter to remove spurious NaNs //pass.setInputCloud (cloud); //pass.setFilterFieldName ("z"); //pass.setFilterLimits (0, 1.5); //pass.filter (*#); //std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; viewer->addPointCloud(cloud, "bunny"); viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0); viewer->registerAreaPickingCallback(pp_callback, (void*)&cloud); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return (0); }
程式執行後看不見點雲按R鍵
接著按下X鍵選中點雲,再按下X鍵
設定偏差閾值為1
圓柱的半徑大於3.5
就可以得到如下結果
係數0、1、2代表圓柱軸線上的原點,3、4、5代表這條軸線的方向向量,係數6就是圓柱的半徑。