本篇博客主要记录lidar点云的分割以及聚类。

1.Segmentation 点云分割
对于lidar反馈回来的数据，我们总是希望从中准确的找到那些是障碍物，那些是道路点。为此，udacity课程中使用了RANSAC算法来进行点云的分割，将lidar点云分为障碍物点（外点）和道路点（内点）两类。
1.1 RANSAC
对于RANSAC算法（随即抽样一致算法），初次接触是在高博的视觉SLAM14讲中，该算法可以从一组包含“局外点”的数据中通过迭代的方式估计数学模型的参数，从而找到符合模型的“局内点”。这里不多做介绍，详细原理可以推荐一篇博客
RANSAC深度解析
在udacity的课程中，也对RANSAC的原理进行了简要介绍和仿真。RANSAC是一种检测数据中异常值的方法。 RANSAC运行最大迭代次数，并返回最合适的模型。 每次迭代都会随机选择数据的子样本，并通过它拟合模型，例如直线或平面。 然后，将具有最大数量的内部值或最低噪声的迭代用作最佳模型。一种类型的RANSAC选择适合的最小点子集。 对于一条线，这将是两个点，对于平面，则将是三个点。 然后，通过迭代遍历每个剩余点并计算其与模型的距离，来计算内部数。 距模型一定距离内的点计为内部值。 那么，具有最大内部数的迭代就是最佳模型。 这也是udacity课程中实现的版本。RANSAC的其他方法可以对模型点的某些百分比进行采样，例如占总点数的20％，然后对其拟合一条线。 然后计算该行的误差，误差最小的迭代就是最佳模型。 由于不需要考虑每次迭代的每个点，因此该方法可能具有一些优点。
1.2 RANSAC仿真
接下来是ransac的仿真部分代码，这里只记录平面点云的RANSAC分割

main函数

int main ()
{
    // Create viewer 创建可视化窗口
    pcl::visualization::PCLVisualizer::Ptr viewer = initScene();
    // Create data  创建点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = CreateData3D();
    // TODO: Change the max iteration and distance tolerance arguments for Ransac function 设定最大迭代次数和距离阈值
    std::unordered_set<int> inliers = Ransac(cloud, 100, 0.5);
    pcl::PointCloud<pcl::PointXYZ>::Ptr  cloudInliers(new       pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOutliers(new pcl::PointCloud<pcl::PointXYZ>());
    
    for(int index = 0; index < cloud->points.size(); index++)
    {
        //对点云进行分割
        pcl::PointXYZ point = cloud->points[index];
        if(inliers.count(index))
            cloudInliers->points.push_back(point);
        else
            cloudOutliers->points.push_back(point);
    }
    // Render 2D point cloud with inliers and outliers，对分割后的点云进行渲染 内点绿色，外点红色
    if(inliers.size())
    {
        renderPointCloud(viewer,cloudInliers,"inliers",Color(0,1,0));
        renderPointCloud(viewer,cloudOutliers,"outliers",Color(1,0,0));
    }
    else
    {
        //其他背景黑色
        renderPointCloud(viewer,cloud,"data");
    }

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce ();
    }

}

加载pcd函数

//加载pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr CreateData3D()
{
    ProcessPointClouds<pcl::PointXYZ> pointProcessor; //创建pointProcessor
    return pointProcessor.loadPcd("../../../sensors/data/pcd/simpleHighway.pcd"); //加载data中的pcd文件
}

创建可视化窗口并初始化

//创建窗口并初始化函数
pcl::visualization::PCLVisualizer::Ptr initScene()
{
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer ("2D Viewer"));//创建可视化窗口
    viewer->setBackgroundColor (0, 0, 0); //背景颜色黑色
    viewer->initCameraParameters(); //初始化窗口参数
    viewer->setCameraPosition(0, 0, 15, 0, 1, 0);//窗口位置及视角
    viewer->addCoordinateSystem (1.0);//创建坐标系
    return viewer;
}

