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

文章目录

一、前言
二、代码目录
三、代码解读
- 3.1、文件描述
- 3.2、代码框架
四、关联矩阵计算
- 4.1、ComputeLocationDistance
- 4.2、ComputeDirectionDistance
- 4.3、ComputeBboxSizeDistance
- 4.4、ComputePointNumDistance
- 4.5、ComputePointNumDistance
- 4.6、result_distance
五、结果

一、前言

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