赣州网站制作较好的公司,郑州市住房和城乡建设厅网站,WordPress的FTP登录凭据,英文网站建设推广文章目录 前言python代码#xff08;带注释#xff09;main.pysort.pykalman.pydistance.py 结语 前言 先放效果图。目标框内左上角#xff0c;显示的是目标距离相机的纵向距离。目标横向距离、速度已求出#xff0c;没在图片展示。这里不仅仅实现对目标检测框的跟踪#… 文章目录 前言python代码带注释main.pysort.pykalman.pydistance.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 Nonemot_tracker Sort(max_agemax_age, min_hitsmin_hits, iou_thresholdiou_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,y1wh ——左上角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_costTrue)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_threshold0.3):if (len(trackers) 0):return np.empty((0, 2), dtypeint), np.arange(len(detections)), np.empty((0, 5), dtypeint)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), axis1)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), dtypeint)else:matches np.concatenate(matches, axis0)return matches, np.array(unmatched_detections), np.array(unmatched_trackers)class Sort(object):def __init__(self, max_age1, min_hits3, iou_threshold0.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, detsnp.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_age3帧都没有匹配上则应该去除这个航迹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, scoreNone):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_x7, dim_z4)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,PX, 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 结果会保存视频。博主在本地是跑通的如果有什么疑问可以私信交流。关于数据我是在网上找的开源数据跑的。相机的参数是模拟的。
