代码：https://github.com/Xiao-Hu-Z/RaderDetectionAndTracking
代码正在写，实时更新！
流程
4D雷达毫米波聚类跟踪流程如下图：
预处理主要包括标定、坐标转换和动静分离。
-
标定使用水平仪、角反，采集数据分析，得到水平和俯仰偏角。
-
坐标转换使用转换矩阵即可。
-
动静分离主要根据径向速度在车辆前进方向的投影进行判断。
聚类
选用经典的dbcan算法，算法描述如下：
DBSCAN聚类算法是一种空间聚类算法，其根据被聚类数据的密度特点进行分类，该算法的聚类结果就是密度相连样本点的最大集合，其聚类结果的优劣主要与邻域半径σ和邻域半径内最小样本点数minpts的选择有关。
基本概念：
- σ邻域：σ代表以某样本点为圆心的圆半径，该样本点的邻域样本点集D定义为在候选集N中以某样本点为圆心，半径为σ的圆内其他样本点，邻域样本点集D为
N σ ( z i ) = { z j ∈ D ∣ d i s t ( z i , z j ) ⩽ σ } N_{\sigma}(z_i) = \{z_j \in D | dist(z_i,z_j) \leqslant \sigma\} Nσ(zi)={zj∈D∣dist(zi,zj)⩽σ}
-
核心样本点：有任意样本点 z i ∈ D z_i \in D zi∈D，如果至少有minpts个样本点在它的邻域范围内，则称zi为核心样本点，表示为
N σ ( z i ) ≥ m i n p t s N_{\sigma}(z_i)\geq minpts Nσ(zi)≥minpts -
直接密度可达：如果样本点 z i z_i zi为聚类中心，则在其邻域样本点集D中的所有样本点zj直接密度可达 z i z_i zi
-
密度可达：如果存在对象链 p 1 , p 2 , … , p T p_1, p_2, …, p_T p1,p2,…,pT，满足 p 1 = z j p_1=z_j p1=zj和 p T = z i p_T=z_i pT=zi，且 p k + 1 p_{k+1} pk+1直接密度可达 p k p_k pk，则称zj密度可达 z i z_i zi
-
密度相连：如果存在样本点 z k ∈ D z_k∈D zk∈D，使样本点 z j z_j zj密度可达 z k z_k zk，样本点zi密度可达zk，则zj到zi密度相连
由DBSCAN聚类算法的参数可知，密度相连样本点的最大集合就是算法的聚类结果，聚类结果中的每个类别都可以有一个或多个核心样本点。如果某类别中只有一个核心样本点，则该类别中的其他核心样本点都在该核心样本点的σ邻域内；如果某类别中有多个核心样本点，则该类别中的任意核心样本点的σ邻域内至少有一个其他核心样本点；这些核心样本点的σ邻域内所有样本点的集合组成了一个DBSCAN聚类类别。
处理流程如下。
-
从数据集中任意选取一个数据对象点 p；
-
如果对于参数 Eps 和 MinPts，所选取的数据对象点 p 为核心点，则找出所有从 p 密度可达的数据对象点，形成一个簇；
-
如果选取的数据对象点 p 是边缘点，选取另一个数据对象点；
-
重复（2）、（3）步，直到所有点被处理。
DBSCAN 算法的计算时间复杂的度为 O ( n ² ) O(n^²) O(n²)，n 为数据对象的数目。这种算法对于输入参数 Eps 和 MinPts 是敏感的。
针对DBSCAN 聚类时间复杂度过大，有人提出基于KD树改进的DBSCAN聚类算法，时间复杂度为 O ( n l o g n ) O(nlogn) O(nlogn)
聚类后，可以得到基本信息，比如目标长度、宽度、高度、体积、投影面积、RCS以及各种统计分布。根据这些信息，可以对目标进行分类，得到目标的分类结果。
另外，可以估计目标的速度和朝向。
目标朝向角为α，目标点云在速度方向的投影即是点云的径向速度Vr，则有：
V(i)*cos(θ(i)-α) = Vr(i)
V(i) = Vr(i) / cos(θ(i)-α)
由于车辆是刚体，车身每个点速度相等，则有V(1) = V(2) = … = V(n)
V(i) = V(j) = V
Vr(i) / cos(θ(i)-α) = Vr(j) / cos(θ(j)-α)
可以计算得到点云的朝向角α，进而得到目标的真实速度和横纵向速度。
由于雷达对速度的测量可能存在模糊或出现错误，因此任意两个点的结果有较高错误概率，但多个点拟合得到的结果有较高的可行度，可以准确估计目标朝向和横纵向速度。
上文已经推导Vr(i) = V* cos(θ(i)-α) = V*(cos(θ(i))cos(α) + sin(θ(i))sin(α))
两边同时除以VVr(i) cos(α)，则有
cos(θ(i))/Vr(i) = 1 / V cos(α) - tan(α) * sin(θ(i))* /Vr(i)
令y(i) = cos(θ(i))/Vr(i), x(i) = sin(θ(i))* /Vr(i)，b = 1 / V cos(α), k = - tan(α)则有
y(i) = k * x(i) + b
根据最小二乘拟合公式，可以得到V和α的估计值。
对于行人，由于不是刚体运动，并不适用上述方法。可以参考：Direction of Movement Estimation of Cyclists with a High-Resolution Automotive Radar
构建obb框
对于4D雷达，更重要的是聚类之后的处理。
4d毫米波雷达点云相比传统的毫米波雷达点数数量多很多，经过聚类后，可以得到多个点云簇信息。此时可以仿照激光传统聚类检测方式处理，通过对点云簇计算box框相关的信息，包括：框大小，中心位置，速度，方向和分类信息等。
- 不考虑z轴，先对点云簇x,y平面 计算凸多边形顶点，进而利用旋转卡壳法拟合最小外接斜矩形，计算box的方向。也有使用主成分分析PCA算法计算聚类后点云的OBB框，pcl库有相应的实现，但拟合出的box框不太紧凑。
目标分类
目标分类可以根据RCS、box框的大小和径向速度对对目标准确分类。
追踪
传统的毫米波点云比较少，聚类后通过点跟踪，4D雷达点云数量较多，不再适用点目标跟踪，可以仿照激光点云目标跟踪方式去做。
