【目标跟踪】3D点云跟踪

一、前言

1. 之前博客一直介绍的是视觉方向的跟踪。不过在如今智能驾驶领域，雷达感知仍然占据主要部分。今天来分享下点云3D跟踪。
2. 视觉跟踪输入就是目标检测的结果。雷达跟踪输入可以是点云检测的结果，也可以是点云聚类的结果。除了一些数据结构、匹配计算，雷达跟踪算法与前面介绍视觉跟踪方法大体相同。
3. 本篇主要探讨雷达如何进行匹配、关联计算，同时解析下代码结构。参考的是 apollo 代码，整体效果还不错。

---

二、代码目录

雷达跟踪所有的代码文件

三、代码解读

3.1、文件描述

文件跳转较多，新手读起代码可能有点吃力。最好记录下每个文件是干什么的，有个大致印象即可。

.h文件	描述
object_track.h	class ObjectTrackSet class ObjectTrack
object.h	(1) struct Object; (2) struct SensorObjects
tracked_object.h	struct TrackedObject 数据类型 框、点云
hm_tracker.h	struct TrckerParm 全是参数
kalman.h	卡尔曼滤波 预测、状态、运动方程
hungarian_matcher.h	目标匹配
track_object_distance.h	计算匹配矩阵权重
geometry_util.h	计算所有点质心、计算3D框、数据转化等
feature_descriptor.h	计算目标的形状特征

一些补充：

barycenter 点云几何中心点（质心）

目标框 direction 朝向角 默认状态(1, 0, 0)

目标框 size 长宽高 (length, width, height)

目标状态 (x, y, z, vx, vy, vz)

状态方程 匀速 x = x + v * t

3.2、代码框架

阅读代码整体顺序如下

（1）跟踪

hm_tracker.cpp

bool HmObjectTracker::Track()

A 初始化 B 数据转化 输入 C 预测 D 匹配 E 更新 F 结果

hm_tracker.cpp 文件 Track 函数基本就是整个运算主函数了

（2）匹配

hungarian_matcher.cpp

void HungarianMatcher::Match()

A 计算关联矩阵 B 计算连接的组件 C 匹配每个子图

这里面有很多种匹配方式，这里主要运用的是A 计算关联矩阵

（3）关联矩阵计算

track_object_distance.cpp

float TrackObjectDistance::ComputeDistance()

这个函数就是我们这次的主角了。计算目标与目标的距离，然后进行匈牙利匹配。

---

四、关联矩阵计算

这部分是核心，我们来好好研究下。截取部分代码

double TrackObjectDistance::s_location_distance_weight_ = 0.6;
double TrackObjectDistance::s_direction_distance_weight_ = 0.2;
double TrackObjectDistance::s_bbox_size_distance_weight_ = 0.1;
double TrackObjectDistance::s_point_num_distance_weight_ = 0.1;
double TrackObjectDistance::s_histogram_distance_weight_ = 0.5;

// new_object测量 track_predict预测 
float TrackObjectDistance::ComputeDistance(
    ObjectTrackPtr track, const Eigen::VectorXf& track_predict,
    const std::shared_ptr<TrackedObject>& new_object) {
  // Compute distance for given track & object
  float location_distance = ComputeLocationDistance(track, track_predict, new_object);  // 分速度慢 速度快
  float direction_distance = ComputeDirectionDistance(track, track_predict, new_object);
  float bbox_size_distance = ComputeBboxSizeDistance(track, new_object);
  float point_num_distance = ComputePointNumDistance(track, new_object);
  float histogram_distance = ComputeHistogramDistance(track, new_object);

  float result_distance = s_location_distance_weight_ * location_distance +
                          s_direction_distance_weight_ * direction_distance +
                          s_bbox_size_distance_weight_ * bbox_size_distance +
                          s_point_num_distance_weight_ * point_num_distance +
                          s_histogram_distance_weight_ * histogram_distance;
  return result_distance;
}

4.1、ComputeLocationDistance

计算中心点距离差 取值范围[0, $+\infty$）

当current_object 中 V <= 2m/s 欧式距离

当current_object 中 V > 2m/s 根据速度方向分解 以速度方向与垂直速度方向建立坐标系。投影速度方向的偏差为1/2倍距离偏差， 投影垂直速度方向的偏差为2倍距离偏差。平方开方求最终偏差。

可以简单理解为：当目标高速行驶时，在速度方向上的位移偏差会稍大，为了补偿这部分偏差，采取降低方向上的位移权重计算最终位移偏差（我是这么理解的）。

4.2、ComputeDirectionDistance

计算方向上的距离 取值范围[0,2]

计算位移在速度方向上的余弦值cos_theta 最终return 1- cos_theta

这个也比较好理解。当物体与检测物体位移差方向与预测速度方向相近时，此时更相信是同一个目标。cos值为1时，说明位移偏差与预测速度同方向，则认为这两物体更容易匹配。

当位移偏差为0时，这里有设定默认cos值为0.994。

4.3、ComputeBboxSizeDistance

取值范围[0,1]

这个稍微有点复杂，待我娓娓道来

old_dir 当前目标的方向，默认偏航为0时 默认值为(1, 0, 0)

new_idr 检测目标的方向

old_size 当前目标的尺寸 (bbox.length, bbox.width, bbox.height)

new_size 检测目标的尺寸

nter&pos_id=img-506WdR2l-1706774251770)

计算dot_00, dot_01。dot_00 可以理解为两目标方向夹角，dot_01理解为目标与另一目标垂直方向夹角。这里不考虑超过90度的夹角，因为目标方向可以是alpha 或 180 - alpha。

为什么两种情况？可以理解为把目标长与宽对齐（我们事先并不知道目标对应的长宽），先根据目标角度的状态判定。当角度小于45°时，目标长与另一目标的长对齐。否则目标长与另一个目标宽对齐，最终计算长或者宽差值的比例，取最小值当做最终值。

4.4、ComputePointNumDistance

取值范围[0,1]

这个公式比较简单

这个很容易理解，点云个数越相近，越容易匹配上。

4.5、ComputePointNumDistance

取值范围[0,3]

直方图距离 把目标所有点云以当个坐标轴分为10个区间 再以xyz三轴共分为30个区间。

如果点云都是均匀排布那么目标形状特征 shape_features = [0.1] * 30

shape_features具体计算过程。以x轴为例，y轴、z轴同理。

计算目标点云 x轴最值，把区间划分为10等分。记录所有点在10个区间点云个数。

如果完全均匀排布则结果为

shape_feature_x = [0.1]*10

shape_features = shape_feature_x + shape_feature_y + shape_feature_z

知道了目标形状特征的定义，可得

4.6、result_distance

最终距离为上述计算的5个距离量乘以对应系数和

五、结果

由于 rviz 无法显示点云跟踪结果，那我们把雷达跟踪结果 topic 录制下来，然后再可视化。当然也可以在过程中保存图片。

整体跟踪效果不错。赞！