文章目录
- 前言
- python代码（带注释）
- main.py
- sort.py
- kalman.py
- distance.py
- 结语
前言
- 先放效果图。目标框内左上角，显示的是目标距离相机的纵向距离。目标横向距离、速度已求出，没在图片展示。
- 这里不仅仅实现对目标检测框的跟踪，且可以实现单相机进行对目标进行测距跟踪。
- 想了解详细原理可以参考往期博客：【目标跟踪】多目标跟踪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)
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))
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)
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
结语
- 运行 main.py ，结果会保存视频。博主在本地是跑通的，如果有什么疑问，可以私信交流。
- 关于数据，我是在网上找的开源数据跑的。相机的参数是模拟的。