RANSAC算法

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
    std::unordered_set<int> inliersResult; //创建关联容器 用于储存内点  std::unordered_set是SLT中的一种容器，基于RB-Tree结构，有机会后面介绍
    srand(time(NULL));
    auto startTime = std::chrono::steady_clock::now();

    // TODO: Fill in this function
    //算法步骤
    // For max iterations
    // Randomly sample subset and fit line
    // Measure distance between every point and fitted line
    // If distance is smaller than threshold count it as inlier
    // Return indicies of inliers from fitted line with most inliers
    while(maxIterations--)
    {
        std::unordered_set<int> inliers; //内点容器
        while(inliers.size()<3)
            inliers.insert(rand()%(cloud->points.size())); //随机选3个点组成平面插入进inlier中

        float x1,x2,x3,y1,y2,y3,z1,z2,z3;

        auto itr = inliers.begin();//将inlier的开始地址赋给itr
        x1 = cloud->points[*itr].x;
        y1 = cloud->points[*itr].y;
        z1 = cloud->points[*itr].z;
        itr++;
        x2 = cloud->points[*itr].x;
        y2 = cloud->points[*itr].y;
        z2 = cloud->points[*itr].z;
        itr++;
        x3 = cloud->points[*itr].x;
        y3 = cloud->points[*itr].y;
        z3 = cloud->points[*itr].z;

        //三点平面公式计算 a b c d
        float a = (y2-y1)*(z3-z1)-(z2-z1)*(y3-y1);
        float b = (z2-z1)*(x3-x1)-(x2-x1)*(z3-z1);
        float c = (x2- x1)*(y3-y1)-(y2-y1)*(x3-x1);
        float d = -(a*x1+b*y1+c*z1);


        for(int index = 0;index < cloud->points.size(); index++)
        {
            if(inliers.count(index) > 0)
                continue;

            pcl::PointXYZ point = cloud->points[index];
            float x3 = point.x;
            float y3 = point.y;
            float z3 = point.z;
            //计算点到平面距离公式
            float d = fabs(a*x3+b*y3+c*z3+d)/sqrt(a*a+ b*b+c*c);

            if(d <= distanceTol)
                inliers.insert(index); //小于阈值，判定为内点
        }

        if(inliers.size() > inliersResult.size())
        {
            inliersResult = inliers; //返回最优结果
        }
    }
    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "Ransac took " << elapsedTime.count() << " milliseconds" << std::endl;

    return inliersResult;

}

结果如图 红色为障碍物，绿色为道路点
在课程的工程中，直接调用了PCL库中的RANSAC功能进行点云的分割。其原理与仿真一致。

在lidar障碍物检测工程中的对应部分

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold)
{
    // Time segmentation process
    auto startTime = std::chrono::steady_clock::now();

    // TODO:: Fill in this function to find inliers for the cloud.
    //过滤之后的点云数据
    //创建分割对象 -- 检测平面参数
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers {new pcl::PointIndices};   //存储内点的点索引集合对象inliers
    pcl::ModelCoefficients::Ptr coefficients{new pcl::ModelCoefficients};

    seg.setOptimizeCoefficients(true);       //设置对估计的模型系数需要进行优化
    seg.setModelType(pcl::SACMODEL_PLANE);  //设置模型类型，检测平面
    seg.setMethodType(pcl::SAC_RANSAC);     //设置方法（随机样本一致性）
    seg.setMaxIterations(maxIterations);    //最大迭代次数
    seg.setDistanceThreshold(distanceThreshold);   //距离阈值

    seg.setInputCloud(cloud);
    seg.segment(*inliers,*coefficients);    //分割操作

    if(inliers->indices.size() == 0)
    {
        std::cout<<"could not estimate a planar model for the given dataset."<<std::endl;
    }


    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);
    return segResult;
}

这个函数在environment.cpp文件中被调用

 std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI.SegmentPlane(inputCloud,25,0.3)

inputCloud为输入点云，是经过滤波处理后的，20为最大迭代次数，0.3为判定阈值，小于阈值的点判定为内点。在SegmentPlane函数最后有这样一条语句

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);

