【目标跟踪】多目标跟踪测距

前言

• 先放效果图。目标框内左上角，显示的是目标距离相机的纵向距离。目标横向距离、速度已求出，没在图片展示。
• 这里不仅仅实现对目标检测框的跟踪，且可以实现单相机进行对目标进行测距跟踪。
• 想了解详细原理可以参考往期博客：【目标跟踪】多目标跟踪sort （python 代码） 。这里不过多赘述，直接上代码，如有疑问，欢迎私信交流。

---

python代码（带注释）

• 代码输入：1、连续帧图片，2、每帧图片的检测结果。(需要数据的可以私信我)
• 代码参考:git地址
• 输出结果以视频形式保存

main.py

检测结果为 det.txt ，图片格式为 000001.jpg 。用的是跟踪挑战开源数据。
这部分代码主要是加载检测数据，读取图片。调用跟踪与测距接口进行计算
可以设置 dispaly 与 video_save 是否 show 图片 与保存视频
x_p 里面包含目标离相机纵向与横向距离，还有速度、加速度。可以自行更改 putText 图片展示信息

import os
import cv2
from sort import *

if __name__ == '__main__':
    display, video_save = False, True  # 是否show,结果是否存视频
    max_age, min_hits, iou_threshold = 3, 3, 0.3  # sort算法参数
    colours = 255 * np.random.rand(32, 3)  # 随机生产颜色
    video = cv2.VideoWriter("video.mp4", cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), 10,
                            (1920, 1080)) if video_save else None
    mot_tracker = Sort(max_age=max_age, min_hits=min_hits, iou_threshold=iou_threshold)  # 创建sort跟踪器
    seq_dets = np.loadtxt("det.txt", delimiter=',')  # 加载检测txt结果
    for frame in range(int(seq_dets[:, 0].max())):
        frame += 1  # 从1帧开始
        dets = seq_dets[seq_dets[:, 0] == frame, 2:7]
        dets[:, 2:4] += dets[:, 0:2]  # [x1,y1,w,h] to [x1,y1,x2,y2] 左上角x1,y1，w，h ——>左上角x1,y1，右下角x2,y2
        mot_tracker.update(dets)  # kalman 预测与更新
        trackers = mot_tracker.trackers
        image_path = os.path.join(".\\img", '%06d.jpg' % (frame))  # 图片路径
        image = cv2.imread(image_path)
        # x_p 目标横向、纵向距离。速度以及加速度
        for d, x_p in trackers:
            x1, y1, w, h = d.get_state()[0]  # 获取 当前目标框状态
            id = d.id
            color = colours[int(id) % 32, :]
            color = (int(color[0]), int(color[1]), int(color[2]))
            cv2.rectangle(image, (int(x1), int(y1)), (int(w), int(h)), color, 3)  # 画框
            cv2.putText(image, str(int(id)), (int(x1), int(y1) - 10), cv2.FONT_HERSHEY_SIMPLEX, 1,
                        color, 3)  # 画id
            cv2.putText(image, str(np.round(x_p[0][0], 2)), (int(x1), int(y1) + 30),
                        cv2.FONT_HERSHEY_SIMPLEX, 1,
                        color, 3)  # 画距离
        if display:
            cv2.namedWindow("show")
            cv2.imshow("show", image)
            cv2.waitKey(0)
        if video_save:
            video.write(image)


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

sort.py

这部分代码为核心计算代码，主要调用 kalman 预测 predict 与 更新 update
在跟踪航迹 self.trackers 里面添加距离信息，也进行一个预测与更新，不参与匹配权重的运算。
主要对测距起到一个平滑的作用。

from __future__ import print_function
from kalman import *
from distance import *


def linear_assignment(cost_matrix):
    try:
        import lap
        _, x, y = lap.lapjv(cost_matrix, extend_cost=True)
        return np.array([[y[i], i] for i in x if i >= 0])  #
    except ImportError:
        from scipy.optimize import linear_sum_assignment
        x, y = linear_sum_assignment(cost_matrix)
        return np.array(list(zip(x, y)))


