基于 Peter Corke《Robotics, Vision & Control》（原第 13–16 章合并）上一篇：图像处理与特征提取配套代码：chap13.ipynb · chap14.ipynb · chap15.ipynb · chap16.ipynb本文示例环境：Python 3.12 + opencv-python + numpy + matplotlib + spatialmath-python阅读对象：零基础新手
摘要： 本文对应书中 第 13 章（相机）→ 第 14 章（多视图与立体）→ 第 15–16 章（视觉伺服），按书上的逻辑顺序展开，对 每个关键词 给出定义、公式、直觉与代码。读完应能回答：
- • 3D 点
(X,Y,Z) 如何变成像素 (u,v)？K、外参、C 各是什么？ - • 两幅图如何约束匹配？F、极线、E 是什么关系？
- • 双目如何得深度？视差、三角化、重投影误差 怎么用？
- • 如何用图像误差驱动机器人？L、IBVS、PBVS 控制律怎么写？
零、读本文之前
上一篇 在 2D 图像里：滤波、分割、提 ORB 特征点 + 描述子。
本篇 回答三个层次的问题：
Chap 11 特征点 p = (u,v)，描述子用于匹配Chap 12 匹配满足 p2ᵀFp1=0 → 三角化得 Z → L·v 控制相机
与全系列衔接：
- • 第 6 篇 SLAM： EKF 融合 地标观测；视觉 SLAM 的地标就是特征点 + 本篇三角化。
- • 第 8 篇雅可比： 机械臂
ẋ = J q̇；本篇 图像 Jacobianṗ = L v_cam。 - • 第 4 篇移动机器人： go-to-point 用 位姿误差；IBVS 用 像素误差，输出再映射为
(v, ω)。
关键词速查（后文会逐一展开）
| |
|---|
| 针孔模型 | |
| 内参 K | |
| 外参 | |
| C | |
| 齐次坐标 | |
| 单应 H | |
| 畸变 | 真实镜头偏离针孔，用 k1,k2,p1,p2 校正 |
| 标定 | |
| solvePnP | |
| F | |
| E | |
| 极线 | |
| 视差 d | |
| 三角化 | |
| L | |
| IBVS | |
| PBVS | |
第一部分：相机模型（对应书中第 13 章）
一、为什么需要「相机模型」？
机器人里的 相机 不是「拍一张照片」这么简单。只要你想做下面任一件事，就必须知道 3D ↔ 2D 的精确数学关系：
相机模型 就是这套数学：给定 3D 点，预测它在哪像素；或给定像素 + 模型，反推 3D 射线。
书中默认 中心透视（central perspective） 模型，即 针孔相机 + 可选 透镜畸变 修正。
二、针孔模型与透视投影
坐标系约定（必读）
相机坐标系（本书/MVT 常用）：
- • Z 轴：沿光轴指向场景（深度 Z > 0 才可见）
- • Y 轴：通常指向图像 下（注意：与有些数学系「Y 向上」不同）
场景点 P = (X, Y, Z) 在相机系下，投影到 图像平面（距光心焦距 f 处）：
u = f · X/Z + u0v = f · Y/Z + v0
透视除法 Z：同一 3D 线段，离相机越远，成像越 小——这就是「近大远小」的数学来源。
图1 针孔模型示意
图中：光心、3D 点、成像平面；虚线为光线。点在相机 后方（Z≤0）不会出现在图像上。
from blog.vision_utils import PinholeCameracam = PinholeCamera(f=600, pp=(320, 240), imagesize=(640, 480))p = cam.project([0.3, 0.4, 2.0]) # 3D → 2Dprint(p) # [[u], [v]] 像素坐标
运行结果直觉： X、Y 越大，u、v 偏离主点越远；Z 越大，同样 X 造成的 u 偏移越小。
三、齐次坐标（后面所有公式的基础）
问题： 透视投影里有 除法X/Z，写矩阵不方便。
齐次坐标： 用多一维表示同一几何对象，把除法变成乘法。
投影写成：
λ · [u, v, 1]ᵀ = C · [X, Y, Z, 1]ᵀ
新手要点： 看到 ~ 或 [u,v,1]ᵀ 时，意思是 差一个尺度因子相等，不是每个分量逐元素相等。
四、内参矩阵 K
内参（intrinsic） 只描述 相机本身（镜头 + 传感器），与相机在世界中放哪 无关。
K = [ fx 0 u0 ] [ 0 fy v0 ] [ 0 0 1 ]
图2 内参 K 与视场角 FOV
| | |
|---|
| fx, fy | | 与物理焦距 f_m、像素尺寸 ρ 有关：fx ≈ f_m / ρ_x |
| u0, v0 | 主点 | |
| ρ | | |
| FOV | | 水平 FOV ≈ 2·arctan(width/(2fx)) |
为什么 fx ≠ fy 有时成立？ 像素非正方形，或数字处理时 x/y 缩放不同。
视场 FOV 直觉： fx 越小（广角镜头），同样传感器看到的 世界范围越大。
五、外参：相机在世界中的位姿
外参（extrinsic） 描述 相机坐标系 ↔ 世界坐标系 的刚体变换，用 SE(3) 表示（第 7 篇学过）：
点在世界系 P_w，先进相机系：
P_c = R · P_w + t （或齐次：P_c ~ T_cw · P_w）
物体位姿 objpose： 若点定义在 物体坐标系，还要乘 T_world_object。
图3 立方体与网格的投影
同一组 3D 点，改变 cam.pose（相机在世界中移动），图像上投影 整体平移、缩放、透视变形——视觉伺服、VO 都利用这一性质。
相机矩阵合并形式：
C = K · [R | t] （3×4）λ p = C P_w
书中 CentralCamera.C() 即此矩阵；project_point(P, objpose=...) 内部完成坐标链。
六、平面单应 Homography
什么时候可以用 H？
当 所有 3D 点落在同一平面（如 Z=0 地面、墙面海报、棋盘格），3D 只剩 2 个自由度 (X,Y)，投影变成 3×3 单应：
p ~ H · [X, Y, 1]ᵀ
H 是 3×3，秩 3（一般可逆）。平面 → 平面 的映射。
图4 平面四角：投影 vs H 计算
注意： 3D 点 不在 该平面上时，不满足 H，用 H 拟合会得到 外点（第 14 节 RANSAC）。
七、透镜畸变
理想针孔：直线物成像为直线。真实镜头 尤其广角，有 径向畸变（桶形/枕形）和 切向畸变（镜片与传感器不完全平行）。
归一化图像坐标（去 K 影响）：
u_n = (u - u0) / fx , v_n = (v - v0) / fyr² = u_n² + v_n²
径向（Brown-Conrady 模型）：
δu_r = u_n · (k1·r² + k2·r⁴ + k3·r⁶)δv_r = v_n · (k1·r² + k2·r⁴ + k3·r⁶)
切向： 用 p1, p2 修正。
图5 径向畸变校正向量场
标定 输出 [k1,k2,p1,p2,k3]；OpenCV undistort 把弯曲的像素映射回针孔模型。不校正 则直线检测、F/H 估计、PnP 都会系统偏差。
八、相机标定（棋盘格）
目标： 已知 3D 的标定板角点（例如 z=0 平面网格，间距 25 mm），拍多张不同姿态照片，联合估计 K、畸变、每张图的外参。
流程（与书中 images2C 一致）：
- 3. 最小化 重投影误差 Σ‖p_obs − π(K,R,t,P)‖²
- 4. 得到 K、distortion、每帧 pose
重投影误差 是标定质量核心指标：平均 < 0.5 像素通常较好。
九、位姿估计 solvePnP
问题： 已知 N 组 3D 点 P_i（物体或世界系）与像素 p_i，求 T_cam_obj（6 DoF）。
最少 4 点（PnP）；点多更鲁棒。OpenCV：
ok, rvec, tvec = cv2.solvePnP(P_3d, p_2d, K, distCoeffs)# rvec,tvec: 物体→相机 的 Rodrigues 旋转 + 平移
图6 solvePnP：观测 vs 重投影
- • 绿叉： 用估计位姿把 3D 点 再投影 回图像
用途： PBVS 第一步、AR、手眼标定初值。PBVS 需要 物体 3D 模型尺寸 正确，否则尺度错。
第二部分：多视图几何与立体（对应书中第 14 章）
十、单目为什么不能直接测「米」
一张图只有 (u,v)，丢失 深度 Z。沿 同一条射线 上所有 3D 点投影到 同一像素——这就是 尺度模糊。
要得 metric 深度，至少需要之一：
十一、两视图特征匹配（复习 + 几何用途）
上一篇用 ORB 得匹配对 (p1, p2)。几何阶段要判断：哪些匹配 符合同一 3D 点，哪些是 误匹配。
orb = cv2.ORB_create(500)k1, d1 = orb.detectAndCompute(img1, None)k2, d2 = orb.detectAndCompute(img2, None)bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)matches = sorted(bf.match(d1, d2), key=lambda m: m.distance)
图7 两视图 ORB 匹配
提高匹配质量（书中强调）：
- 1. Ratio test（Lowe）： 最近邻距离 / 次近邻距离 < 阈值
十二、对极几何与基础矩阵 F
几何构造
两相机光心 O1、O2，空间点 P。三点定平面 → 对极平面。
基线： O1 到 O2 的向量 b。
对极线（epipolar line）： 左图固定 p1，右图所有可能 p2 落在 一条线 上（右图同理）。
代数形式
齐次像素 p1, p2（3×1）：
p2ᵀ · F · p1 = 0
F： 3×3 基础矩阵（fundamental matrix）
图8 极线约束
直觉： 右图搜索对应点从 整幅图 2D 降为 极线上 1D，立体匹配计算量大幅下降。
本质矩阵 E
若两相机 内参 K 已知，转到 归一化相机坐标：
E = [t]× · Rp̂2ᵀ E p̂1 = 0 , F = K2⁻ᵀ E K1⁻¹
- • [t]× 是 t 的反对称矩阵（叉乘写成矩阵乘）
E 有 5 个自由度；分解 E 得 4 组 (R,t) 解，需 ** cheirality 检测**（点在两个相机前 Z>0）选唯一。
十三、RANSAC 估计 F
问题： 匹配中 30%–50% 误匹配很常见；直接最小二乘拟合 F 会被 外点 拉飞。
RANSAC（Random Sample Consensus）步骤：
- 1. 随机抽 最小集（F 至少 7 对点，常用 8 点算法）
- 3. 数 内点：满足
|p2ᵀ F p1| < ε 的对数
图9 F 矩阵 RANSAC：绿=内点，红=外点
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC, 1.0, 0.99)
ε = 1.0 像素 表示允许 1 像素几何误差；置信度 0.99 控制迭代次数。
十四、平面单应 H 与立体视差
单应再访
平面场景，两视图关系用 H（3×3）而非 F。RANSAC 估计 H 时，非平面点 成为外点——可用来 检测场景是否平面（书中 walls 例子）。
图10 单应 RANSAC 内点
双目与视差
Setup： 两相机 平行（极线水平），基线长度 B（米），焦距 f（像素）。
左图像素 (u, v) 在右图对应点 (u - d, v)，d ≥ 0 为 视差（disparity）：
Z = f · B / d
图11 合成立体对 + 视差图
stereo = cv2.StereoBM_create(numDisparities=64, blockSize=15)disp = stereo.compute(left, right)
StereoBM 在做什么（简化）：
- 2. 比较 patch 相似度（SAD 等），找 最佳视差 d
失败模式（书中 14.4.2 详述）：
十五、三角化
已知 投影矩阵 P1, P2（3×4，含 K 与外参）与匹配像素 p1, p2，求 3D 点 X。
线性三角化： 每个像素提供 2 个方程，4 个未知数 (X,Y,Z,λ) → 4×4 齐次方程组，SVD 求最小特征向量。
X_h = cv2.triangulatePoints(P1, P2, p1, p2)X = (X_h[:3] / X_h[3]).reshape(3, -1).T
图12 三角化 3D 点与误差分布
重投影误差： 把 X 再投回两图，与 p1、p2 比较——评估三角化质量。
Bundle Adjustment（BA）： 非线性优化 同时 refine 所有相机位姿与 3D 点，最小化全局重投影误差——SfM/SLAM 后端常用。
十六、透视校正
四边形（斜拍矩形）→ 矩形：H 由四角对应求 findHomography，再 warpPerspective。
图13 梯形立面拉正
与 文档扫描、立面测量 相同。注意：仅当目标 共面 时几何严格。
第三部分：视觉伺服（对应书中第 15–16 章）
十七、视觉伺服问题是什么？
设定： 机器人（臂或底盘）带相机，场景中有 特征（角点、网格点等）。
目标：
- • PBVS： 相机相对物体位姿 T 接近期望 T_d
与 Chap 4 go-to-point 对比：
十八、相机运动与 twist
相机速度 用 6 维 twist（与第 3、8 篇一致）：
v_cam = [vx, vy, vz, ωx, ωy, ωz]ᵀ
小位移：T ← T · exp(v_cam · Δt)，MVT 中 SE3.Delta(v)。
十九、图像 Jacobian L
定义： 特征像素 p=(u,v) 对相机 twist 的导数：
ṗ = L · v_cam
对 一个点，L 是 2×6。N 个点 堆叠 成 2N×6（书中 flatten(order='F') 按列堆误差）。
归一化坐标x=(u-u0)/fx, y=(v-v0)/fy，深度 Z（相机系下特征距离）：
L = [ -1/Z 0 x/Z x·y -(1+x²) y ] [ 0 -1/Z y/Z 1+y² -x·y -x ]
图14 不同相机运动的像素流
L = cam.visjac_p(p, depth=Z)
关键：L 含 1/Z。 Z 估错 → 控制增益全错 → IBVS 发散或极慢（第 24 节）。
二十、IBVS 控制律（逐步）
Step 1 选 N 个特征，测当前 p（2N×1）
Step 2 期望 p_d（示教或任务定义）
Step 3 误差 e = p_d − p
Step 4 算 L(p, Z)（Z 用初值、测距或在线估计）
Step 5 控制（ resolved-rate 形式）：
v_cam = λ · pinv(L) · e
λ 为 增益（0.01–0.2 常见），pinv 因 L 非方阵。
Step 6 更新位姿：T ← T · exp(v_cam · dt)
Step 7 重复直到 ‖e‖ < 阈值
图15 IBVS 误差随迭代下降
优点： 不需 3D 模型。缺点： 深度敏感、局部极小（e→0 但 T 不对）、奇异位形（L 秩亏）。
二十一、PBVS 控制律（逐步）
Step 1 用 solvePnP（或 estpose）从 3D–2D 得 T_est（物体相对相机）
Step 2 给定期望 T_des（任务：例如物体在相机前 1 m）
Step 3 误差 T_err = T_est · T_des⁻¹（SE(3) 上）
Step 4 在 se(3) 上取 twist（平移 + 轴角），按增益驱动相机
Step 5 插值 T ← T · exp(λ · twist · dt)
图16 PBVS 位姿误差各分量
优点： 3D 轨迹可规划、可预测。缺点： 需要 准确 3D 模型；PnP 抖动会进控制环。
二十二、深度 Z 与相机轨迹
图17 假设深度 vs 最终像素误差
若 IBVS 里 所有特征深度都猜成 2 m，真实 0.5 m，则 L 中 1/Z 项 小 4 倍，控制 欠增益，收敛慢或停在不零误差。
对策：
- •
depthest=True（书中 IBVS 类在线更新 Z）
图18 IBVS 相机 3D 轨迹
IBVS 不保证相机走直线，但 像素误差 应单调降（理想情况）。
二十三、进阶（对应第 16 章）
极坐标 IBVS
(u,v) 相对主点在大角度 绕光轴旋转 时，L 病态（条件数大）。改用 (θ, ρ)：
θ = atan2(v - v0, u - u0)ρ = sqrt((u-u0)² + (v-v0)²)
图19 笛卡尔 vs 极坐标特征
分区 IBVS
思想： 先只控 平移（屏蔽 L 的旋转列 / 误差的高维部分），再开 全 6 DoF，避免一开始就大旋转。
图20 分区控制：XY 相 vs 总误差
移动机器人
相机固连在底盘；IBVS 输出 v_cam 需映射到 (v, ω) 或全向 (vx, vy, ω)，并满足 非完整约束（Chap 4 Bicycle）。
图21 移动平台 + 相机 + 目标
球面相机
光线从 单中心 发出，360° FOV；投影与 L 公式不同（IBVS_sph）。
图22 球面成像模型
与机械臂
眼在手外 / 眼在手上： IBVS 得 v_cam，经 末端雅可比J_e：
q̇ = J_e⁺ · v_ee
与 Chap 8 RRMC 同一链条，只是 任务空间 从 Cartesian 换成 图像特征。
二十四、常见问题
Q：F 和 E 什么时候用？A：像素坐标、K 未知或不信任 → F；两相机已标定 → E，再得 R,t。
Q：视差图单位？A：OpenCV StereoBM 输出 often 固定点（×16），要 ÷16 得像素视差。
Q：IBVS 深度必须精确吗？A：不必毫米级，但 数量级要对；错 4 倍就很麻烦。
Q：和 MVT 关系？A：公式一致；MVT 的 CentralCamera、IBVS 类封装了全文流程，本篇 vision_utils 便于无 MVT 环境学习。
Q：还要读什么？A：书中 13.3 鱼眼/全景、14.8 拼接/VO、15.3 线/椭圆特征 为扩展；本文覆盖主干。
二十五、小结
视觉线：10 光/颜色 → 11 图像/特征 → 12 几何/立体/VS（本篇）全书：移动 3–6 | 臂 7–9 | 视觉 10–12
附录 A：配图清单
附录 B：符号表
