📚 课程目录第4章实现自动对焦对比度检测

4. 实现自动对焦对比度检测

任务目标

实现基于Tenengrad梯度算子的自动对焦（AF）算法：对镜头移动过程中采集的图像序列计算对比度值，通过搜索策略找到对比度峰值对应的镜头位置，完成自动对焦闭环。

核心原理

Tenengrad算子使用Sobel核计算图像水平与垂直方向的梯度，梯度幅值平方和作为对比度评价函数。对焦准确时图像边缘最锐利，梯度能量最大；离焦时边缘模糊，梯度能量下降。通过遍历镜头位置并比较对比度值，即可定位最佳对焦位置。

依赖与环境

Python 3.8+，OpenCV（pip install opencv-python numpy）
若使用真实摄像头：需支持镜头位置控制（如UVC协议的V4L2驱动）

关键完整代码

import cv2
import numpy as np

def tenengrad(gray):
    """计算Tenengrad对比度：Sobel梯度幅值平方和"""
    gx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3)
    gy = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=3)
    return float((gx**2 + gy**2).sum())

def autofocus(capture_frame, set_lens_pos, get_lens_pos, pos_range, step=5):
    """
    自动对焦主流程：粗扫+精细搜索
    capture_frame: 返回当前帧BGR图像的可调用对象
    set_lens_pos:  设置镜头位置（输入整数）
    get_lens_pos:  返回当前镜头位置（整数）
    pos_range:     (min_pos, max_pos) 镜头可移动范围
    step:          粗扫步长
    """
    min_pos, max_pos = pos_range
    best_pos, best_score = min_pos, -1.0

    # 阶段1：粗扫描，确定峰值区间
    scores = []
    positions = []
    for pos in range(min_pos, max_pos + 1, step):
        set_lens_pos(pos)
        frame = capture_frame()
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        score = tenengrad(gray)
        scores.append(score)
        positions.append(pos)
        if score > best_score:
            best_score = score
            best_pos = pos

    # 阶段2：在粗扫峰值附近精细搜索（±step范围，步长1）
    fine_start = max(min_pos, best_pos - step)
    fine_end = min(max_pos, best_pos + step)
    for pos in range(fine_start, fine_end + 1):
        set_lens_pos(pos)
        frame = capture_frame()
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        score = tenengrad(gray)
        if score > best_score:
            best_score = score
            best_pos = pos

    # 回到最佳位置
    set_lens_pos(best_pos)
    return best_pos, best_score

# ===== 模拟验证（无摄像头时使用） =====
def simulate_focus_sequence():
    """生成模拟离焦-聚焦图像序列，验证搜索逻辑"""
    # 合成测试图：黑白棋盘格
    test_img = np.zeros((200, 200), dtype=np.uint8)
    test_img[::20, :] = 255
    test_img[:, ::20] = 255

    def blur_at(pos):
        # 模拟镜头位置：pos=50时最清晰，偏离则高斯模糊
        sigma = abs(pos - 50) * 0.15 + 0.1
        return cv2.GaussianBlur(test_img, (0, 0), sigma)

    # 模拟摄像头帧（转BGR）
    def capture():
        cur = get_lens_pos()
        gray = blur_at(cur)
        return cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)

    lens_pos = [0]
    def set_lens_pos(pos):
        lens_pos[0] = pos
    def get_lens_pos():
        return lens_pos[0]

    best_pos, best_score = autofocus(
        capture, set_lens_pos, get_lens_pos, (0, 100), step=10
    )
    print(f"搜索到最佳位置: {best_pos} (期望50)")
    print(f"最佳Tenengrad得分: {best_score:.2f}")
    assert abs(best_pos - 50) <= 1, "对焦位置偏差过大"
    print("验证通过：模拟对焦成功")

if __name__ == "__main__":
    simulate_focus_sequence()

运行与验证

模拟验证：直接运行脚本，输出应显示搜索到位置接近50（模拟清晰位置），并打印“验证通过”。该测试使用高斯模糊模拟离焦，验证搜索算法能收敛到清晰位置。
真实摄像头接入：将capture_frame替换为摄像头读取（如cap.read()），set_lens_pos/get_lens_pos替换为V4L2镜头控制命令（如v4l2-ctl --set-ctrl focus_absolute=）。运行后观察镜头是否移动到画面最清晰的位置。

判定标准

模拟测试：搜索到的位置与真实清晰位置（50）偏差≤1，且得分明显高于离焦位置。
真实场景：对焦后画面主观清晰度显著提升；手动微调镜头位置时，Tenengrad得分在算法返回位置处最大。

常见问题

搜索到局部峰值：若场景纹理单一，可改用更大粗扫步长或增加Sobel核尺寸（ksize=5）提高鲁棒性。
帧率低导致搜索慢：可先缩小图像（如cv2.resize至1/4）计算对比度，减少计算量。
镜头回程差：机械镜头存在回程间隙，建议搜索方向保持一致（如始终从近到远），或到位后等待50ms再采集帧。