def iou_batch(bb_test, bb_gt):
    bb_gt = np.expand_dims(bb_gt, 0)
    bb_test = np.expand_dims(bb_test, 1)

    xx1 = np.maximum(bb_test[..., 0], bb_gt[..., 0])
    yy1 = np.maximum(bb_test[..., 1], bb_gt[..., 1])
    xx2 = np.minimum(bb_test[..., 2], bb_gt[..., 2])
    yy2 = np.minimum(bb_test[..., 3], bb_gt[..., 3])
    w = np.maximum(0., xx2 - xx1)
    h = np.maximum(0., yy2 - yy1)
    wh = w * h
    o = wh / ((bb_test[..., 2] - bb_test[..., 0]) * (bb_test[..., 3] - bb_test[..., 1])
              + (bb_gt[..., 2] - bb_gt[..., 0]) * (bb_gt[..., 3] - bb_gt[..., 1]) - wh)
    return (o)


def associate_detections_to_trackers(detections, trackers, iou_threshold=0.3):
    if (len(trackers) == 0):
        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 5), dtype=int)

    iou_matrix = iou_batch(detections, trackers)

    if min(iou_matrix.shape) > 0:
        a = (iou_matrix > iou_threshold).astype(np.int32)
        if a.sum(1).max() == 1 and a.sum(0).max() == 1:
            matched_indices = np.stack(np.where(a), axis=1)
        else:
            matched_indices = linear_assignment(-iou_matrix)
    else:
        matched_indices = np.empty(shape=(0, 2))

    unmatched_detections = []
    for d, det in enumerate(detections):
        if (d not in matched_indices[:, 0]):
            unmatched_detections.append(d)
    unmatched_trackers = []
    for t, trk in enumerate(trackers):
        if (t not in matched_indices[:, 1]):
            unmatched_trackers.append(t)

    matches = []
    for m in matched_indices:
        if (iou_matrix[m[0], m[1]] < iou_threshold):
            unmatched_detections.append(m[0])
            unmatched_trackers.append(m[1])
        else:
            matches.append(m.reshape(1, 2))
    if (len(matches) == 0):
        matches = np.empty((0, 2), dtype=int)
    else:
        matches = np.concatenate(matches, axis=0)

    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)


class Sort(object):
    def __init__(self, max_age=1, min_hits=3, iou_threshold=0.3):
        self.max_age = max_age
        self.min_hits = min_hits
        self.iou_threshold = iou_threshold
        self.trackers = []
        self.frame_count = 0
        self.distance_kalman = Distance(0.1)  # 0.1s 1s 十帧
        self.p = np.eye(6)  # 初始化协方差
        self.r_t = np.array(
            [0, 0, 1, 0,
             1, 0, 0, 0,
             0, 1, 0, 1.2,
             0., 0., 0., 1.]).reshape(4, 4)  # 相机外参
        self.k = np.array([1000, 0.0, 960, 0.0, 1000, 540, 0.0, 0.0, 1.0]).reshape(3, 3)  # 相机内参
        self.h = 1.2  # 相机离地面高度 1.2 m
        self.pitch = 0  # 相机 pitch （俯仰角）

    def update(self, dets=np.empty((0, 5))):
        self.frame_count += 1
        # 根据上一帧航迹的框 预测当前帧的框.
        trks = np.zeros((len(self.trackers), 5))
        to_del, ret = [], []
        for t, trk in enumerate(trks):
            pos = self.trackers[t][0].predict()[0]  # 预测框的状态
            self.trackers[t][1] = self.distance_kalman.predict_kalman(self.trackers[t][1])  # 预测距离的状态
            trk[:] = [pos[0], pos[1], pos[2], pos[3], 0]
            if np.any(np.isnan(pos)):
                to_del.append(t)
        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
        for t in reversed(to_del):
            self.trackers.pop(t)
        # 匈牙利匹配 上一帧预测框与当前帧检测框进行 iou 匹配
        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets, trks, self.iou_threshold)
        # 如果匹配上 则更新修正当前检测框
        for m in matched:
            det = dets[m[0], :]
            distance = get_distance((det[0] + det[2]) / 2, det[3], self.h, self.pitch, self.k, self.r_t[:3, :3],
                                    self.r_t[:3, 3])
            self.trackers[m[1]][1] = self.distance_kalman.updata_kalman([distance[0], distance[1]],
                                                                        self.trackers[m[1]][1])
            self.trackers[m[1]][0].update(det)
        # 如果检测框未匹配上，则当作新目标，新起航迹
        for i in unmatched_dets:
            det = dets[i, :]
            distance = get_distance((det[0] + det[2]) / 2, det[3], self.h, self.pitch, self.k, self.r_t[:3, :3],
                                    self.r_t[:3, 3])  # 目标测距
            # 目标状态 （x,y,vx,vy,ax,ay） kalman协方差
            x_p = (np.array([[distance[0], 0, 0, distance[1], 0, 0]]).T, self.p)
            trk = [KalmanBoxTracker(det), x_p]
            self.trackers.append(trk)
        i = len(self.trackers)
        for trk in reversed(self.trackers):
            d = trk[0].get_state()[0]
            if (trk[0].time_since_update < 1) and (
                    trk[0].hit_streak >= self.min_hits or self.frame_count <= self.min_hits):
                ret.append(np.concatenate((d, [trk[0].id + 1])).reshape(1, -1))  # +1 as MOT benchmark requires positive
            i -= 1
            # 如果超过self.max_age（3）帧都没有匹配上，则应该去除这个航迹
            if (trk[0].time_since_update > self.max_age):
                self.trackers.pop(i)
        if (len(ret) > 0):
            return np.concatenate(ret)
        return np.empty((0, 5))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

