实现质心跟踪+卡尔曼滤波算法跟踪车辆代码

以下是一个简单的质心跟踪+卡尔曼滤波算法跟踪车辆的示例代码，使用Python编写：

import cv2
import numpy as np

# 帧大小
FRAME_WIDTH = 640
FRAME_HEIGHT = 480

# ROI（感兴趣区域）坐标和大小
ROI_X = 100
ROI_Y = 100
ROI_WIDTH = 400
ROI_HEIGHT = 300

# 车辆颜色阈值
CAR_COLOR_MIN = np.array([0, 0, 0], np.uint8)
CAR_COLOR_MAX = np.array([255, 255, 150], np.uint8)

# 卡尔曼滤波参数
KF_STATE_DIM = 4 # 状态量维度（位置x, 位置y, 速度x, 速度y）
KF_MEASUREMENT_DIM = 2 # 测量量维度（位置x, 位置y）
KF_PROCESS_NOISE_COV = np.eye(KF_STATE_DIM, dtype=np.float32) * 0.1 # 过程噪声协方差
KF_MEASUREMENT_NOISE_COV = np.eye(KF_MEASUREMENT_DIM, dtype=np.float32) * 0.01 # 测量噪声协方差
KF_ERROR_COV_POST = np.eye(KF_STATE_DIM, dtype=np.float32) # 后验误差协方差
KF_STATE_POST = np.zeros((KF_STATE_DIM, 1), dtype=np.float32) # 后验状态

# 创建卡尔曼滤波器对象
kf = cv2.KalmanFilter(KF_STATE_DIM, KF_MEASUREMENT_DIM)
kf.processNoiseCov = KF_PROCESS_NOISE_COV
kf.measurementNoiseCov = KF_MEASUREMENT_NOISE_COV
kf.errorCovPost = KF_ERROR_COV_POST
kf.statePost = KF_STATE_POST

# 摄像头对象
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)

while True:
    # 读取帧
    ret, frame = cap.read()
    if not ret:
        break
    
    # 截取感兴趣区域
    roi = frame[ROI_Y:ROI_Y+ROI_HEIGHT, ROI_X:ROI_X+ROI_WIDTH]
    
    # 车辆识别
    hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    car_mask = cv2.inRange(hsv, CAR_COLOR_MIN, CAR_COLOR_MAX)
    car_contours, _ = cv2.findContours(car_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    # 仅考虑最大的车辆轮廓
    if len(car_contours) > 0:
        car_contour = max(car_contours, key=cv2.contourArea)
        
        # 计算质心
        car_moments = cv2.moments(car_contour)
        car_center = (int(car_moments["m10"] / car_moments["m00"]), int(car_moments["m01"] / car_moments["m00"]))
        
        # 卡尔曼滤波
        kf.correct(np.array(car_center, dtype=np.float32).reshape((2, 1)))
        predicted_center = np.int32(kf.predict())
        
        # 绘制质心和预测中心
        cv2.circle(roi, tuple(car_center), 5, (0, 0, 255), -1)
        cv2.circle(roi, tuple(predicted_center), 5, (0, 255, 0), -1)
    
    # 显示帧
    cv2.imshow("Frame", frame)
    
    # 按下q键退出
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放资源
cap.release()
cv2.destroyAllWindows()

该代码使用OpenCV库中的一些函数进行车辆识别、质心跟踪和卡尔曼滤波。在每一帧中，它首先截取感兴趣区域，然后根据车辆颜色阈值找到车辆二值图像，并使用最大的车辆轮廓计算质心。然后，它使用卡尔曼滤波器对质心进行滤波，并绘制质心和预测中心。最后，它显示帧并等待按下q键退出。