单目视觉光流法定位代码

单目视觉光流法是一种基于连续帧之间的像素运动关系来实现定位的方法。下面是一个简单的单目视觉光流法定位的代码示例：

import numpy as np
import cv2

# 读取视频文件或摄像头
cap = cv2.VideoCapture(0)

# 创建Lucas-Kanade光流法对象
lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# 读取第一帧并提取特征点
ret, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **dict(maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7))

# 创建一个空的蒙版图像
mask = np.zeros_like(old_frame)

while True:
    # 读取当前帧并转换为灰度图像
    ret, frame = cap.read()
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 计算光流
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)

    # 选择好的特征点
    good_new = p1[st == 1]
    good_old = p0[st == 1]

    # 画出轨迹
    for i, (new, old) in enumerate(zip(good_new, good_old)):
        a, b = new.ravel()
        c, d = old.ravel()
        mask = cv2.line(mask, (a, b), (c, d), (0, 255, 0), 2)
        frame = cv2.circle(frame, (a, b), 5, (0, 0, 255), -1)

    # 在当前帧上显示轨迹
    img = cv2.add(frame, mask)

    # 更新上一帧和特征点
    old_gray = frame_gray.copy()
    p0 = good_new.reshape(-1, 1, 2)

    # 显示结果
    cv2.imshow('frame', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

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

注释：

- `cv2.VideoCapture(0)`：打开摄像头。
- `dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))`：设置Lucas-Kanade光流法的参数。
- `cv2.goodFeaturesToTrack`：提取特征点。
- `cv2.calcOpticalFlowPyrLK`：计算光流。
- `cv2.line`：画出特征点连线。
- `cv2.circle`：画出特征点。
- `cv2.add`：将原始帧和轨迹合并。
- `cv2.waitKey`：等待用户按下“q”键退出。
- `cap.release()`：释放摄像头。
- `cv2.destroyAllWindows()`：关闭所有窗口。