第4章：视觉传感器原理与建模
视觉传感器，说白了就是机器人的眼睛。但跟人眼不一样，它看到的是一堆像素点。怎么从这些像素里还原出三维世界？这就是本章要聊的事。
我最早接触视觉SLAM时，觉得相机模型就是个简单的针孔成像。后来在实际项目中才发现，这里面的坑比想象中多得多。嗯，咱们一步步来。
4.1 单目相机模型
单目相机是最常见的。你手机上的摄像头就是单目。它的核心是针孔模型——光线通过一个小孔，在成像平面上形成倒像。
数学上，一个三维点 P(X, Y, Z) 投影到像素坐标 (u, v) 的过程可以写成：
u = fx * X / Z + cx
v = fy * Y / Z + cy这里的 fx, fy 是焦距（以像素为单位），cx, cy 是光心偏移。这4个参数就是相机内参。
关键点：单目相机丢失了深度信息。一个点在图像上的位置，只能告诉你它在哪条射线上，但不知道距离。这就是所谓的「尺度不确定性」。
我在做AR项目时遇到过一个问题：用单目相机估计物体位置，结果物体忽大忽小。后来才发现是尺度没固定。你想想看，单目SLAM如果没有其他传感器辅助，地图的尺度就是模糊的。
4.2 双目相机模型
双目相机模拟人的双眼。两个相机之间有个固定的基线距离 B。通过左右图像的视差 d，就能算出深度：
Z = B * f / d其中 f 是焦距，d = u_left - u_right 是视差。
这里有个实际问题：视差 d 是整数像素，但真实视差是连续的。所以我们需要亚像素插值。我习惯用抛物线拟合来得到亚像素精度。
我的经验：双目相机的基线越长，深度测量精度越高，但视差搜索范围也越大。一般室内用5-10cm基线，室外可以用20-30cm。我曾经在无人机上用过15cm基线，效果不错。
4.3 深度相机（RGB-D）
深度相机直接输出每个像素的深度值。主流技术有两种：
- 结构光：投射红外点阵，通过变形计算深度。代表：Kinect v1、Intel RealSense
- ToF（飞行时间）：发射红外脉冲，测量往返时间。代表：Kinect v2、Azure Kinect
深度相机的优点是直接得到深度，省去了立体匹配的计算。但缺点也很明显：
- 容易受环境光干扰（尤其是室外）
- 测量范围有限（一般0.5-5米）
- 边缘处有空洞
避坑指南：我曾经在室外用Kinect做SLAM，结果深度数据全是噪声。后来换成双目+IMU才搞定。深度相机在室内弱光环境下表现最好，别指望它在阳光下正常工作。
4.4 内参标定
内参标定就是求 fx, fy, cx, cy 以及畸变参数。最经典的方法是张正友标定法——用棋盘格拍多张照片，然后优化求解。
标定流程：
- 打印棋盘格，贴在平面上
- 从不同角度拍10-20张照片
- 检测角点
- 用最小二乘法求解内参
- 优化畸变参数
我用OpenCV做过很多次标定，代码大致是这样：
import cv2
import numpy as np
# 棋盘格尺寸
pattern_size = (9, 6)
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)
objpoints = [] # 世界坐标
imgpoints = [] # 图像坐标
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
if ret:
objpoints.append(objp)
imgpoints.append(corners)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None)我的习惯：标定时让棋盘格占图像面积的1/3到1/2，角度变化要覆盖各个方向。我一般拍15张，重投影误差控制在0.3像素以内才算合格。
4.5 外参标定
外参描述相机在机器人坐标系中的位置和姿态。说白了就是知道相机装在哪、朝哪看。
外参包含旋转矩阵 R 和平移向量 t。标定方法通常是用已知尺寸的标定物，或者用手眼标定（Eye-in-Hand / Eye-to-Hand）。
手眼标定的核心方程是：
AX = XB其中 A 是机械臂末端位姿变化，B 是相机观测到的标定板位姿变化，X 就是待求的手眼矩阵。
实际案例：我在做机械臂抓取项目时，外参标定花了整整两天。原因是标定板没固定好，每次移动都有微小偏移。后来用磁吸座固定标定板，一次就成功了。
4.6 特征点提取与匹配
特征点是图像中具有独特性的点。SLAM里最常用的就是ORB和SIFT。
ORB（Oriented FAST and Rotated BRIEF）
ORB是OpenCV的标配。它结合了FAST角点检测和BRIEF描述子，还加了旋转不变性。优点是速度快，适合实时SLAM。
ORB的提取步骤：
- 构建图像金字塔（尺度不变性）
- 用FAST检测角点
- 计算灰度质心方向（旋转不变性）
- 用rBRIEF生成二进制描述子
SIFT（Scale-Invariant Feature Transform）
SIFT是经典中的经典。它用DoG（高斯差分）检测极值点，生成128维浮点描述子。精度高，但速度慢，而且有专利限制。
我个人的建议：
- 实时SLAM用ORB，速度快，够用
- 三维重建或视觉定位用SIFT，精度更高
- 如果做深度学习，可以用SuperPoint或D2-Net
特征匹配
匹配就是找两幅图像中对应的特征点。最常用的是暴力匹配（Brute-Force）和FLANN匹配。
匹配后一定要做筛选：
- 比值测试：最近距离 / 次近距离 < 0.7
- RANSAC：用单应矩阵或基础矩阵剔除误匹配
# ORB特征匹配示例
orb = cv2.ORB_create()
kp1, des1 = orb.detectAndCompute(img1, None)
kp2, des2 = orb.detectAndCompute(img2, None)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
matches = bf.knnMatch(des1, des2, k=2)
# 比值测试
good_matches = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good_matches.append(m)避坑指南：我曾经在纹理稀疏的环境（比如白墙）做ORB匹配，结果匹配点全错了。后来加了光照补偿和自适应阈值才改善。记住，特征点提取的前提是图像有足够的纹理信息。
4.7 知识体系总览
下面这张图总结了本章的核心逻辑。从传感器原理到特征匹配，每一步都是后续SLAM和轨迹优化的基础。
嗯，以上就是视觉传感器原理与建模的核心内容。从相机模型到标定，再到特征提取与匹配，每一步都有它的物理意义和工程技巧。你在实际项目中遇到问题时，可以回头看看这张图，理清思路。
可可精品资料库