这条语句中调用了SeparateClouds函数，这个函数也在processPointCloud.cpp文件中，函数功能是将提取点云并存为平面点云和目标物点云

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud)
{
    // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
    typename pcl::PointCloud<PointT>::Ptr obstCloud (new pcl::PointCloud<PointT> ()); //目标点云指针
    typename pcl::PointCloud<PointT>::Ptr planeCloud (new pcl::PointCloud<PointT> ());//平面点云指针

    for(int index: inliers->indices)
        planeCloud->points.push_back(cloud->points[index]);

    pcl::ExtractIndices<PointT> extract;  //ExtractIndices通过分割算法提取部分点云数据子集的下标索引
    extract.setInputCloud (cloud); //设置输入点云
    extract.setIndices(inliers); //提取内点
    extract.setNegative(true); //提取外点，继续处理
    extract.filter(*obstCloud);//将外点（即目标点）提取至obstCloud

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(obstCloud, planeCloud);//储存两种点云
    return segResult;
}

2.聚类Clustering
在对点云进行分割后，工程已经可以将lidar的返回点云数据分割成目标物（障碍物）点云和平面（车道平面）点云，接下来要完成的任务就是将障碍物的点云进行聚类，将属于一个障碍物的点云归为一类。课程中使用的聚类方法是欧氏距离聚类，这个算法是根据点之间的紧密程度来关联点组。 为了有效地进行最近邻居搜索，可以使用KD-Tree数据结构，该结构平均可以将查找时间从O（n）加快到O（log（n））。相关资料可以参考以下文章：
KD-Tree与欧式距离聚类
udacity的工程中使用的是PCL库的欧式聚类算法，但同样对采用KD-Tree结构进行欧式聚类的方法进行的简要介绍和仿真。这里不过多记录，直接看工程中的实现部分
在environment.cpp函数中下面语句调用了Clustering函数

std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI.Clustering(segmentCloud.first,0.53,10,500);  //障碍物点云，采用KdTree将其进行欧式聚类

Clustering函数有三个参数
参数1为输入点云，这里segmentCloud.first表示是障碍物点云，我们对障碍物点云进行聚类
参数2为设置欧式聚类的近邻搜索的搜索半径，半径越大，搜索范围越大
参数3、4为一个类中点的上限和下限

Clustering函数定义如下

std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

    // Time clustering process
    auto startTime = std::chrono::steady_clock::now();

    std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;

    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
    typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);    //kd树，是一种分割k维数据空间的数据结构。

    tree->setInputCloud(cloud);  //输入点云

    std::vector<pcl::PointIndices> clusterIndices;   //找到簇的索引
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance(clusterTolerance);   //设置近邻搜索的搜索半径
    ec.setMinClusterSize(minSize); //设置一个类所需要的最少点
    ec.setMaxClusterSize(maxSize); //设置一个类点云的上限
    ec.setSearchMethod(tree);    //设置点云的搜索机制，此处为KdTree
    ec.setInputCloud(cloud);
    ec.extract(clusterIndices);     //从点云中提取聚类，并保存在点云索引clusterIndices中

    for(pcl::PointIndices getIndices : clusterIndices)
    {
        typename pcl::PointCloud<PointT>::Ptr cloudCluster (new pcl::PointCloud<PointT>);

        for(int index : getIndices.indices)
            cloudCluster->push_back(cloud->points[index]);

        cloudCluster->width = cloudCluster->points.size();
        cloudCluster->height = 1;
        cloudCluster->is_dense = true;

        clusters.push_back(cloudCluster);
    }
    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl;

    return clusters;     //返回聚类
}

拿聚类仿真的图片来说明以下效果吧，同样颜色的点被分为一类。这也是KD-Tree的数据结构

