目标跟踪¶
目标跟踪（Object Tracking）是自主机器人所需的核心感知能力之一。**目标检测器**能告诉你画面中*有什么*以及*在哪里*，而**跟踪器**则能告诉你*哪个目标是哪个目标*——即使在遮挡、外观变化和漏检的情况下，也能保持一致的身份标识。
对于在拥挤走廊中导航的移动机器人、跟随目标的无人机，或跟踪传送带上物体的机械臂，可靠的多目标跟踪（MOT）对于安全高效的运行至关重要。
第 t 帧 第 t+1 帧 第 t+2 帧
┌──────────┐ ┌──────────┐ ┌──────────┐
│ ┌───┐ │ │ ┌───┐ │ │ ┌───┐ │
│ │ A │ │ │ │ A │ │ │ │ A │ │
│ └───┘ │ │ └───┘ │ │ └───┘ │
│ │ │ │ │ │
│ ┌───┐ │ │ ┌───┐ │ │ ┌───┐ │
│ │ B │ │ │ │ B │ │ │ │ B │ │
│ └───┘ │ │ └───┘ │ │ └───┘ │
└──────────┘ └──────────┘ └──────────┘
仅检测: "两个框" "两个框" "两个框"
有跟踪: A=1, B=2 A=1, B=2 A=1, B=2
学习目标¶
学完本章后，你将能够：
- 解释目标检测和目标跟踪的区别，以及各自的适用场景。
- 使用 OpenCV 实现稀疏和稠密光流。
- 构建卡尔曼滤波器用于单目标状态估计和跟踪。
- 描述 SORT 和 DeepSORT 跟踪流水线。
- 了解基于 Transformer 的跟踪架构。
- 使用标准 MOT 指标（MOTA、MOTP、HOTA、IDF1）评估跟踪器。
- 在 ROS 2 机器人系统中集成跟踪功能。
1. 跟踪与检测¶
| 方面 | 检测 | 跟踪 |
|---|---|---|
| 输出 | 每帧的边界框 + 类别标签 | 跨帧的一致 ID |
| 时间信息 | 无（逐帧处理） | 维护目标随时间的身份 |
| 处理遮挡 | 目标消失后重新出现视为"新目标" | 短暂遮挡后保持 ID |
| 速度 | 较慢（完整前向传播） | 较快（无需检测即可预测） |
| 失败模式 | 误检 / 漏检 | ID 切换、轨迹碎片化 |
何时仅使用检测¶
- 静态场景（例如，清点货架上的物体）。
- 只需要当前状态，不需要历史信息。
何时使用跟踪¶
- 需要时间一致性的任务：跟随一个人、计数进出某区域的物体、轨迹预测。
- 需要平滑噪声检测结果时（卡尔曼滤波预测填补空缺）。
- 下游规划依赖目标身份时（例如，"专门避开那辆红色汽车"）。
常见范式：先检测后跟踪——每帧运行检测器，使用跟踪器将检测结果跨帧关联。
2. 光流¶
光流估计连续两帧之间像素（或特征）的表观运动。它是许多跟踪器内部使用的底层基础组件。
2.1 Lucas-Kanade（稀疏光流）¶
Lucas-Kanade 方法跟踪一组稀疏的特征点。给定某个点周围的小窗口，假设亮度恒定，求解使以下公式最小化的位移 \((u, v)\)：
\[
\sum_{(x,y) \in W} \left[ I(x+u, y+v, t+1) - I(x, y, t) \right]^2
\]
使用一阶泰勒展开，这变成如下线性系统：
\[
\begin{bmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{bmatrix}
\begin{bmatrix} u \\ v \end{bmatrix}
= -
\begin{bmatrix} \sum I_x I_t \\ \sum I_y I_t \end{bmatrix}
\]
代码：基于 Lucas-Kanade 的稀疏光流
import cv2
import numpy as np
def lucas_kanade_demo(video_path=0):
"""使用 Lucas-Kanade 光流跟踪稀疏特征点。"""
cap = cv2.VideoCapture(video_path)
if not cap.isOpened():
print(f"无法打开视频: {video_path}")
return
# Shi-Tomasi 角点检测参数
feature_params = dict(
maxCorners=200,
qualityLevel=0.01,
minDistance=10,
blockSize=7
)
# Lucas-Kanade 参数
lk_params = dict(
winSize=(15, 15),
maxLevel=2,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
)
# 用于可视化的随机颜色
np.random.seed(42)
colors = np.random.randint(0, 255, (500, 3), dtype=np.uint8)
ret, old_frame = cap.read()
if not ret:
return
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)
# 创建用于绘制轨迹的遮罩图像
mask = np.zeros_like(old_frame)
while True:
ret, frame = cap.read()
if not ret:
break
frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# 计算光流
p1, st, err = cv2.calcOpticalFlowPyrLK(
old_gray, frame_gray, p0, None, **lk_params
)
if p1 is not None:
# 选择好的点
good_new = p1[st.flatten() == 1]
good_old = p0[st.flatten() == 1]
# 绘制轨迹
for i, (new, old) in enumerate(zip(good_new, good_old)):
a, b = new.ravel().astype(int)
c, d = old.ravel().astype(int)
mask = cv2.line(mask, (a, b), (c, d), colors[i % 500].tolist(), 2)
frame = cv2.circle(frame, (a, b), 5, colors[i % 500].tolist(), -1)
img = cv2.add(frame, mask)
cv2.imshow('Lucas-Kanade 光流', img)
if cv2.waitKey(30) & 0xFF == 27: # ESC 退出
break
# 更新上一帧和特征点
old_gray = frame_gray.copy()
# 定期重新检测特征点
if len(good_new) < 50:
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)
else:
p0 = good_new.reshape(-1, 1, 2)
cap.release()
cv2.destroyAllWindows()
if __name__ == '__main__':
lucas_kanade_demo(0) # 使用 0 表示摄像头，或提供视频文件路径
关键参数：
| 参数 | 说明 |
|---|---|
maxCorners |
最大跟踪特征数 |
qualityLevel |
角点最低质量（0–1） |
minDistance |
特征点之间的最小欧氏距离 |
winSize |
LK 搜索窗口大小 |
maxLevel |
金字塔层数（处理更大运动） |
2.2 Farneback 稠密光流¶
与稀疏方法不同，稠密光流为*每个*像素计算速度向量。Farneback 算法用多项式展开对邻域信号建模，并估计位移。
import cv2
import numpy as np
def farneback_dense_flow_demo(video_path=0):
"""使用 Farneback 方法计算并可视化稠密光流。"""
cap = cv2.VideoCapture(video_path)
if not cap.isOpened():
print(f"无法打开视频: {video_path}")
return
ret, old_frame = cap.read()
if not ret:
return
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
hsv_mask = np.zeros_like(old_frame)
hsv_mask[:, :, 1] = 255 # 设置饱和度为最大值
while True:
ret, frame = cap.read()
if not ret:
break
frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# 计算稠密光流
flow = cv2.calcOpticalFlowFarneback(
prev=old_gray,
next=frame_gray,
flow=None,
pyr_scale=0.5, # 金字塔缩放比例
levels=3, # 金字塔层数
winsize=15, # 窗口大小
iterations=3, # 每层金字塔的迭代次数
poly_n=5, # 多项式邻域大小
poly_sigma=1.2, # 多项式平滑的高斯标准差
flags=0
)
# 将光流转为极坐标（幅度、角度）
magnitude, angle = cv2.cartToPolar(flow[..., 0], flow[..., 1])
# 用角度表示色调，用幅度表示亮度
hsv_mask[:, :, 0] = angle * 180 / np.pi / 2
hsv_mask[:, :, 2] = cv2.normalize(
magnitude, None, 0, 255, cv2.NORM_MINMAX
)
# 将 HSV 转为 BGR 用于显示
rgb_flow = cv2.cvtColor(hsv_mask, cv2.COLOR_HSV2BGR)
# 叠加到原始帧上
overlay = cv2.addWeighted(frame, 0.6, rgb_flow, 0.4, 0)
cv2.imshow('稠密光流 (Farneback)', overlay)
cv2.imshow('光流场', rgb_flow)
if cv2.waitKey(30) & 0xFF == 27:
break
old_gray = frame_gray
cap.release()
cv2.destroyAllWindows()
if __name__ == '__main__':
farneback_dense_flow_demo(0)
HSV 色轮可视化解读：
3. 卡尔曼滤波跟踪¶
卡尔曼滤波器是目标跟踪的核心工具。它在线性系统中给定噪声观测时，提供最优（最小方差）状态估计。即使真实动力学是轻度非线性的（大多数跟踪场景都是如此），它也能很好地工作。
3.1 状态预测¶
我们将目标状态建模为向量。对于用恒定速度跟踪边界框中心：
\[
\mathbf{x} = \begin{bmatrix} x \\ y \\ \dot{x} \\ \dot{y} \end{bmatrix}
\]
状态转移（预测）步骤：
\[
\mathbf{x}_{k|k-1} = \mathbf{F} \, \mathbf{x}_{k-1|k-1} + \mathbf{B} \, \mathbf{u}_k
\]
\[
\mathbf{P}_{k|k-1} = \mathbf{F} \, \mathbf{P}_{k-1|k-1} \, \mathbf{F}^\top + \mathbf{Q}
\]
其中 \(\mathbf{F}\) 是状态转移矩阵：
\[
\mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
\]
\(\mathbf{Q}\) 是过程噪声协方差（建模运动模型中的不确定性）。
3.2 测量更新¶
当测量 \(\mathbf{z}_k\) 到来时（检测到的边界框中心）：
\[
\mathbf{y}_k = \mathbf{z}_k - \mathbf{H} \, \mathbf{x}_{k|k-1} \quad \text{（新息）}
\]
\[
\mathbf{S}_k = \mathbf{H} \, \mathbf{P}_{k|k-1} \, \mathbf{H}^\top + \mathbf{R} \quad \text{（新息协方差）}
\]
\[
\mathbf{K}_k = \mathbf{P}_{k|k-1} \, \mathbf{H}^\top \, \mathbf{S}_k^{-1} \quad \text{（卡尔曼增益）}
\]
\[
\mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + \mathbf{K}_k \, \mathbf{y}_k
\]
\[
\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \, \mathbf{H}) \, \mathbf{P}_{k|k-1}
\]
其中 \(\mathbf{H}\) 是观测矩阵（将状态映射到测量空间），\(\mathbf{R}\) 是测量噪声协方差。
3.3 实现——使用 cv2.KalmanFilter 跟踪球体¶
import cv2
import numpy as np
def kalman_ball_tracker():
"""使用卡尔曼滤波器和 HSV 颜色分割跟踪彩色球体。"""
cap = cv2.VideoCapture(0)
if not cap.isOpened():
print("无法打开摄像头")
return
# --- 卡尔曼滤波器设置 ---
# 状态: [x, y, vx, vy] 测量: [x, y]
kf = cv2.KalmanFilter(4, 2)
dt = 1.0 # 假设每帧 1 个时间单位
# 状态转移矩阵 F
kf.transitionMatrix = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
], dtype=np.float32)
# 测量矩阵 H
kf.measurementMatrix = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
], dtype=np.float32)
# 过程噪声协方差 Q
kf.processNoiseCov = np.eye(4, dtype=np.float32) * 1e-2
# 测量噪声协方差 R
kf.measurementNoiseCov = np.eye(2, dtype=np.float32) * 1e-1
# 初始误差协方差 P
kf.errorCovPost = np.eye(4, dtype=np.float32)
initialized = False
# 黄色网球的 HSV 范围（根据你的物体调整）
lower_color = np.array([29, 86, 6])
upper_color = np.array([64, 255, 255])
while True:
ret, frame = cap.read()
if not ret:
break
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower_color, upper_color)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# 查找轮廓
contours, _ = cv2.findContours(
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
)
measurement = None
if contours:
c = max(contours, key=cv2.contourArea)
((mx, my), radius) = cv2.minEnclosingCircle(c)
if radius > 10:
measurement = np.array([[np.float32(mx)], [np.float32(my)]])
# 绘制检测结果
cv2.circle(frame, (int(mx), int(my)), int(radius), (0, 255, 0), 2)
# --- 卡尔曼滤波 ---
if not initialized and measurement is not None:
# 用第一次测量初始化状态
kf.statePost = np.array([
[measurement[0, 0]],
[measurement[1, 0]],
[0], [0]
], dtype=np.float32)
initialized = True
if initialized:
# 预测
prediction = kf.predict()
px, py = int(prediction[0, 0]), int(prediction[1, 0])
# 更新（仅在有检测结果时）
if measurement is not None:
kf.correct(measurement)
# 绘制预测位置
cv2.circle(frame, (px, py), 10, (0, 0, 255), 2)
cv2.putText(frame, f"预测 ({px},{py})", (px + 15, py),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
# 绘制校正位置
cx = int(kf.statePost[0, 0])
cy = int(kf.statePost[1, 0])
cv2.circle(frame, (cx, cy), 6, (255, 0, 0), -1)
cv2.putText(frame, f"校正 ({cx},{cy})", (cx + 15, cy + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
cv2.imshow('卡尔曼球体跟踪器', frame)
if cv2.waitKey(30) & 0xFF == 27:
break
cap.release()
cv2.destroyAllWindows()
if __name__ == '__main__':
kalman_ball_tracker()
提示： 即使检测器漏检了物体（measurement 为 None），卡尔曼滤波器仍会继续预测。这种"填补空缺"的能力是它在跟踪中最大的优势之一。
4. SORT — 简单在线实时跟踪¶
SORT（Bewley 等，2016）是一个实用的跟踪器，结合了：
- 外部检测器（如 Faster R-CNN、YOLO）的检测结果。
- 卡尔曼滤波器——每个被跟踪目标一个——用于预测下一状态。
- 匈牙利算法——将预测轨迹与新检测结果关联。
┌──────────────────────────────────────────────────────────┐
│ SORT 流水线 │
│ │
│ 第 t 帧 ──► 检测器 ──► 检测结果 D_t │
│ │ │
│ 已有轨迹 ──► 卡尔曼预测 ──► 预测结果 P_t │
│ │ │
│ ┌─────────┴──────────┐ │
│ │ 关联 │ │
│ │ (匈牙利算法， │ │
│ │ 基于 IoU 矩阵) │ │
│ └─────────┬──────────┘ │
│ ┌───────────┼───────────┐ │
│ 已匹配 未匹配检测 D 未匹配轨迹 T │
│ (更新) (创建新轨迹) (增加年龄) │
│ │ │
│ 轨迹年龄超过 │
│ max_age 帧 ──► 删除 │
└──────────────────────────────────────────────────────────┘
关键设计选择¶
| 组件 | 选择 |
|---|---|
| 运动模型 | 卡尔曼滤波器（恒定速度，[x,y,s,r,ẋ,ẏ,ṡ]），其中 s=面积，r=宽高比 |
| 关联度量 | IoU（交并比），预测框与检测框之间 |
| 分配 | 匈牙利算法（scipy.optimize.linear_sum_assignment） |
| 新轨迹 | 当检测结果连续 n_init 帧未匹配时创建 |
| 轨迹删除 | 连续 max_age 帧无匹配检测后删除 |
优势： 简单、快速（~260 FPS）、可与任意检测器配合。
弱点： 遮挡时容易出现 ID 切换（仅使用运动信息，无外观信息）。
5. DeepSORT¶
DeepSORT（Wojke 等，2017）在 SORT 的基础上增加了**外观描述符**——一种深度 Re-ID（重识别）嵌入，有助于在目标重叠或被遮挡时区分它们。
5.1 深度外观特征¶
一个小型 CNN（通常在人员 Re-ID 数据集上训练）从每个检测裁剪区域提取一个 128 维嵌入向量。每个轨迹维护最近 \(N\) 个嵌入的缓冲区。
轨迹 \(i\) 与检测 \(j\) 之间的**外观代价**为：
\[
d_{\text{app}}^{(i,j)} = \min \{ 1 - e_j^\top e_k^{(i)} \mid e_k^{(i)} \in \text{gallery}_i \}
\]
5.2 级联匹配¶
DeepSORT 用一个**级联匹配**替代了单一的全局匈牙利分配，优先考虑*最近*被看到的轨迹：
对于 age = 1, 2, 3, ... :
┌──────────────────────────────────────┐
│ 代价矩阵 = λ · d_app + (1-λ)·d_IoU │
│ (外观 + 运动) │
│ 对该年龄的剩余轨迹和 │
│ 未匹配检测运行匈牙利算法 │
└──────────────────────────────────────┘
这防止了一个频繁出现的轨迹被一个新的检测"抢走"——即使该检测在空间上更接近一个长时间丢失的轨迹。
架构图：
┌───────────────────────────────────────────────────────┐
│ DeepSORT │
│ │
│ ┌──────────┐ ┌──────────┐ ┌────────────────┐ │
│ │ 检测器 │───►│ 检测 │───►│ Re-ID CNN │ │
│ │ (YOLO, │ │ 裁剪 │ │ (128 维嵌入) │ │
│ │ 等) │ └──────────┘ └───────┬────────┘ │
│ └──────────┘ │ │
│ ┌───────────────────────┘ │
│ ▼ │
│ ┌────────────────────────────────────────────┐ │
│ │ 级联匹配 │ │
│ │ ┌──────────────┐ ┌────────────────────┐ │ │
│ │ │ IoU 代价 │ │ 外观代价 │ │ │
│ │ │ 矩阵 │ │ 矩阵 │ │ │
│ │ └──────┬───────┘ └────────┬───────────┘ │ │
│ │ └────────┬──────────┘ │ │
│ │ 合并代价矩阵 │ │
│ │ │ │ │
│ │ ┌────────────┼────────────┐ │ │
│ │ 已匹配 未匹配检测 D 未匹配轨迹 T │ │
│ └────────────────────────────────────────────┘ │
│ │ │
│ 卡尔曼滤波更新 / 新建轨迹 / 删除 │
└───────────────────────────────────────────────────────┘
6. 基于 Transformer 的跟踪器¶
近年来的 MOT 方法利用 Transformer 在单一架构中联合执行检测和跟踪。
MOTRv2（2023）¶
MOTRv2 建立在 DETR 风格的检测器之上。关键思想：
- 跟踪查询（Track Queries）： 每个已有的轨迹有一个可学习的查询嵌入，跨帧传播。解码器预测该轨迹在当前帧中的边界框。
- 检测查询（Detection Queries）： 新目标由不与任何轨迹关联的"新生"查询检测。
- 匈牙利匹配器在一个步骤中联合处理检测和关联。
第 t 帧 ──► CNN 骨干网络 ──► 特征图
│
轨迹查询 t-1 ──► │
▼
Transformer 解码器
┌───────────────────┐
│ 轨迹查询 t │──► 已有轨迹
│ 新检测查询 │──► 新轨迹
└───────────────────┘
TrackFormer（2022）¶
- 同样基于 DETR；引入**轨迹嵌入（Track Embeddings）**，对当前帧特征进行注意力计算。
- 使用注意力机制跨帧传播身份，无需显式的运动模型。
- 不需要单独的卡尔曼滤波器或 Re-ID 网络——Transformer 隐式学习跟踪。
跟踪范式对比：
| 方法 | 运动模型 | 外观信息 | 速度 | 复杂度 |
|---|---|---|---|---|
| SORT | 卡尔曼滤波器 | 无 | ~260 FPS | 低 |
| DeepSORT | 卡尔曼滤波器 | Re-ID CNN | ~40 FPS | 中 |
| TrackFormer | 学习的注意力 | 学习的 | ~15 FPS | 高 |
| MOTRv2 | 学习的注意力 | 学习的 | ~20 FPS | 高 |
7. 评估指标¶
评估多目标跟踪器需要捕捉不同方面性能的指标：
| 指标 | 全称 | 衡量内容 | 范围 | 说明 |
|---|---|---|---|---|
| MOTA | 多目标跟踪精度 | 1 - (FN + FP + ID切换数) / GT | (-∞, 100%] | 最常用；对漏检、误检和 ID 切换同等惩罚 |
| MOTP | 多目标跟踪精确度 | 匹配检测的平均 IoU | [0, 1] | 衡量定位质量，不涉及关联 |
| HOTA | 高阶跟踪精度 | 检测质量和关联质量的几何平均 | [0, 100] | 平衡 DetA 和 AssA；推荐的现代指标 |
| IDF1 | ID F1 分数 | 正确识别的检测比例 | [0, 1] | 聚焦于身份保持 |
HOTA 分解为：
\[
\text{HOTA}_\alpha = \sqrt{\text{DetA}_\alpha \cdot \text{AssA}_\alpha}
\]
其中 \(\text{DetA}\) 是检测精度，\(\text{AssA}\) 是在给定 IoU 阈值 \(\alpha\) 下的关联精度。
常用基准数据集：
| 数据集 | 领域 | 序列数 | 主要挑战 |
|---|---|---|---|
| MOT17/20 | 行人 | 16 / 4 | 拥挤场景 |
| KITTI | 驾驶 | 21 | 3D 跟踪，尺度变化 |
| DanceTrack | 舞蹈 | 100+ | 快速运动，外观相似 |
| BDD100K | 驾驶 | 1,000 | 多样天气/条件 |
8. ROS 2 中的多目标跟踪¶
在 ROS 2 机器人系统中，跟踪数据以消息的形式在话题上传递。
标准消息类型¶
ROS 2 感知流水线:
摄像头/RGB ──► /image_raw (sensor_msgs/Image)
│
▼
检测节点 ──► /detections (vision_msgs/Detection2DArray)
│
▼
跟踪节点 ──► /tracked_objects (vision_msgs/Detection2DArray)
│ 带有唯一 ID 的检测结果
▼
规划/控制节点
关键消息类型：
| 消息 | 包 | 用途 |
|---|---|---|
vision_msgs/Detection2DArray |
vision_msgs | 带有边界框的 2D 检测数组 |
vision_msgs/Detection3DArray |
vision_msgs | 3D 检测（用于 LiDAR 或深度相机） |
vision_msgs/ObjectHypothesisWithPose |
vision_msgs | 每个假设的类别 + 置信度 + 位姿 |
geometry_msgs/PoseStamped |
geometry_msgs | 单个跟踪目标的位姿 |
custom_msg/TrackedObjectArray |
自定义 | 通常为更丰富的跟踪信息创建（ID、速度等） |
示例：跟踪器节点骨架¶
import rclpy
from rclpy.node import Node
from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose
from geometry_msgs.msg import PoseStamped
import numpy as np
from scipy.optimize import linear_sum_assignment
class SimpleTrackerNode(Node):
def __init__(self):
super().__init__('simple_tracker')
self.sub = self.create_subscription(
Detection2DArray, '/detections', self.detection_callback, 10
)
self.pub = self.create_publisher(
Detection2DArray, '/tracked_objects', 10
)
self.tracks = {} # id -> state (x, y, w, h)
self.next_id = 0
self.get_logger().info('SimpleTracker 已启动。')
def detection_callback(self, msg: Detection2DArray):
detections = []
for det in msg.detections:
cx = det.bbox.center.position.x
cy = det.bbox.center.position.y
w = det.bbox.size_x
h = det.bbox.size_y
detections.append([cx, cy, w, h])
if not detections:
return
det_array = np.array(detections)
track_ids = list(self.tracks.keys())
track_states = np.array([self.tracks[tid] for tid in track_ids]) if track_ids else np.empty((0, 4))
if len(track_states) > 0:
# 简单的欧氏距离代价
cost = np.linalg.norm(
track_states[:, None, :2] - det_array[None, :, :2], axis=2
)
row_idx, col_idx = linear_sum_assignment(cost)
matched_tracks = set()
matched_dets = set()
for r, c in zip(row_idx, col_idx):
if cost[r, c] < 100: # 阈值
self.tracks[track_ids[r]] = det_array[c]
matched_tracks.add(r)
matched_dets.add(c)
# 为未匹配的检测创建新轨迹
for i in range(len(det_array)):
if i not in matched_dets:
self.tracks[self.next_id] = det_array[i]
self.next_id += 1
else:
for det in det_array:
self.tracks[self.next_id] = det
self.next_id += 1
# 发布跟踪目标
out_msg = Detection2DArray()
out_msg.header = msg.header
for tid, state in self.tracks.items():
d = Detection2D()
d.bbox.center.position.x = float(state[0])
d.bbox.center.position.y = float(state[1])
d.bbox.size_x = float(state[2])
d.bbox.size_y = float(state[3])
hyp = ObjectHypothesisWithPose()
hyp.hypothesis.class_id = str(tid)
hyp.hypothesis.score = 1.0
d.results.append(hyp)
out_msg.detections.append(d)
self.pub.publish(out_msg)
def main():
rclpy.init()
node = SimpleTrackerNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
常用 ROS 2 跟踪包：
| 包 | 描述 |
|---|---|
ros2_deepsort |
DeepSORT 的 ROS 2 封装 |
yolov8_ros |
YOLOv8 内置跟踪（BoT-SORT、ByteTrack） |
open_track |
开源多目标跟踪器，带 ROS 2 接口 |
9. 练习¶
练习 1：光流可视化¶
在录制的视频上运行 Farneback 稠密光流演示。修改 HSV 可视化，仅高亮快速运动的物体（对幅度设置阈值）。当增大 levels 和 winsize 时会发生什么？
练习 2：卡尔曼滤波调参¶
使用球体跟踪的卡尔曼滤波代码：
1. 将 processNoiseCov 增大 10 倍。预测会怎样变化？
2. 将 measurementNoiseCov 增大 10 倍。滤波器如何响应？
3. 将宽度和高度添加到状态向量中，使滤波器也能预测边界框大小。
练习 3：从零实现 SORT¶
以第 3 节的卡尔曼滤波代码为起点：
1. 创建一个封装 cv2.KalmanFilter 的 Track 类。
2. 实现两个边界框之间的 IoU 计算。
3. 使用 scipy.optimize.linear_sum_assignment 进行关联。
4. 在 MOT17 验证集上测试。
练习 4：DeepSORT 外观特征¶
用运动 + 外观的组合代价替换练习 3 中的 IoU 代价：
1. 加载预训练的 Re-ID 模型（如 torchreid 库）。
2. 从检测裁剪区域提取嵌入。
3. 实现第 5.2 节中的级联匹配。
练习 5：ROS 2 跟踪器节点¶
扩展第 8 节的 ROS 2 跟踪器节点骨架： 1. 为每个轨迹使用卡尔曼滤波器，而非简单的位置匹配。 2. 添加轨迹生命周期管理（连续 N 次检测后创建，连续 M 帧漏检后删除）。 3. 在发布位置的同时发布速度估计。
参考资料¶
- Lucas, B. D., & Kanade, T. (1981). An iterative image registration technique with an application to stereo vision. IJCAI.
- Farnebäck, G. (2003). Two-frame motion estimation based on polynomial expansion. SCIA.
- Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. ASME Journal of Basic Engineering.
- Bewley, A., Ge, Z., Ott, L., Ramos, F., & Upcroft, B. (2016). Simple online and realtime tracking. ICIP.
- Wojke, N., Bewley, A., & Paulus, D. (2017). Simple online and realtime tracking with a deep association metric. ICIP.
- Zhang, Y., Sun, P., Jiang, Y., et al. (2022). ByteTrack: Multi-object tracking by associating every detection box. ECCV.
- Meinhardt, T., Kirillov, A., Leal-Taixe, L., & Feichtenhofer, C. (2022). TrackFormer: Multi-object tracking with transformers. CVPR.
- Zhang, Y., Wang, T., & Zhang, X. (2023). MOTRv2: Bootstrapping end-to-end multi-object tracking by pretrained object detectors. CVPR.
- Luiten, J., Osep, A., Dendorfer, P., et al. (2021). HOTA: A higher order metric for evaluating multi-object tracking. IJCV.
- OpenCV 文档. Optical Flow — https://docs.opencv.org/4.x/d7/d8b/tutorial_py_lucas_kanade.html
