06 目标跟踪 (OC-SORT) + 移动轨迹

检测之后接 OC-SORT 做多目标跟踪，给每个目标分配稳定 ID，并绘制移动轨迹。

6.1 OC-SORT 简介

OC-SORT（Observation-Centric SORT）是纯运动模型的多目标跟踪器，输入每帧检测 [x1,y1,x2,y2,score]，输出带 track ID 的轨迹。适合拥挤和非线性运动场景，实时性好。

6.2 依赖

OC-SORT 的 kalmanfilter.py 用了 filterpy，装到系统 python：

1
/usr/bin/python3 -m pip install filterpy

association.py 的 linear_assignment 有 lap/scipy 回退，系统 python 有 scipy 就行，不用装 lap。

6.3 在节点里引入 OC-SORT

OC-SORT 仓库不需要 pip install，加路径即可。在 carla_detector.py 顶部：

1
2
3
4
import sys
OC_SORT_PATH = "~/models/OC_SORT"   # 替换为你的路径
sys.path.insert(0, OC_SORT_PATH)
from trackers.ocsort_tracker.ocsort import OCSort

6.4 OC-SORT API

1
2
3
4
5
6
tracker = OCSort(det_thresh=0.5)   # 其他参数默认: max_age=30, min_hits=3, iou_threshold=0.3

# 每帧调用(即使没检测也要调用,传空数组)
dets = np.array([[x1,y1,x2,y2,score], ...])   # 原图坐标
tracks = tracker.update(dets, (img_h, img_w), (img_h, img_w))
# 返回 (N,5): [x1,y1,x2,y2,track_id]，track_id 从 1 开始

img_info 和 img_size 都传原图尺寸，scale=1，不做坐标缩放。
返回的 track_id = 内部 id + 1。
OC-SORT 不跟踪类别，需把 track 框匹配回检测框（IoU）拿类别。

6.5 集成到检测节点

在检测回调里，检测后喂给 tracker，再把 track 框匹配回检测拿类别：

1
2
3
4
5
6
7
8
9
10
# 1. 检测得到 boxes, scores, cids (已过滤)
# 2. 喂给 tracker
dets_in = np.concatenate([boxes, scores.reshape(-1,1)], axis=1) if len(boxes)>0 else np.empty((0,5))
tracks = tracker.update(dets_in, (h, w), (h, w))   # [x1,y1,x2,y2,track_id]

# 3. 每个 track 匹配回检测拿类别
for x1,y1,x2,y2,tid in tracks:
    ious = iou_one_to_many([x1,y1,x2,y2], boxes)
    best = ious.argmax()
    cls = COCO_CLASSES.get(int(cids[best]), "obj")   # 若 IoU>阈值

6.6 移动轨迹

维护一个 track_id -> [中心点列表] 的字典，每帧追加当前中心点，画折线：

1
2
3
4
5
6
7
8
9
10
trails = defaultdict(list)
TRAIL_LEN = 40   # 保留点数

cx, cy = (x1+x2)//2, (y1+y2)//2
trails[tid].append((cx, cy))
if len(trails[tid]) > TRAIL_LEN:
    trails[tid].pop(0)
# 画折线
for i in range(1, len(trails[tid])):
    cv2.line(ann, trails[tid][i-1], trails[tid][i], color, 2)

颜色按 track_id 生成稳定值，不同目标不同色。

6.7 发布带 track_id 的检测结果

Detection2D 消息有个 id 字段（string），用来放 track_id：

1
2
3
4
5
6
7
8
9
10
d = Detection2D()
d.id = str(tid)                          # track_id
d.bbox.center.position.x = (x1+x2)/2     # vision_msgs/Pose2D 结构: center.position.x
d.bbox.center.position.y = (y1+y2)/2
d.bbox.size_x = x2 - x1
d.bbox.size_y = y2 - y1
hyp = ObjectHypothesisWithPose()
hyp.hypothesis.class_id = str(int(cids[best]))   # COCO ID
hyp.hypothesis.score = float(scores[best])
d.results.append(hyp)

⚠️ humble 的 vision_msgs/BoundingBox2D.center 是 vision_msgs/Pose2D，结构是 center.position.x/y + center.theta（不是 center.x）。直接用 center.x 会报 'Pose2D' object has no attribute 'x'。

6.8 最终效果

rviz2 里 /carla/hero/rgb/detection_image 显示：

  • 每个目标一个稳定 ID 的框（颜色按 ID 区分）。
  • 标签 类别:ID（如 car:1、person:3）。
  • 移动轨迹（彩色折线，保留最近 40 个点）。

完整节点代码见 scripts/carla_detector.py。