使用点云拟合圆柱，并使用ceres对圆柱方程参数进行非线性优化

本文主要针对给定的圆柱点云拟合圆柱方程，在得到方程的参数之后，对圆柱方程参数使用ceres进行非线性优化，最终得出优化后的圆柱方程。
首先我们给出圆柱面方程的一般形式

因此我们可以根据圆柱面方程构建一个如下所示的最小化问题。

本文使用AT960激光跟踪仪和T-SCAN对机器人加工刀具主轴进行点云扫描，获得点云文件。
下面针对该解决方案的关键代码进行讲解。

//构建残差块
struct CostFun{
  //残差块构造函数 
    CostFun(double x,double y,double z):x_(x),y_(y),z_(z){}
//运算符重载函数，params为优化参数数组，residual为误差
    template<typename T>
    bool operator()(const T* const parm,T* residual) const{
        residual[0]=pow((x_-parm[0]),2)+pow((y_-parm[1]),2)+pow((z_-parm[2]),2)-pow(parm[3]*(x_-parm[0])+parm[4]*(y_-parm[1])+parm[5]*(z_-parm[2]),2)-pow(parm[6],2);
        return true;
    }
//圆柱点云数据
    const double x_,y_,z_;
};

构建好残差块之后，首先使用pcl库对点云进行分割，然后提取圆柱点云，获取圆柱方程初始参数。

//读取点云
  reader.read ("/home/dongfu/project/Cylinder_extract/210127_101328_height.pcd", *cloud);
  std::cerr << "PointCloud has: " << cloud->size () << " data points." << std::endl;

//滤波
pcl::PassThrough<PointT> pass;
pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (-1000,1000);
  pass.filter (*cloud_filtered);
  std::cerr << "PointCloud after filtering has: " << cloud_filtered->size () << " data points." << std::endl;

//估计法线
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud_filtered);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);
  std::cout<<"normals size="<<cloud_normals->size()<<endl;
 pcl::NormalEstimation<PointT, pcl::Normal> ne;

//分割点云，提取点云
 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
 pcl::ExtractIndices<PointT> extract;
seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (5);
  seg.setRadiusLimits (0, 600);
  seg.setInputCloud (cloud_filtered);
  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 (cloud_filtered);
  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->size () << " data points." << std::endl;
	  writer.write ("/home/dongfu/projects/Cylinder_extract/cylinder.pcd", *cloud_cylinder, false);
  }

//设置优化初值
  double x0=coefficients_cylinder->values[0];
  double y0=coefficients_cylinder->values[1];
  double z0=coefficients_cylinder->values[2];
  double a=coefficients_cylinder->values[3];
  double b=coefficients_cylinder->values[4];
  double c=coefficients_cylinder->values[5];
  double r0=coefficients_cylinder->values[6];

//开始创建优化问题进行优化
//开始优化
  //设置参数块
  double param[7]={x0,y0,z0,a,b,c,r0};
  //定义优化问题
  ceres::Problem problem;
  for(int i=0;i<cloud_cylinder->size();i++)
  {
     problem.AddResidualBlock(
     //添加残差块，主要参数包括误差函数、输入维度、输出维度
         new ceres::AutoDiffCostFunction<CostFun,7,1>(new CostFun(cloud_cylinder->points[i].x,cloud_cylinder->points[i].y,cloud_cylinder->points[i].z)),
        nullptr,
        param
     );
  }
  //配置求解器
  ceres::Solver::Options options;
  options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;//增量方程求解
  options.minimizer_progress_to_stdout=true;//输出到cout
  ceres::Solver::Summary summary;
  ceres::Solve(options,&problem,&summary);
  cout<<summary.BriefReport()<<endl;
  cout<<"优化之后的结果为:"<<endl;
  for(auto x:param)
  {
      cout<<x<<" ";
  }
  cout<<std::endl;
  cout<<"优化后半径为"<<param[6]<<endl;

//误差评价
 //计算优化后的参数误差
   double r_=param[6];
   double sum_error=0;
   vector<double> error;
   double ave_error=0;
  for(int i=0;i<cloud_cylinder->size();i++)
  {
     double temp1=pow(cloud_cylinder->points[i].x-param[0],2)+pow(cloud_cylinder->points[i].y-param[1],2)+pow(cloud_cylinder->points[i].z-param[2],2);
     double temp2=pow(param[3]*(cloud_cylinder->points[i].x-param[0])+param[4]*(cloud_cylinder->points[i].y-param[1])+param[5]*(cloud_cylinder->points[i].z-param[2]),2);
     double r_predict_2=temp1-temp2;
     double r_predict=sqrt(r_predict_2);
     double temp_error=r_-r_predict;
     double temp_error_2=pow(temp_error,2);
     sum_error+=temp_error_2;
     error.push_back(temp_error);
 }
 //标准差
 ave_error=sqrt(sum_error/cloud_cylinder->size());
 cout<<"平均误差为"<<ave_error<<endl;
 //cout<<"总误差"<<sum_error<<endl;

通过运行该优化程序，最终得出的优化结果为：
优化后预测半径和优化半径之间的标准差为0.009mm



本文源码见github：https://github.com/dongfufu/ceres_learn