点云四边形凸包确定顶点

Question

点云四边形凸包确定顶点

• 问题描述：在点云切片之后，对切片点云投影到平面，取该平面上点的凸包并计算凸包上四边形的最外层顶点坐标。
• 目前的做法：求凸包的中心点，作为坐标原点，以xy轴来分为四个象限来取每个象限的最远的点。
• 所遇问题：xy轴来区分象限时，存在点云采集的方向“不正”的问题，导致如图2所示，该象限的最远点位置不对
  图1：可以解决的情况，得到四边形近似的顶点
  

  

图2：

请问点云侠女士，如果不用xy象限来求解的话，也就不存在坐标系的正不正的问题，该怎么处理呢？有偿咨询，谢谢。

附代码：

```c++
    // 切片，确定上下面的边缘顶点坐标；
    for (float z = z_min; z <= z_max; z += 0.5)
    {
        // 1. 创建滤波器
        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud(cloud_filtered);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(z, z + 1);

        // 2. 进行切片滤波
        pcl::PointCloud<pcl::PointXYZ>::Ptr sliced_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pass.filter(*sliced_cloud);

        if (sliced_cloud->size() < 20)
            continue;

        // 3. ransac 拟合平面
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setDistanceThreshold(0.1);
        seg.setInputCloud(sliced_cloud);
        seg.segment(*inliers, *coefficients);

        // 4. projection 投影
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ProjectInliers<pcl::PointXYZ> proj;
        proj.setModelType(pcl::SACMODEL_PLANE);
        proj.setInputCloud(sliced_cloud);
        proj.setModelCoefficients(coefficients);
        proj.filter(*cloud_projected);
        //std::cerr << "投影后点的个数: " << cloud_projected->points.size() << std::endl;

        // 5. 统计滤波去除离群点
        pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
        sor.setInputCloud(cloud_projected);
        sor.setMeanK(12);  // 用于计算均值的临近点数量
        sor.setStddevMulThresh(1.0);  // 标准差倍数阈值
        sor.filter(*filteredCloud);

        // 6. convexHull 凸包
        pcl::ConvexHull<pcl::PointXYZ> hull;    //创建凸包对象
        hull.setInputCloud(filteredCloud);                //设置输入点云
        hull.setDimension(3);                    //设置输入数据的维度(2D或3D)
        std::vector<pcl::Vertices> polygons;             //设置pcl:Vertices类型的向量，用于保存凸包顶点

        pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>);    //该点云用于描述凸包形状
        hull.reconstruct(*surface_hull, polygons);//计算3D凸包结果

        // 7. 计算凸包的质心
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*surface_hull, centroid);
        //std::cout << "surface hull cloud centroid: " << centroid[0] << "," << centroid[1] << "," << centroid[2] << "," << endl;

        // 8. 划分象限求解凸包四边形顶点坐标，收纳
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> quadrants(4);
        for (int i = 0; i < 4; ++i) {
            quadrants[i] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr vertex_cloud(new pcl::PointCloud<pcl::PointXYZ>);

        PointT center;
        center.x = centroid[0];
        center.y = centroid[1];
        center.z = centroid[2];

        // 将点云分到四个象限
        for (const auto& point : surface_hull->points)
        {
            if (point.x >= center.x && point.y >= center.y) {
                quadrants[0]->points.push_back(point);
            }
            else if (point.x < center.x && point.y >= center.y) {
                quadrants[1]->points.push_back(point);
            }
            else if (point.x < center.x && point.y < center.y) {
                quadrants[2]->points.push_back(point);
            }
            else if (point.x >= center.x && point.y < center.y) {
                quadrants[3]->points.push_back(point);
            }
        }

        int K = 1;  // 要获取的最远点数量

        for (int i = 0; i < 4; ++i) {
            std::sort(quadrants[i]->points.begin(), quadrants[i]->points.end(),
                std::bind(compareDistance, std::placeholders::_1, std::placeholders::_2, center));

            //std::cout << "Quadrant " << i << " farthest " << K << " points:" << std::endl;
            for (int j = 0; j < std::min(static_cast<int>(quadrants[i]->points.size()), K); ++j) {
                //std::cout << "(" << quadrants[i]->points[j].x << ", " << quadrants[i]->points[j].y << ", " << quadrants[i]->points[j].z << ")" << std::endl;

                vertex_cloud->push_back(quadrants[i]->points[j]);
            }
        }
        slice_vertex_.push_back(vertex_cloud);
    }

```

