pcl不规则圆求圆心

基本思路借鉴等距中心的算法思想, 1、已经用拟合圆、obb边界框、质心，求出最合适的中心，中心判断思想为此点距离圆环点云的最大距离和最小距离；2、以此中心为圆心，以(最大-最小)/2为最大半径画10个同心圆，求所有同心圆上的点与圆环点云的最大最小距离，迭代20次。

bool Imgdeal::lylord_center(pcl_ptr &generate_cloud, pcl::PointXYZ &circleCenter, pcl_ptr &target_cloud, pcl_nml &cloud_normals, double &devlate, double &lylord_x, double &lylord_y)
{
    // distance()函数输入
    double min_dt = 0.0, max_dt =0.0;                                                 // 距离的最大最小值
    double generate_cloud_x = 0.0 ,generate_cloud_y =0.0;                             // 求距离的中心值
    double lylord_delta[100000]={0.0};                                                // 迭代算法的差值的集合

    double x =0.0,y =0.0;                                                             // 计算出来的偏移量最小时，对应的中心坐标
    double lylord_devlate = devlate;                                                  // 5种方法求出的最小距离差值，在此用于比较
    
    Eigen::Vector3f c = circleCenter.getVector3fMap();                                // c为5种方法求到的中心坐标
 
    // 首先在原来的中心做圆 半径为差值的一半
    // Generate3DCircle(generate_cloud, circleCenter, cloud_normals, devlate/1.5);  
    Generate3DCircle(generate_cloud, circleCenter, cloud_normals, devlate/2);        
    writer.write("../PointsCloud/circle3DCloud.pcd", *generate_cloud);
    std::cout<<"devlate/2: " <<  devlate/2 << std::endl;

    bool lylord_flag = true;            // 迭代算法的标志                                           
    bool new_change = false;            // 有距离更新的标志
    int number = 0;
    do 
    {
        for (int i = 0; i < generate_cloud->size(); i++)
        {
            generate_cloud_x = generate_cloud->points[i].x;
            generate_cloud_y = generate_cloud->points[i].y;
            distance(generate_cloud_x, generate_cloud_y, min_dt, max_dt, target_cloud);
            lylord_delta[i] = max_dt - min_dt;
            // std::cout<<" lylord_delta" <<  i <<"  :"<< lylord_delta[i]  << std::endl;
            if(lylord_delta[i] <= lylord_devlate)
            {
                lylord_devlate = lylord_delta[i];
                // std::cout<<"迭代最小的距离差值lylord_devlate_____" << lylord_devlate  << std::endl;
                x = generate_cloud->points[i].x;
                y = generate_cloud->points[i].y;
                new_change = true ;
                // std::cout<<" x1______" << x <<" _____y1______" << y<< std::endl;
            }
        }
        std::cout<<" new_change______" << new_change << std::endl;
        std::cout<<"迭代的结果" << " x_:" << x <<" , y_:" << y<< " , lylord_devlate_:" << lylord_devlate <<std::endl;

        if( new_change && (lylord_devlate/2)> 0.010)
        {
            pcl::PointXYZ lylord_Center{(float)x, (float)y, 2.0};
            // Generate3DCircle(generate_cloud, lylord_Center, cloud_normals, lylord_devlate/2);
            Generate3DCircle(generate_cloud, lylord_Center, cloud_normals, lylord_devlate/(2+number));
            std::stringstream ss;
            ss << "../PointsCloud/lyord_" << number << ".pcd";
            writer.write<pcl::PointXYZ> (ss.str (), *generate_cloud); // 保存文件

            lylord_flag = true;
        }
        else
        {
            lylord_flag = false;
        }

        number++;


        if(new_change && number > 20)
        {
            lylord_flag = false;
        }
        
    }while(lylord_flag);

    if(new_change)
    {
        lylord_x = x;
        lylord_y = y;
    }
    else{
        lylord_x = c[0];
        lylord_y = c[1];
    }

    std::cout<<"优化后的中心lylord_x: " << lylord_x << ", lylord_y: "<< lylord_y << ", lylord_devlate: " << lylord_devlate << ", number: "<< number << std::endl;
    return true;
}