至此，点云的分割和聚类操作已经完成，为方便观看，接下来要对一类的点云进行修饰，用一个立方体将一类的点云数据框起来， 这部分代码相对简单，直接上代码

Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster)
{

    // Find bounding box for one of the clusters
    PointT minPoint, maxPoint;
    pcl::getMinMax3D(*cluster, minPoint, maxPoint
    );

    Box box;
    box.x_min = minPoint.x;
    box.y_min = minPoint.y;
    box.z_min = minPoint.z;
    box.x_max = maxPoint.x;
    box.y_max = maxPoint.y;
    box.z_max = maxPoint.z;

    return box;
}

这部分在传感器融合的第一篇文章中也记录过，不再赘述。
最后的结果就是下面这样
绿色点为车道平面点云 障碍物的点云分别渲染成了不同颜色，并对一类的点云，用立方体进行包围。每一个立方体就代表一个障碍物。

点云数据是LiDAR的主要数据，尤其是早期的LiDAR数据处理都是针对点云数据进行的。机载LiDAR激光脚点的分布是按照时间序列进行采样和存储的，其在地面上的分布不是规则的，其空间分布呈现为离散的数据“点云”。这些点中，有些点位于真实地形表面上，有些位于不同的地物上（房屋、管线、烟囱等），还有些落在植被上（数木、灌木、草)等。

激光脚点分布

LiDAR点云数据中信息的分类提取，即点云数据的分类工作在整个 LiDAR数据后处理过程中占 60%-80%的工作量，但目前还没有一种全能的方法能有效的提取复杂地形中的各种信息。且现有的分类算法的具体操作细节也由于版权问题极少公开，这也增加了分类算法研究的难度。机载LiDAR数据采集具有一定的盲目性，激光脚点的位置是随机的，不能保证地形特征点和地物特征点能有数据，这给激光脚点性质的判别带来了很大的困难，使机载LiDAR数据的分类很困难。

LiDAR点云数据的分类可分为一级分类和二级分类，一级分类主要指区分地面点和地物点，而二级分类是指在一级分类的基础上对地物点集的再分类，主要分类类别包括有植被、建筑物、道路信息和电力线等等。一般的分类方法还停留在一级分类上。
国外 LiDAR点云的分类算法的研究起步较早，发展的分类方法理论基础主要分成四类：基于坡度理论的分类方法、最小区域分类方法、基于面理论的分类方法和聚类/分割算法。
1、坡度理论
计算相异的两点间的坡度值或高差，当这个坡度值或高差超过设定的阈值时，就认为其中的较高点就是地物点（如图 2-27（a）所示）。这种理论的基础是假设陡坡只会出现在地物数据集中。
2、最小区域理论

设定一个水平面，并在其垂直方向上设置一定区域作为缓冲区，包含在这个三维缓冲区域内的点都划为地面点集（如图2-27（b）所示）。

3、面理论
不同于最小区域理论，面理论选择的面是一个由参数计算的不规则的面，在面上也设定一个缓冲区，包含在这个三维缓冲区域内的点划为地面点集（如图 2-27（c）所示）。
4、聚类/分割算法
用周围邻近区点域进行分类时，聚类/分割算法的理论依据是一个类中所有的点共同来描述一个完整的物体，而非物体中单独的面（如图2-27（d）所示）。聚类算法是由 Filin（2002） 和 Roggero（2002）提出，分割算法是由 Lee, Schenk 和 Sithole（2002）提出。

目前基于面理论的分类方法使用得最广泛。依据模拟近似地面方法的不同，可以再将分类方法细分为两种：
1、内插的分类方法
线性加权最小二乘内插模拟近似地面（Kraus and Pfeifer，1999）， 计算点到初始模拟面的高差。通常情况下，地物点到初始模拟面的高差为正，而地面点到初始模拟面的高差则为负。在重新计算近似地面时，正高差的权较小，甚至为负；负高差将获取较大的权，甚至接近 1。将分级的概念融合到内插中可以得到一种新的分类方法（Pfeifer et al，2001）。进一步改进，用标准化最小二乘取代最小二乘得到另外一种新方法，但此方法需要预先获取一些参数的值（Lee 和 Younan， 2003）。

