PCL提取圆柱系数

Posted 2021-01-29 summerio

网上看了很多教程，没看到圆柱提取后的系数解释。

源码如下：

#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>());

// Datasets
pcl::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 
");
     // 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就是圆柱的半径。