alpha-shape计算二维点云边界--c++实现

alpha-shape原理看过很久了，今天花点时间实现一下：
原理参考：
博客
平面点云边界提取算法研究[D].长沙理工大学,2017.
《综合机载与车载LiDAR数据的建筑物三维重建》
《基于体元的机载LiDAR建筑物提取》
注意：个人原创，转载注明出处，如果感觉有帮助，点个赞，有疑问，欢迎留言交流。

show the codes：

// ashape.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include
#include 
#include
#include
#include

double get2DDist(pcl::PointXYZ p1, pcl::PointXYZ p2)
{
    double dist = std::pow(((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y)), 0.5);
    return dist;
}
int main()
{
    std::string file_name("Plane_0002.pcd");
    pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPCDFile<pcl::PointXYZ>(file_name, *cloud_in);
    //投影到二维平面
    for (auto& point : *cloud_in)
    {
        point.z = 0.0;
    }
    pcl::io::savePCDFile<pcl::PointXYZ>("二维平面.pcd", *cloud_in);
    std::vector<int> boundary_bool(cloud_in->size(), 0);
    double r = 1.8;
    pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
    kdtree.setInputCloud(cloud_in);
    std::vector<int> indices(0,0);
    std::vector<float>dist(0, 0.0);
    for (size_t i = 0; i < cloud_in->size(); ++i)
    {
        pcl::PointXYZ p = cloud_in->points[i];
        kdtree.radiusSearch(cloud_in->points[i], 2*r, indices,dist ,100);
        //indices[0]对应的点云即为查询点本身
        for (size_t j = 1; j < indices.size(); ++j)
        {
            pcl::PointXYZ p1 = cloud_in->points[indices[j]];
            double s_2 = std::pow((p.x - p1.x), 2) +
                std::pow((p.y - p1.y), 2);
            double h = std::pow((r * r / s_2 - 0.25), 0.5);
            double x2 = p.x + 0.5 * (p1.x - p.x) - h * (p1.y - p.y);
            double y2 = p.y + 0.5 * (p1.y - p.y) - h * (p.x - p1.x);
            double x3 = p.x + 0.5 * (p1.x - p.x) + h * (p1.y - p.y);
            double y3 = p.y + 0.5 * (p1.y - p.y) + h * (p.x - p1.x);
            pcl::PointXYZ p2(x2, y2, 0.0);
            pcl::PointXYZ p3(x3, y3, 0.0);
            //计算邻域内除了p1之外的点到p2,p3的距离
            std::vector<double>distp2,distp3;
            std::vector<int>distp2_bool(0, 0), distp3_bool(0, 0);
            int count = 0;
            for (size_t k = 1; k < indices.size(); ++k)
            {
                pcl::PointXYZ p_other = cloud_in->points[indices[k]];
                if (k != j)
                {
                    ++count;
                    double distance_p2 = get2DDist(p_other, p2);
                    double distance_p3 = get2DDist(p_other, p3);
                    //比较距离与r的大小
                    if (distance_p2 > r)
                    {
                        distp2_bool.push_back(1);
                    }
                    if (distance_p3 > r)
                    {
                        distp3_bool.push_back(1);
                    }
                }
            }
            //如果邻域内所有点到p2，或者p3的距离均大于r，则有distp2_bool.size()==count
            //则表明p是边界点,退出循环，不用再计算邻域内点距离了
            if (count == distp2_bool.size() || count == distp3_bool.size())
            {
                boundary_bool[i] = 1;
                break;
            }
        }      
    }
    pcl::PointCloud<pcl::PointXYZ> boundary_cloud;
    for (size_t it=0;it< boundary_bool.size();++it)
    {
        if (boundary_bool[it] == 1)
        {
            boundary_cloud.push_back(cloud_in->points[it]);
        }
    }
    boundary_cloud.height = boundary_cloud.size();
    boundary_cloud.width = 1;
    boundary_cloud.resize(boundary_cloud.height * boundary_cloud.width);
    if (boundary_cloud.size())
    {
        pcl::io::savePCDFile<pcl::PointXYZ>("边界点云.pcd", boundary_cloud);

    }

    std::cout << "Hello World!\n";
}

实验效果：（红色为提取的边界点）

结论：该方法提取边界效率较慢（可以看出来三层循环，速度慢），效果还行（中间有一部分错误边界点）