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

一、前言

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

二、代码目录

雷达跟踪所有的代码文件

三、代码解读

3.1、文件描述

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

一些补充：

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,

）

当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 录制下来，然后再可视化。当然也可以在过程中保存图片。

整体跟踪效果不错。赞！