kalman.py

这部分代码是 kalman 算法核心代码
主要对目标框 bbox 进行预测与更新。bbox 状态为 [center_x, center_y, s, r, center_x’, center_y’, s’]
s = w * h r = w / h bbox 宽高比保持不变

import numpy as np
from filterpy.kalman import KalmanFilter


def convert_bbox_to_z(bbox):
    w = bbox[2] - bbox[0]
    h = bbox[3] - bbox[1]
    x = bbox[0] + w / 2.
    y = bbox[1] + h / 2.
    s = w * h
    r = w / float(h)
    return np.array([x, y, s, r]).reshape((4, 1))


def convert_x_to_bbox(x, score=None):
    w = np.sqrt(x[2] * x[3])
    h = x[2] / w
    if (score == None):
        return np.array([x[0] - w / 2., x[1] - h / 2., x[0] + w / 2., x[1] + h / 2.]).reshape((1, 4))
    else:
        return np.array([x[0] - w / 2., x[1] - h / 2., x[0] + w / 2., x[1] + h / 2., score]).reshape((1, 5))


class KalmanBoxTracker(object):
    count = 0

    def __init__(self, bbox):
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        self.kf.F = np.array(
            [[1, 0, 0, 0, 1, 0, 0],
             [0, 1, 0, 0, 0, 1, 0],
             [0, 0, 1, 0, 0, 0, 1],
             [0, 0, 0, 1, 0, 0, 0],
             [0, 0, 0, 0, 1, 0, 0],
             [0, 0, 0, 0, 0, 1, 0],
             [0, 0, 0, 0, 0, 0, 1]])
        self.kf.H = np.array(
            [[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]])

        self.kf.R[2:, 2:] *= 10.
        self.kf.P[4:, 4:] *= 1000.
        self.kf.P *= 10.
        self.kf.Q[-1, -1] *= 0.01
        self.kf.Q[4:, 4:] *= 0.01

        self.kf.x[:4] = convert_bbox_to_z(bbox)
        self.time_since_update = 0
        self.id = KalmanBoxTracker.count
        KalmanBoxTracker.count += 1
        self.history = []
        self.hits = 0
        self.hit_streak = 0
        self.age = 0

    def update(self, bbox):
        self.time_since_update = 0
        self.history = []
        self.hits += 1
        self.hit_streak += 1
        self.kf.update(convert_bbox_to_z(bbox))

    def predict(self):
        if ((self.kf.x[6] + self.kf.x[2]) <= 0):
            self.kf.x[6] *= 0.0
        self.kf.predict()
        self.age += 1
        if (self.time_since_update > 0):
            self.hit_streak = 0
        self.time_since_update += 1
        self.history.append(convert_x_to_bbox(self.kf.x))
        return self.history[-1]

    def get_state(self):
        return convert_x_to_bbox(self.kf.x)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