2、基于数学形态学原理的分类方法

利用数学形态学的操作运算来获取近似地面，如开运算。数学形态学的方法从概念上来讲，是简单易操作的。只要激光点云数据分辨率足够好，开运算就能有效地剔除地面物信息。如果数据分辨率太低，就需要较大的操作窗口来提出地物信息，相应的一些地面突出特征也会被抹平（Qi Chen，2007）。

图2-27 国外分类方法的分类原理示意图
(a)坡度理论；(b) 最小区域理论；© 面理论；(d) 聚类/分割理论
[参考文献] 王丽英. 机载LiDAR数据误差处理理论与方法[M]. 测绘出版社, 2013

无人驾驶汽车系统入门（三十一）——点云分割和聚类算法详解

本篇详细讲解点云处理中的基本分割和聚类的算法原理。

Lidar基本常识

lidar的分辨率要高于radar，但是radar可以直接测量目标的速度。通过融合两者，可以获得对目标较好的位置和速度估计。

激光雷达坐标系：右手法则，大拇指朝上为z，食指朝前为x，中指朝左为y, lidar的解析度很大程度上取决于线数，其解析度指标分为横向解析度和纵向解析度，横向解析度即一条激光束扫描一圈（360度）的反射点的数量，作为参考，Velodyne的16线激光雷达的横向解析度为1800个反射点，即：

$$\frac{360}{1800}=0.2degree/point$$

也就是说这一款雷达横向上每0.2度有一个反射点。纵向解析度即雷达的线束数量，线束越多解析度越高。

常用的point cloud处理工具：

• PCL: 主要用于点云滤波、分割和聚类
• OpenCV
• ROS：提供了一些点云数据表示、消息格式、坐标变化、可视化工具
• Eigen：线性运算库，可用于描述各类变换，通常用于点云的坐标变换（点云SLAM里用的比较多）

传统的点云感知算法处理流程：

1. 点云滤波：降采样点云以加速后续计算
2. 点云分割：分别提取出地面点和非地面点
3. 点云聚类：将非地面点通过空间关系聚类成点云簇
4. 目标轮廓拟合：使用设定形状（如bounding box）将点云簇拟合

本文详细讲解基础的点云分割和点云聚类算法。

点云滤波和降采样

为什么做滤波(filter)和降采样？

滤波是点云预处理的步骤，可以显著减低后续处理的计算复杂度，有些滤波方法还能够滤除点云中的噪点。PCL提供多种点云滤波方法的实现，其中应用最多的是体素滤波：Voxel grid filter. 体素滤波即将空间按照一定尺寸（比如说$1m\times 1m\times 1m$）的立方体格进行划分，每个立方格内仅保留一个点。下图是voxel size为0.3m的降采样结果。

PCL提供各类成熟的点云滤波方法，以体素滤波为例：

typename pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(filterRes, filterRes, filterRes);
typename pcl::PointCloud<PointT>::Ptr ds_cloud(new pcl::PointCloud<PointT>);
voxel_filter.filter(*ds_cloud);

点云分割：RANSAC 算法

RANSAC: RANdom SAmple Consensus, 通过随机采样和迭代的方法用一种模型（比如说直线模型、平面模型）拟合一群数据，在点云处理中通常用于点云数据的分割，以最简单的直线拟合为例：

给定平面内若干个点的集合 P n : { ( x 0 , y 0 ) , ( x 1 , y 1 ) , . . . , ( x n , y n ) } P_n: \{(x_0, y_0), (x_1, y_1), ... , (x_n, y_n)\} Pn​:{(x0​,y0​),(x1​,y1​),...,(xn​,yn​)}，要求得一条直线 $L:ax+by+c=0$ 使得拟合尽可能多的点，也就是 $n$ 个点到直线 $L$ 的距离和最短；RANSAC的做法是通过一定的迭代次数(比如1000次)，在每次迭代中随机在点集 $P_n$ 中选取两个点 $(x1,y1),(x2,y2)$，计算 $a,b,c$:

