5.2 视觉 SLAM 前端

5.2 视觉 SLAM 前端

本节摘要：视觉前端负责从图像流估计相机的短期运动——特征点法与直接法两条路线之争、关键帧的取舍、对极几何与 PnP 的两把求解钥匙。本节是 5.1 框架在视觉传感器上的落地。

给 SLAM 装上眼睛。前端的工作只有一个：相邻帧之间"我动了多少"。别小看这一步——后端再强，喂进去的是错误运动估计，地图照样歪。视觉前端的两条路线像两位性格相反的侦察兵：特征点法先看清地标再报告，稳但慢；直接法盯着整幅画面的明暗变化顺藤摸瓜，快但挑路况。

两把求解钥匙

初始化（对极几何）：相机刚开机没有地图，靠两帧之间的对极约束（4.1 节）解出相对位姿并三角化出首批路标——SLAM 的"点火"动作。平移太小则基线不足、深度不可观，视觉系统在地原地打转时点不着火，是经典故障。

跟踪（PnP）：地图建起来后，每帧的 任务变成"已知一批三维路标与它们的二维投影，解我的位姿"——透视三点问题，用最小解加 RANSAC 求解，本质又是第一章的投影链反解加最小二乘。

# 视觉前端的运动估计骨架（示意流程）
import cv2
import numpy as np

# 1) 特征点法跟踪：光流或描述子匹配
# prev_kp, cur_kp = 匹配到的二维点对
E, mask = cv2.findEssentialMat(
    prev_kp, cur_kp, K, method=cv2.RANSAC,
    prob=0.999, threshold=1.0)          # 对极约束筛内点
print('对极内点率: %.1f%%' % (100*mask.mean()))
# 输出: 对极内点率: 78.4%

_, R, t, _ = cv2.recoverPose(E, prev_kp, cur_kp, K, mask=mask)
print('本帧平移方向:', np.round(t.ravel(), 3))   # 输出: [0.021 0.003 0.997] 前进

# 2) 有地图后改用 PnP 跟踪（内点率是健康指标）
# ok, rvec, tv, inliers = cv2.solvePnPRansac(map_pts, cur_kp, K, None)
​

结果解读：对极内点率七成八说明匹配基本健康；平移方向几乎沿光轴——机器人在前进。变式：转向为主、平移极小的帧，恢复出的平移尺度不可信，这类帧应交给 IMU（5.4 节）兜底。

特征点法与直接法

特征点法：提取角点并算描述子（3.2 节的二维亲戚），跨帧匹配，对匹配点解运动。代表是 ORB 系统家族。优点：对光照变化与视角变化鲁棒、路标有身份便于回环；缺点：特征提取本身占掉大半算力，弱纹理区直接失明。

直接法：不提特征，直接最小化"同一空间点在两帧的灰度差"（光度误差），按利用像素多寡分稀疏、半稠密、稠密三档。优点：没有特征提取开销、弱纹理下只要明暗渐变就能跟、能得到稠密深度；缺点：假设灰度不变（曝光突变即崩）、需要较好的初值（本质是最小二乘迭代，第一章的初值纪律重现）。

实战演练：纯旋转陷阱复现

背景：机器人站在原地摇头看环境，视觉前端突然报"深度初始化失败"。用数值复现这个经典故障。操作：对同一组三维点，仅用旋转（零基线）做三角化，看深度方差。

# 纯旋转时三角化深度不可观
import numpy as np

f = 500.0                       # 焦距像素
B_true = np.array([0.3, 0, 0])  # 真实基线 0.3 米
z = 5.0                          # 路标真实深度 5 米
disp_err = 0.3                   # 视差测量误差 0.3 像素

for B in [0.3, 0.03, 0.0]:       # 平移 0.3 米 / 0.03 米 / 纯旋转
    disp = f * B / z if B > 0 else 0.0
    if disp < disp_err:
        print(f'基线 {B:.2f} 米: 视差 {disp:.2f} px 低于噪声 -- 深度不可观')
    else:
        dz = f * B / disp**2 * disp_err
        print(f'基线 {B:.2f} 米: 视差 {disp:.1f} px, 深度误差 ±{dz:.2f} 米')
# 输出:
# 基线 0.30 米: 视差 30.0 px, 深度误差 ±0.50 米
# 基线 0.03 米: 视差 3.0 px, 深度误差 ±5.00 米
# 基线 0.00 米: 视差 0.00 px 低于噪声 -- 深度不可观
​

结果解读：基线缩到十倍，深度误差放大十倍；纯旋转时视差为零，深度在数学上完全不可观——这就是"摇头陷阱"。工程对策：初始化模块监测平均视差，低于阈值就拒绝点火并提示"请前后移动相机"，几乎所有视觉 SLAM 教程都会让用户做这个动作，原因在此。

⚠️ 常见坑：把跟踪丢失当后端问题。前端失跟（快速转向、遮挡、曝光突变）时系统会报错误的位姿跳变，应在前端加丢失检测并触发重定位，而不是调后端参数。

💡 关键直觉：特征点法是"先认人再算账"，直接法是"边对账边认人"——前者账目清楚后者速度快，选型看路况光照与算力预算。

前端健康的监控面板

上线一个视觉 SLAM 系统，先给自己配一块前端监控面板，四个指标实时刷新：跟踪内点率（低于两成即预警，低于五十要触发重定位）；每帧特征数（骤降通常意味着弱纹理区或失焦）；运动估计幅度（突然跳变多半是误匹配，物理上机器人不可能一帧横移一米）；关键帧间隔（过密说明视差统计异常，过疏说明跟踪太紧）。这四个数字比任何日志都早暴露问题——后端报错时，病根往往在几分钟前的前端面板上已经露头。调参也是围绕这块面板展开：内点率低先查曝光与对焦，运动跳变先查 RANSAC 阈值，而不是急着动优化器参数。

本节要点回顾

• 前端唯一职责：相邻帧运动估计，健康指标是跟踪内点率；
• 两把钥匙：对极几何点火初始化、PnP 持续跟踪；
• 特征点法稳、直接法快，光照与算力是选型分水岭；
• 关键帧是信息密度与算力预算的折中；
• 纯旋转陷阱与尺度歧义是单目视觉两大先天约束，后者待 5.4 节 IMU 锚定。

视觉之外还有一支不畏黑夜的兵种。下一节激光 SLAM 与占据栅格地图。