【目标跟踪】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()

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


四、关联矩阵计算

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

代码语言:javascript
复制
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 录制下来,然后再可视化。当然也可以在过程中保存图片。

整体跟踪效果不错。赞!