待整理和写代码，准备先用dbcan聚类，用激光那一套做做看看效果
流程
4D雷达毫米波聚类跟踪流程如下图：
预处理主要包括标定、坐标转换和动静分离。
-
标定使用水平仪、角反，采集数据分析，得到水平和俯仰偏角。
-
坐标转换使用转换矩阵即可。
-
动静分离主要根据径向速度在车辆前进方向的投影进行判断。
聚类
选用经典的dbcan算法
聚类后，可以得到基本信息，比如目标长度、宽度、高度、体积、投影面积、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雷达点云数量较多，不再适用点目标跟踪，可以仿照激光点云目标跟踪方式去做。
