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

前言

• 先放效果图。目标框内左上角，显示的是目标距离相机的纵向距离。目标横向距离、速度已求出，没在图片展示。
• 这里不仅仅实现对目标检测框的跟踪，且可以实现单相机进行对目标进行测距跟踪。
• 想了解详细原理可以参考往期博客：【目标跟踪】多目标跟踪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 Nonemot_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,y2mot_tracker.update(dets)  # kalman 预测与更新trackers = mot_tracker.trackersimage_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.idcolor = 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)  # 画idcv2.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_assignmentx, 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 * ho = 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_ageself.min_hits = min_hitsself.iou_threshold = iou_thresholdself.trackers = []self.frame_count = 0self.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 mself.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 positivei -= 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 KalmanFilterdef 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 * hr = 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] / wif (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 = 0def __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.01self.kf.Q[4:, 4:] *= 0.01self.kf.x[:4] = convert_bbox_to_z(bbox)self.time_since_update = 0self.id = KalmanBoxTracker.countKalmanBoxTracker.count += 1self.history = []self.hits = 0self.hit_streak = 0self.age = 0def update(self, bbox):self.time_since_update = 0self.history = []self.hits += 1self.hit_streak += 1self.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.0self.kf.predict()self.age += 1if (self.time_since_update > 0):self.hit_streak = 0self.time_since_update += 1self.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 npdef 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_ycamera_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 = zx = 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, yclass Distance():def __init__(self, t):self.t = t  # 时间间隔0.1sself.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/s2self.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, Pdef predict_kalman(self, X_P):X, P = X_PX = self.F @ XP = self.F @ P @ np.transpose(self.F) + self.Qreturn X, P

---

结语

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