$$\begin{gathered} a=y1-y2, \\ b=x2-x1, \\ c=x1\ast y2-x2\ast y1 \end{gathered}$$

接着遍历 $P_n$ 每一个点$(x_i,y_i)$到直线 $L$ 的距离 $dist$,

$$dist=\frac{fabs(a\ast x_i+b\ast y_i+c)}{sqrt(a\ast a+b\ast b)}$$

当$dist$小于我们给定的一个距离阈值 $D_{threshold}$，则认为点在L上，穿过点最多的直线即为RANSAC搜索的最优（拟合）的直线。平面拟合同理。

PCL中已经成熟地实现了各类模型地RANSAC拟合，以平面拟合为例：

// cloud are input cloud, maxIterations and
// distanceThreshold are hyperparameters
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

pcl::SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(maxIterations);
seg.setDistanceThreshold(distanceThreshold);

seg.setInputCloud(cloud);
// the indices in inliers are the points in line
seg.segment(*inliers, *coefficients);

点云聚类之用于加速邻近点搜索的数据结构——KD树

KD树是一种用于加速最邻近搜索的二叉树，主要通过划分区域以实现加速，以2D的KD树为例，只需考虑x和y两个划分，假定点集如下：

{ ( 7 , 2 ) , ( 5 , 4 ) , ( 9 , 6 ) , ( 4 , 7 ) , ( 8 , 1 ) , ( 2 , 3 ) } \{(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)\} {(7,2),(5,4),(9,6),(4,7),(8,1),(2,3)}

往kd树中插入一个点$(7,2)$,在x方向对平面进行划分如下（横轴为x，纵轴为y）：

接着依次插入第二个和第三个点，第二个点是$(5,4)$, 5小于7所有该点在左边，$(9,6)$的$x$大于7所以在右边，基于y对空间进行第二次划分（图中蓝线）：

2D KD树的划分顺序为: x->y->x->y…, 所以到第四个点$(4,7)$, 其$x$小于$(7,2)$所以在根节点的左边， $y$大于$(5,4)$的$y$所以在它的右边，依次类推得到对二维空间的划分图：

构造的树如下：

因为按区域进行了划分，kd树显著的降低了搜索的步数，以上图为例，假定我们聚类的距离阈值为4，要搜索出点$(2,3)$的距离阈值内的点，先从根节点$(7,2)$开始,算得距离大于4，类似于插入点的流程，查找左边的点$(5,4)$， 算得距离小于4，得到一个近距离点，接着搜索，虽然$(2,3)$在$(5,4)$左侧，但是为其本身，且无叶子节点，所以左侧搜索结束，看$(5,4)$右侧节点，计算得$(4,7)$到目标点的距离大于4，搜索结束。

以下是一个用c++实现的2维KD树的例子：

// Structure to represent node of kd tree
struct Node{
	std::vector<float> point;
	int id;
	Node* left;
	Node* right;

	Node(std::vector<float> arr, int setId)
	: point(arr), id(setId), left(NULL), right(NULL){}
};

struct KdTree
{
	Node* root;

	KdTree(): root(NULL){}

	void insert(std::vector<float> point, int id){
		recursive_insert(&root, 0, point, id);
	}

	void recursive_insert(Node **root, int depth, std::vector<float>  point, int id){
	    if (*root!= NULL){
	        int i = depth%2;
	        if(point[i] < (*root)->point[i]){
	            // left
	            recursive_insert(&(*root)->left, depth+1, point, id);
	        } else{
	            //right
	            recursive_insert(&(*root)->right, depth+1, point, id);
	        }
	    }else{
            *root = new Node(point, id);
	    }
	}

