点云随笔（七）鞋带公式计算投影面积

       投影面面积采用格网法计算时，主要还是根据网格尺寸来决定，网格设置的越小，面积也就越准确，但是也会造成计算上的冗余。为此可以基于鞋带公式进行投影面面积的计算工作。

       首先需要进行的是将贴合边界统计出来，这就需要使用凹包算法，具体不过多在此处赘述，该算法主要将点云数据内凹型边界提取出来，然后采用中心点方位角法进行点云边界排序处理:

       式中为边界点与边界点云中心点的y值作差，为边界点与边界点云中心点的x值作差， angle则为每个点与中心点计算所得的方位角，然后基于方位角对边界点云按照大小进行排序，排序后基于高斯面积公式进行道路危险区投影面面积计算：

        式中为当前点坐标值， 为下一点坐标值。

double area_shoelace_formula(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) {//鞋带公式计算投影面积和凹包算法
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull(new pcl::PointCloud<pcl::PointXYZ>), cloud2(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ConcaveHull<pcl::PointXYZ> chull;
    chull.setInputCloud(cloud);
    chull.setAlpha(1);
    chull.reconstruct(*cloud_hull);

    pcl::PointXYZ maxpt, minpt;
    pcl::getMinMax3D(*cloud_hull, minpt, maxpt);
    double xx = (maxpt.x + minpt.x) / 2, yy = (maxpt.y + minpt.y) / 2;
    vector<pair<double, int>> pairs;

    for (int i{ 0 }; i < cloud_hull->size(); i++) {

        double angle{ 0 }, delta_x = cloud_hull->points[i].x - xx, delta_y = cloud_hull->points[i].y - yy;
        //第一象限
        if (delta_x > 0 && delta_y > 0) {
            angle = (atan(delta_y / delta_x)) * 180 / PI;
        }
        //第二象限
        else if (delta_x < 0 && delta_y > 0) {
            angle = 180 - (atan(delta_y / abs(delta_x))) * 180 / PI;
        }
        //第三象限
        else if (delta_x < 0 && delta_y < 0) {
            angle = 180 + (atan(abs(delta_y) / abs(delta_x))) * 180 / PI;
        }
        //第四象限
        else if (delta_x > 0 && delta_y < 0) {
            angle = 360 - (atan(abs(delta_y) / delta_x)) * 180 / PI;
        }
        pairs.push_back(make_pair(angle, i));
    }
    sort(pairs.begin(), pairs.end());
    double area{ 0 };
    for (int i = 0; i < cloud_hull->size()-1; i++) 
    {
        area += cloud_hull->points[pairs[i].second].x * cloud_hull->points[pairs[i + 1].second].y - cloud_hull->points[pairs[i + 1].second].x * cloud_hull->points[pairs[i].second].y;
    }
    double area_add = cloud_hull->points[pairs[cloud_hull->size() - 1].second].x * cloud_hull->points[pairs[0].second].y - cloud_hull->points[pairs[0].second].x * cloud_hull->points[pairs[cloud_hull->size() - 1].second].y;
    area = abs(area+area_add) / 2;
    cout << "凹包边界-多边形面积公式计算得到面积为" << area << endl;
    return area;
}