激光雷达获取中心线 中位线 中间线 获取两边障碍物的中间线

效果如图：

环境如下：
c++
pcl:1.81
opencv:3.2

float hight_min=-0.1;
float hight_max=0.3;
float SAC_max_error=0.2;
int line_points_num_min=50;
float down_size_leaf=0.05;
float line_distance_max=2.0;
float line_angle_max_error = 0.01;


bool get_zxx(PointCloudType_::Ptr cloud, PointCloudType_::Ptr  out ){

    PointCloudType_  cloud_out_line;
    PointCloudType_  cloud_in;
    std::vector<PointCloudType_> pc_line;

    int point_num = cloud->size();
    for(int i=0;i<point_num;i++){
        PointType_ added_pt;
        if(cloud->points[i].z<hight_min||cloud->points[i].z>hight_max)
            continue;
        added_pt.x = cloud->points[i].x;
        added_pt.y = cloud->points[i].y;
        added_pt.z = 0;
        cloud_in.push_back(added_pt);
    }

    pcl::VoxelGrid<PointType_> sor;
    sor.setInputCloud(cloud_in.makeShared());
    sor.setLeafSize(down_size_leaf, down_size_leaf, down_size_leaf);
    sor.filter(cloud_in);

    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	pcl::SACSegmentation<PointType_> seg;               // 创建拟合对象
	seg.setOptimizeCoefficients(true);                     // 设置对估计模型参数进行优化处理
	seg.setModelType(pcl::SACMODEL_LINE);                  // 设置拟合模型为直线模型
	seg.setMethodType(pcl::SAC_RANSAC);                    // 设置拟合方法为RANSAC
	seg.setMaxIterations(100);                             // 设置最大迭代次数
	seg.setDistanceThreshold(SAC_max_error);                       // 判断是否为模型内点的距离阀值/设置误差容忍范围

    point_num = cloud_in.size();

    int k=0;
	while (k < 2 && cloud_in.points.size() > 0.2 * point_num){
		pcl::ModelCoefficients coefficients;
		seg.setInputCloud(cloud_in.makeShared());                         // 输入点云						 
		seg.segment(*inliers, coefficients);              // 内点的索引，模型系数

		PointCloudType_::Ptr cloud_line(new PointCloudType_);
		PointCloudType_::Ptr outside(new PointCloudType_);
		if (inliers->indices.size() > line_points_num_min) // 判断提取直线上的点数是否小于20个点，小于的话该直线不合格
		{
			// lineCoff.push_back(coefficients);             // 将参数保存进vector中
			pcl::ExtractIndices<PointType_> extract;   // 创建点云提取对象
			extract.setInputCloud(cloud_in.makeShared());
			extract.setIndices(inliers);
			extract.setNegative(false);                   // 设置为false，表示提取内点
			extract.filter(*cloud_line);

			extract.setNegative(true);                    // true提取外点（该直线之外的点）
			extract.filter(*outside);                     // outside为外点点云
			cloud_in.makeShared() .swap(outside);                          // 将cloud_f中的点云赋值给cloud
            line_point[k]=cloud_line->size();

            pc_line.push_back(*cloud_line);
		}
		else
		{
			break;
		}
		k++;
    }

    if(pc_line.size()!=2){
        pc_line.clear();    
        return 0;
    }


    int i=0;
    float k_[2];
    float dis[2];

    cv::Mat matA1(2, 2, CV_32F, cv::Scalar::all(0));
    cv::Mat matD1(1, 2, CV_32F, cv::Scalar::all(0));
    cv::Mat matV1(2, 2, CV_32F, cv::Scalar::all(0));
    int index=0;
    float x[2];
    float y[2];

    for(i=0;i<pc_line.size();i++){
        float cx = 0, cy = 0, cz = 0;        
        int num = pc_line[i].size();
        for(int j=0;j<num;j++){
            cx += pc_line[i].points[j].x;
            cy += pc_line[i].points[j].y;
        }
        if(pc_line.size()==0) return 0;
        cx/=num;
        cy/=num;        

        float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
        for (int j = 0; j < num; j++) {
            float ax = pc_line[i].points[j].x - cx;
            float ay = pc_line[i].points[j].y - cy;
            a11 += ax * ax; 
            a12 += ax * ay; 
            a22 += ay * ay; 
        }
        a11/=num;
        a12/=num;
        a22/=num;
        matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12;
        matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; 
        cv::eigen(matA1, matD1, matV1);
        if (matD1.at<float>(0, 0) < 3 * matD1.at<float>(0, 1)) {
            return 0;
        }
        float x1 = cx + 0.1 * matV1.at<float>(0, 0);
        float y1 = cy + 0.1 * matV1.at<float>(0, 1);
        float x2 = cx - 0.1 * matV1.at<float>(0, 0);
        float y2 = cy - 0.1 * matV1.at<float>(0, 1);

        float x0=0;
        float y0=0;

        float a012 = sqrt(abs(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)))) ;
        float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
        float ld2 = a012 / l12;

        if(ld2>line_distance_max)continue;

        k_[index] = (y2-y1)/(x2-x1);
        dis[index] = ld2;
        x[index] = cx;
        y[index] = cy;

        index++;
    }
    pc_line.clear();    

    if(abs(atan(k_[0])-atan(k_[1]))>line_angle_max_error)
        return 0;

    float angle=(atan(k_[0])+atan(k_[1]))/2;

    float  y_ =(dis[1]-dis[0])*sin(angle);
    float  x_ = (dis[1]-dis[0])*cos(angle);

    for(i=-100;i<100;i++){
        
        PointType_ added_pt;
        added_pt.x = x_+ 0.1*i*cos(angle);
        added_pt.y =  y_+ 0.1*i*sin(angle);
        added_pt.z = 0;
        out->push_back(added_pt);
    }

    return 1;
}