python# ====================== # 单个轨迹的卡尔曼滤波器 # ====================== class KalmanTrack:def __init__(self, bbox):# 状态向量:[x, y, vx, vy]中心坐标+速度self.kf = cv2.KalmanFilter(4, 2)# 状态转移矩阵(匀速模型)self.kf.transitionMatrix = np.array([[1, 0, 1, 0],[0, 1, 0, 1],[0, 0, 1, 0],[0, 0, 0, 1]], np.float32)# 测量矩阵self.kf.measurementMatrix = np.array([[1, 0, 0, 0],[0, 1, 0, 0]], np.float32)# 初始化状态x, y, w, h = bboxself.kf.statePost = np.array([[x + w/2], [y + h/2], [0], [0]], np.float32)# 过程噪声协方差self.kf.processNoiseCov = np.eye(4, dtype=np.float32) * 0.03# 测量噪声协方差self.kf.measurementNoiseCov = np.eye(2, dtype=np.float32) * 1.0self.time_since_update = 0# 未更新帧数self.id = 0self.hits = 0# 匹配成功次数self.is_confirmed = False# 是否为有效轨迹def predict(self):"""预测下一帧位置"""pred = self.kf.predict()self.time_since_update += 1return pred[:2].flatten()# 返回预测的中心坐标 [x, y]def update(self, bbox):"""用检测结果更新"""x, y, w, h = bboxmeasurement = np.array([[x + w/2], [y + h/2]], np.float32)self.kf.correct(measurement)self.time_since_update = 0self.hits += 1# 连续匹配3帧以上确认为有效轨迹if self.hits >= 3:self.is_confirmed = Truedef get_bbox(self, default_size=6):"""获取当前目标框(极小目标默认固定尺寸)"""x, y = self.kf.statePost[:2].flatten()return [int(x - default_size/2), int(y - default_size/2), default_size, default_size] # ====================== # 计算IOU(用于匹配代价) # ====================== def bbox_iou(box1, box2):x1, y1, w1, h1 = box1x2, y2, w2, h2 = box2inter_x1 = max(x1, x2)inter_y1 = max(y1, y2)inter_x2 = min(x1 + w1, x2 + w2)inter_y2 = min(y1 + h1, y2 + h2)inter_area = max(0, inter_x2 - inter_x1) * max(0, inter_y2 - inter_y1)union_area = w1 * h1 + w2 * h2 - inter_areareturn inter_area / (union_area + 1e-6) # ====================== # 纯运动跟踪器主类 # ====================== class MotionTracker:def __init__(self, max_age=10, min_hits=3, iou_threshold=0.1):self.tracks = []self.next_id = 1self.max_age = max_age# 最大丢失帧数self.min_hits = min_hits# 确认轨迹最小匹配次数self.iou_threshold = iou_thresholddef update(self, detections):# 1. 所有轨迹先做预测for track in self.tracks:track.predict()# 2. 计算代价矩阵(IOU越大代价越小)pred_boxes = [t.get_bbox() for t in self.tracks]det_boxes = detections.tolist() if len(detections) > 0 else []if len(pred_boxes) > 0 and len(det_boxes) > 0:cost_matrix = np.zeros((len(pred_boxes), len(det_boxes)))for i, pred in enumerate(pred_boxes):for j, det in enumerate(det_boxes):cost_matrix[i, j] = 1 - bbox_iou(pred, det)# 匈牙利算法匹配row_ind, col_ind = linear_sum_assignment(cost_matrix)matched_trks = set()matched_dets = set()for i, j in zip(row_ind, col_ind):if cost_matrix[i, j] < (1 - self.iou_threshold):self.tracks[i].update(det_boxes[j])matched_trks.add(i)matched_dets.add(j)unmatched_dets = [j for j in range(len(det_boxes)) if j not in matched_dets]else:unmatched_dets = list(range(len(det_boxes)))# 3. 未匹配的检测新建轨迹for j in unmatched_dets:new_track = KalmanTrack(det_boxes[j])new_track.id = self.next_idself.next_id += 1self.tracks.append(new_track)# 4. 删除长时间丢失的轨迹self.tracks = [t for t in self.tracks if t.time_since_update < self.max_age]# 5. 返回已确认的轨迹return [t.get_bbox() + [t.id] for t in self.tracks if t.is_confirmed] |