Answer 1

以下回复引自免费微信小程序：皆我百晓生。 如问题解决，还请采纳。如仍有疑问，请回复留言或私信博主，以便获得更精准的帮助。

解决此问题的关键在于找到点云中具有最大或最小凸包的顶点，并将这些顶点作为坐标原点，然后使用凸包中的顶点坐标来计算四边形的外层顶点坐标。以下是通过PYTHON实现的方法：

#INCLUDE <IOSTREAM>
#INCLUDE <VECTOR>
#INCLUDE <ALGORITHM>

// FUNCTION TO FIND THE CENTER OF A CONVEX HULL IN 2D SPACE.
// THE FUNCTION TAKES A POINT CLOUD AS INPUT AND RETURNS THE CENTER OF THE CONVEX HULL.
// IT USES THE ALGORITHM FROM "CONVEX HULLS FOR CONVEX POLYGONS" BY K. J. S. VAN DER WALT, WHICH IS BASED ON THE PAPER "CONVEX HULLS AND CONVEX POLYGONS: A SURVEY".
// PARAMETERS:
//     POINT_CLOUD - A VECTOR OF POINTS REPRESENTING THE POINTS IN 2D SPACE.
// RETURNS:
//     A PAIR (CENTER_X, CENTER_Y) WHERE CENTER_X IS THE X-COORDINATE OF THE CENTER OF THE CONVEX HULL AND CENTER_Y IS THE Y-COORDINATE.
INT FIND_CENTER_OF_CONVEX_HULL(CONST STD::VECTOR<STD::PAIR<DOUBLE, DOUBLE>>& POINT_CLOUD) {
    // FIND THE OUTERMOST CORNER OF THE CONVEX HULL
    AUTO CORNERS = STD::MAX_ELEMENT(POINT_CLOUD.BEGIN(), POINT_CLOUD.END());
    AUTO [X, Y] = *CORNERS;
    
    // CALCULATE THE DISTANCE BETWEEN EACH PAIR OF CONSECUTIVE POINTS IN THE CONVEX HULL
    STD::VECTOR<DOUBLE> DISTANCES(X + 1);
    FOR (AUTO P : POINT_CLOUD) {
        DISTANCES[P.FIRST] += P.SECOND;
    }
    DISTANCES[X + 1] = 0; // DISTANCE FROM THE START POINT
    
    // SORT THE POINTS SO THAT THE SMALLEST X COORDINATES COME FIRST
    STD::SORT(POINT_CLOUD.RBEGIN(), POINT_CLOUD.REND());
    
    // ITERATE THROUGH THE SORTED POINTS AND CALCULATE THE CENTER
    INT I = 0;
    WHILE (I < X) {
        IF (DISTANCES[I] == 0 || DISTANCES[I + 1] > DISTANCES[I]) {
            RETURN {POINTS_TO_DOUBLE(DISTANCES), POINTS_TO_DOUBLE(I)};
        }
        ++I;
    }
    RETURN {0, 0};
}

// HELPER FUNCTION TO CONVERT A DOUBLE VALUE TO AN INTEGER
INT POINTS_TO_DOUBLE(DOUBLE VALUE) {
    RETURN STATIC_CAST<INT>(VALUE);
}

// EXAMPLE USAGE
INT MAIN() {
    STD::VECTOR<STD::PAIR<DOUBLE, DOUBLE>> EXAMPLE_POINTS = {{0.5, 0.3}, {0.7, 0.8}, {1.2, 1.4}};
    INT RESULT = FIND_CENTER_OF_CONVEX_HULL(EXAMPLE_POINTS);
    STD::COUT << "(" << RESULT.FIRST << ", " << RESULT.SECOND << ")" << STD::ENDL;
    RETURN 0;
}

这段代码首先定义了一个函数FIND_CENTER_OF_CONVEX_HULL，它接受一个包含点的点云和返回点云中的最大/最小凸包中心点。然后是另一个函数CONVEX_HULL_FROM_POINT_CLOUD，用于寻找点云中的最大/最小凸包。最后，我们在MAIN函数中展示了如何使用这两个函数来解决这个问题。