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

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


四、关联矩阵计算

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

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

整体跟踪效果不错。赞!

相关推荐
xingshanchang3 小时前
PyTorch 不支持旧GPU的异常状态与解决方案:CUDNN_STATUS_NOT_SUPPORTED_ARCH_MISMATCH
人工智能·pytorch·python
reddingtons4 小时前
Adobe Firefly AI驱动设计:实用技巧与创新思维路径
大数据·人工智能·adobe·illustrator·photoshop·premiere·indesign
CertiK4 小时前
IBW 2025: CertiK首席商务官出席,探讨AI与Web3融合带来的安全挑战
人工智能·安全·web3
Deepoch5 小时前
Deepoc 大模型在无人机行业应用效果的方法
人工智能·科技·ai·语言模型·无人机
试着5 小时前
【数据标注师】3D标注
3d·数据标注师·3d标注
Deepoch5 小时前
Deepoc 大模型:无人机行业的智能变革引擎
人工智能·科技·算法·ai·动态规划·无人机
kngines6 小时前
【字节跳动】数据挖掘面试题0003:有一个文件,每一行是一个数字,如何用 MapReduce 进行排序和求每个用户每个页面停留时间
人工智能·数据挖掘·mapreduce·面试题
Binary_ey6 小时前
AR衍射光波导设计遇瓶颈,OAS 光学软件来破局
人工智能·软件需求·光学软件·光波导
昵称是6硬币6 小时前
YOLOv11: AN OVERVIEW OF THE KEY ARCHITECTURAL ENHANCEMENTS目标检测论文精读(逐段解析)
图像处理·人工智能·深度学习·yolo·目标检测·计算机视觉
平和男人杨争争6 小时前
机器学习2——贝叶斯理论下
人工智能·机器学习