distance.py

这部分代码是测距核心代码，以及对目标测距的预测与更新
目标状态为 （x,y,vx,vy,ax,ay） 目标横向距离，纵向距离，横向速度，纵向速度，横向加速度，纵向加速度。
关于目标前后帧匹配，是利用 iou 匹配进行的，所以要基于目标检测框的匹配跟踪。

import numpy as np


def get_distance(pixe_x, pixe_y, h, pitch, K, R, T):
    sigma = np.arctan((pixe_y - K[1][2]) / K[1][1])
    z = h * np.cos(sigma) / np.sin(sigma + pitch)  # 深度
    x_pixe, y_pixe = 2 * K[0][2] - pixe_x, 2 * K[1][2] - pixe_y
    camera_x = z * (x_pixe / K[0][0] - K[0][2] / K[0][0])
    camera_y = z * (y_pixe / K[1][1] - K[1][2] / K[1][1])
    camera_z = z
    x = R[0][0] * camera_x + R[0][1] * camera_y + R[0][2] * camera_z + T[0]
    y = R[1][0] * camera_x + R[1][1] * camera_y + R[1][2] * camera_z + T[1]
    # z = R[2][0] * camera_x + R[2][1] * camera_y + R[2][2] * camera_z + T[2]
    return x, y


class Distance():
    def __init__(self, t):
        self.t = t  # 时间间隔0.1s
        self.F = np.array([[1, t, t * t / 2, 0, 0, 0],
                           [0, 1, t, 0, 0, 0],
                           [0, 0, 1, 0, 0, 0],
                           [0, 0, 0, 1, t, t * t / 2],
                           [0, 0, 0, 0, 1, t],
                           [0, 0, 0, 0, 0, 1]])
        self.sigma_a = 0.02  # 加速度误差0.2m/s2
        self.sigma_x, self.sigma_y = 0.3, 0.2  # x、y测量距离误差
        self.Q = np.array([[np.power(t, 4) / 4, np.power(t, 3) / 3, np.power(t, 2) / 2, 0, 0, 0],
                           [np.power(t, 3) / 3, np.power(t, 2) / 2, t, 0, 0, 0],
                           [np.power(t, 2) / 2, t, 1, 0, 0, 0],
                           [0, 0, 0, np.power(t, 4) / 4, np.power(t, 3) / 3, np.power(t, 2) / 2],
                           [0, 0, 0, np.power(t, 3) / 3, np.power(t, 2) / 2, t],
                           [0, 0, 0, np.power(t, 2) / 2, t, 1]]) * self.sigma_a * self.sigma_a  # 过程噪声矩阵
        self.R_n = np.array([[self.sigma_x ** 2, 0], [0, self.sigma_y ** 2]])  # 测量噪声协方差
        self.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])

    def updata_kalman(self, Z, X_P):
        """
        :param Z:测量值
        :param X:状态矩阵   [x,vx,ax,y,vy,ay]
        :param P:状态协方差矩阵
        :return:更新后的X,P
        """
        X, P = X_P
        # print(H @ P @ H.T)
        Z_1 = np.array([Z]).T
        # print(Z_1)
        K = P @ np.transpose(self.H) @ np.linalg.inv(np.dot(np.dot(self.H, P), np.transpose(self.H)) + self.R_n)
        # print(H @ K)
        X = X + K @ (Z_1 - self.H @ X)
        P = (np.identity(6) - K @ self.H) @ P @ np.transpose(np.identity(6) - K @ self.H) + K @ self.R_n @ np.transpose(
            K)
        return X, P

    def predict_kalman(self, X_P):
        X, P = X_P
        X = self.F @ X
        P = self.F @ P @ np.transpose(self.F) + self.Q
        return X, P
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

---

结语

• 运行 main.py ，结果会保存视频。博主在本地是跑通的，如果有什么疑问，可以私信交流。
• 关于数据，我是在网上找的开源数据跑的。相机的参数是模拟的。