	void recursive_search(Node * node, int depth, std::vector<int> &ids, 
                       std::vector<float> target, float distanceTol){
	    if(node != NULL){
	        // compare current node to target
	        if ((node->point[0] >= (target[0]-distanceTol)) && (node->point[0] <= (target[0]+distanceTol)) &&
                    (node->point[1] >= (target[1]-distanceTol)) && (node->point[1] <= (target[1]+distanceTol))){
	            
	            float dis = sqrt((node->point[0]-target[0]) * (node->point[0]-target[0]) +
	                    (node->point[1]-target[1]) * (node->point[1]-target[1]));
	            
	            if (dis <= distanceTol){
	                ids.push_back(node->id);
	            }
	        }
	        if((target[depth%2] - distanceTol)<node->point[depth%2]){
	            // go to left
                recursive_search(node->left, depth + 1, ids, target, distanceTol);
            }
            if((target[depth%2] + distanceTol)>node->point[depth%2]){
                // go to right
                recursive_search(node->right, depth+1, ids, target, distanceTol);
            }
	    }
	}

	// return a list of point ids in the tree that are within distance of target
	std::vector<int> search(std::vector<float> target, float distanceTol){
		std::vector<int> ids;
		recursive_search(root, 0, ids, target, distanceTol);
		return ids;
	}
};

欧几里得聚类算法

点云聚类中常用的欧几里得聚类算法就是基于KD树实现的，聚类的目的是搜索出点云中“聚在一起”的点云簇，从而得到感知目标的位置和轮廓，以2D点欧几里得聚类为例，其处理过程如下：

std::vector<std::vector<int>> euclideanCluster(const std::vector<std::vector<float>>& points, KdTree* tree, float distanceTol){
	
    std::vector<std::vector<int>> clusters;
    std::vector<bool> processed(points.size(), false);

    for (int i = 0; i < points.size(); ++i) {
        if (processed[i] == true){
            continue;
        } else{
            std::vector<int> cluster;
            Proximity(cluster, points, processed, distanceTol, tree, i);
            clusters.push_back(cluster);
        }
    }
	return clusters;
}

// 递归聚类
void Proximity(std::vector<int> & cluster, std::vector<std::vector<float>> point,
               std::vector<bool> &processed, float distanceTol, KdTree* tree, int ind){
    processed[ind] = true;
    cluster.push_back(ind);
    std::vector<int> nearby_points = tree->search(point[ind], distanceTol);
    for (int i = 0; i < nearby_points.size(); ++i) {
        if(processed[nearby_points[i]]){
            continue;
        } else {
            Proximity(cluster, point, processed, distanceTol, tree, nearby_points[i]);
        }
    }
}

PCL中的欧几里得聚类

以上是关于KD树和欧几里得聚类的基本介绍，实际业务中我们并不需要自行实现欧几里得聚类，而且采用PCL中实现的欧几里得聚类，其基本用法如下：

template<typename PointT>
std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

    std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;

    // perform euclidean clustering to group detected obstacles
    typename pcl::search::KdTree<PointT>::Ptr kd_tree(new pcl::search::KdTree<PointT>());
    kd_tree->setInputCloud(cloud);
    typename pcl::EuclideanClusterExtraction<PointT> eu_cluster;
    eu_cluster.setClusterTolerance(clusterTolerance);
    eu_cluster.setMinClusterSize(minSize);
    eu_cluster.setMaxClusterSize(maxSize);
    eu_cluster.setSearchMethod(kd_tree);
    eu_cluster.setInputCloud(cloud);

    std::vector<pcl::PointIndices> cluster_list;
    eu_cluster.extract(cluster_list);

    for (auto cluster_indices : cluster_list){
        typename pcl::PointCloud<PointT>::Ptr cluster_points (new pcl::PointCloud<PointT>());
        for(auto ind : cluster_indices.indices){
            cluster_points->points.push_back(cloud->points[ind]);
        }
        cluster_points->width = cluster_points->points.size();
        cluster_points->height = 1;
        cluster_points->is_dense = true;
        clusters.push_back(cluster_points);
    }

    return clusters;
}

更详细的ros实现可以参考我的这篇博客：https://blog.csdn.net/AdamShan/article/details/83015570?spm=1001.2014.3001.5501