一个基于特征的统计突变而分割点云的处理过程及代码

最近需要处理优化点云，从一个点云数据中提出目标点云，如下图，我要提取出更大的那个椭圆点云。

分析：那个比较小的点云数据和比较大的点云数据，虽然在同一个平面上，但其在Y轴方向的位置上是有明显差异的，且我始终需要提取出最大的椭圆点云。
一开始使用了椭圆拟合，聚类分等方法，都看起来很复杂，而且吧只要涉及到距离容忍度都要调试设置。
理想思路：
分析数据发现，这个点云在Y坐标上完全分开的，如果我们能基于点云求得Y最大和最小点的坐标，然后作一条直线，然后计算直线与点云的交点（最近距离点），这样4个交点就把点云分成两段，我就可以基于分段的坐标点对点云过滤了。

确实，求得了如上4个点，再基于如下代码即可计算过滤出目标点云

pcl::PassThrough<pcl::PointXYZ> passFilter;
passFilter.setInputCloud(Inputcloud);
passFilter.setFilterFieldName("y");
passFilter.setFilterLimits(fStart, fEnd);//fStart, fEnd是如上交点的Y坐标
passFilter.filter(*Outputcloud);

但是，困难在于这些点云是散点，不太可能真的与直线有交点，其实需要求的还是距离最近的点，又会涉及到距离筛选和容忍度了。容忍度，做点云处理的人最最熟悉不过了。。。

那么还是来点不一样的思路吧，这也是今天记录这篇博客的原因~
因为经历了计算了椭圆拟合、面积比较，还有这个求直线、计算距离最近点这些过程，最后还是想用统计分段的方式去解决。
处理思路梳理：
（1）已知点云在Y轴上表现出了明显的类间差异，所以需要先将所有的点云投影到Y轴，其实就是提出Y坐标
（2）对Y坐标进行冒泡排序，没错，就是冒泡排序
（3）计算排序后的相邻坐标突变位置，其实就是计算排序后的相邻差值，找到差值最值
因为同类点云经过冒泡以后，相邻的突变其实很小的（类内的点云数据密度相对有保障），但类间的Y可是断崖是的突变。
（4）基于相邻差值最值反映射出适合分离两个类的Y值
只要找到差值突变值，然后求得其对应的两个位置，求得两个坐标的中值，那么基于Y坐标的分类阈值就get了。

我们的目标是求得更大的椭圆点云，而干扰点云与目标点云间有足够的距离，且目标点云和干扰点云都有自己规整的形状轮廓，这样类内部的相邻坐标就不存在那么大的突变，从而可以采用捕捉类间突变的思路。
（5）直接上代码段

可直接使用

        //-------------------过滤多余点-------------------------------
        std::vector<double> y_values;
        double fMin = mesh_filteredSecond->points[0].y, fMax = mesh_filteredSecond->points[0].y;
        //mesh_filteredSecond 为过滤前的点云
        for (const auto& point : mesh_filteredSecond->points) {
            if (fMin > point.y)
            {
                fMin = point.y;
            }
            if (fMax < point.y)
            {
                fMax = point.y;
            }
            y_values.push_back(point.y);
        }
        //冒泡排序
        bubble_sort(y_values, mesh_filteredSecond->points.size()); 

        //计算前半段差值
        std::vector<double> y_Delta1;
        int nStartY = 1;
        int nEndY = y_values.size()/2;
        for (int i = nStartY; i < nEndY; i++)
        {
            y_Delta1.push_back(y_values[i] - y_values[i - 1]);
        }
        double fMaxd1 = y_Delta1[0];
        int nInedx1 = 0;
        for (int i = 0; i < y_Delta1.size(); i++)
        {
            if (fMaxd1 < y_Delta1[i])
            {
                fMaxd1 = y_Delta1[i];
                nInedx1 = i;
            }
        }
        //计算后半段差值
        std::vector<double> y_Delta2;
        nStartY = y_values.size() / 2;
        nEndY = y_values.size();
        for (int i = nStartY; i < nEndY; i++)
        {
            y_Delta2.push_back(y_values[i] - y_values[i - 1]);
        }
        double fMaxd2 = y_Delta2[0];
        int nInedx2 = 0;
        for (int i = 0; i < y_Delta2.size(); i++)
        {
            if (fMaxd2 < y_Delta2[i])
            {
                fMaxd2 = y_Delta2[i];
                nInedx2 = i+ nStartY;
            }
        }
   
        if (fabs(fMaxd2 - fMaxd1) < 0.08) //突变阈值比较
        {
            if( (fMax - fMin) < 0.35)//椭圆过滤半径极限
            {
                pcl::copyPointCloud(*mesh_filteredSecond, *mesh_Finalfiltered);
            }
            else//三分类情况
            {
                int nlen1 = nInedx2 -nInedx1;
                int nlen2 = y_values.size() - nInedx2;                    
                double fY1 = (y_values[nInedx1] + y_values[nInedx1 + 1]) / 2;
                double fY2 = (y_values[nInedx2-1] + y_values[nInedx2 ]) / 2; 
                if ((nlen1 > nInedx1 && nlen1 > nlen2) || fabs(fY1+ fY2)<0.05)
                {
                    doPassThrough("y", fY1 , fY2, mesh_filteredSecond, mesh_Finalfiltered);
                }
                else if (nlen1 > nInedx1) 
                {
                    doPassThrough("y", fY2,fMax+0.001, mesh_filteredSecond, mesh_Finalfiltered);
                }
                else if (nlen1 > nlen2)
                {
                    doPassThrough("y", fMin - 0.001, fY1, mesh_filteredSecond, mesh_Finalfiltered);
                }
                else 
                {
                    if (nInedx1 > nlen2)
                    {
                        doPassThrough("y", fMin - 0.001, fY1, mesh_filteredSecond, mesh_Finalfiltered);
                    }
                    else
                    {
                        doPassThrough("y", fY2, fMax + 0.001, mesh_filteredSecond, mesh_Finalfiltered);
                    }
                }
            }
        }
        else 
        {
            if (fMaxd1 > fMaxd2) 
            {
                double fY = (y_values[nInedx1] + y_values[nInedx1 + 1]) / 2;
                doPassThrough("y", fY, fMax + 0.001, mesh_filteredSecond, mesh_Finalfiltered);
            }
            else 
            {
                double fY = (y_values[nInedx2-1] + y_values[nInedx2]) / 2;
                doPassThrough("y", fMin - 0.001, fY, mesh_filteredSecond, mesh_Finalfiltered);
            }
        }

冒泡排序

void bubble_sort(std::vector<double> &y_values, int len)
{
    int i, j;
    double temp = 0.f;
    for (i = 0; i < len - 1; i++)
        for (j = 0; j < len - 1 - i; j++)
            if (y_values[j] > y_values[j + 1])
            {
                temp = y_values[j];
                y_values[j] = y_values[j + 1];
                y_values[j + 1] = temp;
            }
}