MyTest——边界检测

实现思路如下：

Step1：大文件的内存映射，多线程数据读取，加快读取速度。

Step2：点云数据预处理（抽希、去噪点）。

Step3：Step2处理后数据使用kdtree进行离散点排序。

Step4：遍历点云数据，对于点i，利用kdtree半径检索，得到每个点的邻近点集，计算点集的重心；判断该i点距离其邻近点集重心的距离，距离作为阈值，进行判断是否在边界。

                                            图1 效果图

                                          图2 细节

核心代码：

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud_filtered);

    PointT search_point;    //存储搜索点

    // neighbors within radius search  根据搜索半径搜索
    std::vector<int> pointidxradiussearch;
    std::vector<float> pointradiussquareddistance;

    float radius = 10;   //256.0f * rand() / (rand_max + 1.0f);

    for (size_t i = 0; i < cloud_filtered->points.size(); i++)
    {
        search_point = cloud_filtered->points[i];
        if (kdtree.radiusSearch(search_point, radius, pointidxradiussearch, pointradiussquareddistance) > 0)
        {
            float centerX = 0;
            float centerY = 0;
            float centerZ = 0;

            for (size_t i = 0; i < pointidxradiussearch.size(); ++i)
            {
                centerX += cloud_filtered->points[pointidxradiussearch[i]].x;
                centerY += cloud_filtered->points[pointidxradiussearch[i]].y;
                //centerZ += cloud_filtered->points[pointidxradiussearch[i]].z;

                /*    std::cout << "    " << cloud->points[pointidxradiussearch[i]].x
                << " " << cloud->points[pointidxradiussearch[i]].y
                << " " << cloud->points[pointidxradiussearch[i]].z
                << " (squared distance: " << pointradiussquareddistance[i] << ")" << std::endl;
                */
            }
            centerX = centerX / pointidxradiussearch.size();
            centerY = centerY / pointidxradiussearch.size();
            //centerZ = centerZ / pointidxradiussearch.size();

            //double distenceTemp = sqrt((search_point.x - centerX)*(search_point.x - centerX) +
            //    (search_point.y - centerY)*(search_point.y - centerY) + (search_point.z - centerZ)*(search_point.z - centerZ));
            double distenceTemp = sqrt((search_point.x - centerX)*(search_point.x - centerX) +
                (search_point.y - centerY)*(search_point.y - centerY));

            if (distenceTemp > 0.4*radius)
            {
                cloudFinal->points.push_back(search_point);
            }
        }
    }