粒子滤波目标跟踪代码

粒子滤波是一种基于蒙特卡洛方法的非参数化滤波算法，广泛应用于目标跟踪领域。它通过一组随机样本（粒子）来近似表示目标状态的后验概率分布，并通过重采样和权重更新来逐步逼近真实的后验分布。以下是一个简单的粒子滤波目标跟踪代码示例：

import numpy as np
import cv2

# 初始化粒子
def initialize_particles(num_particles, img_size):
    particles = np.random.uniform(0, img_size, (num_particles, 2))
    weights = np.ones(num_particles) / num_particles
    return particles, weights

# 运动模型
def motion_model(particles, motion_noise):
    particles += np.random.normal(0, motion_noise, particles.shape)
    return particles

# 观测模型
def observation_model(particles, target_color, img):
    weights = np.zeros(particles.shape[0])
    for i in range(particles.shape[0]):
        x, y = int(particles[i, 0]), int(particles[i, 1])
        if x >= 0 and x < img.shape[0] and y >= 0 and y < img.shape[1]:
            weights[i] = 1 / (1 + np.sum(np.abs(img[x, y] - target_color)))
    weights += 1e-6
    weights /= np.sum(weights)
    return weights

# 重采样
def resample(particles, weights):
    indices = np.random.choice(len(particles), len(particles), p=weights)
    particles = particles[indices]
    weights = np.ones(len(particles)) / len(particles)
    return particles, weights

# 主函数
def particle_filter_tracking(video_path, num_particles=1000, motion_noise=5):
    cap = cv2.VideoCapture(video_path)
    ret, frame = cap.read()
    img_size = frame.shape[:2]
    particles, weights = initialize_particles(num_particles, img_size)
    target_color = frame[int(img_size[0]/2), int(img_size[1]/2)]

    while ret:
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        particles = motion_model(particles, motion_noise)
        weights = observation_model(particles, target_color, frame)
        particles, weights = resample(particles, weights)

        # 绘制结果
        x, y = np.mean(particles, axis=0).astype(int)
        cv2.circle(frame, (y, x), 10, (0, 0, 255), -1)
        cv2.imshow('Particle Filter Tracking', frame)
        if cv2.waitKey(30) & 0xFF == ord('q'):
            break

        ret, frame = cap.read()

    cap.release()
    cv2.destroyAllWindows()

# 调用主函数
particle_filter_tracking('path_to_video.mp4')

这个代码示例展示了如何使用粒子滤波进行目标跟踪。代码中包含以下主要步骤：

1. 初始化粒子。
2. 运动模型：模拟粒子的运动。
3. 观测模型：根据目标颜色更新粒子权重。
4. 重采样：重新采样粒子以逼近真实的后验分布。
5. 主函数：读取视频并调用上述步骤进行跟踪。