视觉SLAM前置知识：相机模型

相机模型详解：从世界坐标到图像坐标的完整推导

视觉 SLAM 的核心之一就是重投影误差（Reprojection Error） 。要彻底理解它，首先必须理解相机模型（Camera Model） 。本篇文章从坐标系出发，详细讲解相机的几何关系、内外参数、投影模型、畸变、以及重投影误差的数学来源。

一、相机模型的核心思想

相机的作用：

把三维世界中的一个点 P(Xw,Yw,Zw)P(X_w, Y_w, Z_w)P(Xw,Yw,Zw) 投影成二维图像平面上的一个像素点 p(u,v)p(u, v)p(u,v)。

这个过程主要分为两步：

外参（Extrinsic Parameters） ：描述世界坐标系 → 相机坐标系的变换；
内参（Intrinsic Parameters） ：描述相机坐标系 → 图像像素坐标系的变换。

二、坐标系的层次关系

相机模型中有三个主要坐标系：

世界坐标系（World Coordinate System）

表示现实世界中点的真实位置。

一个点可表示为：
Pw= $XwYwZw1$ P_w = \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} Pw= XwYwZw1
相机坐标系（Camera Coordinate System）

原点在相机光心，Z 轴指向成像方向（在视觉 SLAM、OpenCV、ORB-SLAM 等主流实现中，Z 轴是从相机出发，指向前方的世界）。

点在相机坐标系下的表示为：
Pc= $XcYcZc$ P_c = \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} Pc= XcYcZc
图像坐标系（Image Coordinate System）

原点位于图像平面中心，Z 轴垂直于成像平面。

对应的像素坐标为 p(u,v)p(u, v)p(u,v)。

三、外参：世界坐标到相机坐标

世界坐标到相机坐标的转换由 旋转矩阵 ( R ) 和 平移向量 ( t ) 决定：

Pc=RPw+t P_c = R P_w + t Pc=RPw+t

用齐次坐标表示为：

Pc1\]=\[Rt01\]\[Pw1\]\\begin{bmatrix} P_c \\\\ 1 \\end{bmatrix} =\\begin{bmatrix} R \& t \\\\ 0 \& 1 \\end{bmatrix} \\begin{bmatrix} P_w \\\\ 1 \\end{bmatrix} \[Pc1\]=\[R0t1\]\[Pw1

这部分称为 外参矩阵，代表相机在世界中的位姿。

四、透视投影模型（Pinhole Model）

在相机坐标系下，点 Pc(Xc,Yc,Zc)P_c(X_c, Y_c, Z_c)Pc(Xc,Yc,Zc) 投影到成像平面上：

x=XcZc,y=YcZc x = \frac{X_c}{Z_c}, \quad y = \frac{Y_c}{Z_c} x=ZcXc,y=ZcYc

这里的 (x,y)(x, y)(x,y) 是归一化平面坐标（焦距 = 1）。

五、内参：相机坐标到像素坐标

相机的内参矩阵KKK 定义了从归一化平面坐标到图像像素坐标的变换：

K= $fx0cx0fycy001$ K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1

其中：

fx=f/dxf_x = f / d_xfx=f/dx：焦距与像素比例（x方向）
fy=f/dyf_y = f / d_yfy=f/dy：焦距与像素比例（y方向）
(cx,cy)(c_x, c_y)(cx,cy)：主点（principal point）坐标（相机的光轴（即相机坐标系的 Z 轴）与图像平面（成像平面）相交的那个点）

将其应用到归一化坐标上：

$uv1$ =K $xy1$ = $fxx+cxfyy+cy1$ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= K \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}= \begin{bmatrix} f_x x + c_x \\ f_y y + c_y \\ 1 \end{bmatrix} uv1 =K xy1 = fxx+cxfyy+cy1

六、整体投影方程

综合外参与内参，完整的相机投影模型为：

s $uv1$ =K $R∣t$ $XwYwZw1$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= K $R\|t$ \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} s uv1 =K $R∣t$ XwYwZw1

其中 s=Zcs = Z_cs=Zc是尺度因子。

七、畸变模型（Distortion Model）

真实镜头会引入畸变，一般分为两种：

径向畸变 （Radial）：
xdistorted=x(1+k1r2+k2r4+k3r6) x_{distorted} = x(1 + k_1r^2 + k_2r^4 + k_3r^6) xdistorted=x(1+k1r2+k2r4+k3r6)
ydistorted=y(1+k1r2+k2r4+k3r6) y_{distorted} = y(1 + k_1r^2 + k_2r^4 + k_3r^6) ydistorted=y(1+k1r2+k2r4+k3r6)

其中 (r^2 = x^2 + y^2)。
切向畸变 （Tangential）：
x′=x+2p1xy+p2(r2+2x2) x' = x + 2p_1xy + p_2(r^2 + 2x^2) x′=x+2p1xy+p2(r2+2x2)
y′=y+p1(r2+2y2)+2p2xy y' = y + p_1(r^2 + 2y^2) + 2p_2xy y′=y+p1(r2+2y2)+2p2xy

最终像素坐标由畸变修正后的坐标计算得来。

八、重投影误差（Reprojection Error）

现在我们就能定义视觉 SLAM 优化的核心目标函数------重投影误差。

设：

实际观测到的像素坐标为 xi,kobs\mathbf{x}^{obs}_{i,k}xi,kobs
根据当前估计位姿 Tk= $Rk∣tk$ T_k = $R_k \| t_k$ Tk= $Rk∣tk$ 和地图点 XiX_iXi 投影得到的预测像素为：

xi,kproj=π(K $Rk∣tk$ Xi) \mathbf{x}^{proj}_{i,k} = \pi \big( K $R_k\|t_k$ X_i \big) xi,kproj=π(K $Rk∣tk$ Xi)

那么重投影误差就是两者的差：

ei,k=xi,kobs−xi,kproj \mathbf{e}{i,k} = \mathbf{x}^{obs}{i,k} - \mathbf{x}^{proj}_{i,k} ei,k=xi,kobs−xi,kproj

视觉 SLAM 的优化目标，就是最小化所有观测点的重投影误差平方和：

min⁡{Rk,tk,Xi}∑i,kρ ⁣(∥ei,k∥2) \min_{\{R_k, t_k, X_i\}} \sum_{i,k} \rho \!\left( \| \mathbf{e}_{i,k} \|^2 \right) {Rk,tk,Xi}mini,k∑ρ(∥ei,k∥2)

其中：

ρ(⋅)\rho(\cdot)ρ(⋅)：鲁棒核函数（Huber、Cauchy等）
优化变量：相机姿态 Rk,tkR_k, t_kRk,tk 和地图点 XiX_iXi

在优化过程中，每一个匹配点都提供一个几何约束：真实图像中的点，预测投影点，之间的偏差越小，说明相机的姿态与地图的几何关系越正确。重投影误差直接反映了整个系统的几何一致性。

延伸：如何使用高斯-牛顿法求解重投影误差

在视觉 SLAM 中，我们的目标是让所有投影到图像上的特征点都尽可能接近它们的真实观测位置。也就是说，我们要求解一个非线性最小二乘问题：

min⁡{Rk,tk,Xi}∑i,kρ ⁣(∥ei,k∥2) \min_{\{R_k, t_k, X_i\}} \sum_{i,k} \rho\!\left(\|\mathbf{e}_{i,k}\|^2\right) {Rk,tk,Xi}mini,k∑ρ(∥ei,k∥2)

其中，重投影误差定义为：
ei,k=xi,kobs−π(K $Rk∣tk$ Xi) \mathbf{e}{i,k} = \mathbf{x}^{obs}{i,k} - \pi(K $R_k\|t_k$ X_i) ei,k=xi,kobs−π(K $Rk∣tk$ Xi)
π(⋅)\pi(\cdot)π(⋅) 是投影函数（包含除以 ZcZ_cZc）。

重投影误差是非线性的，因为投影涉及：

除以 (Z_c)（非线性）；
旋转矩阵 (R)（在 SO(3) 上，非线性）。

因此，我们无法直接解析求解，只能线性化近似并迭代优化。高斯--牛顿法正是处理这类"非线性最小二乘"问题的经典方法。

假设我们有 (N) 个残差：
E=12∑i=1N∥ei(x)∥2 E = \frac{1}{2} \sum_{i=1}^{N} \|\mathbf{e}_i(\mathbf{x})\|^2 E=21i=1∑N∥ei(x)∥2

其中 x\mathbf{x}x 是待优化变量（包括相机姿态、地图点等）。我们希望找到 x\mathbf{x}x 使得 EEE最小。

我们在当前估计 x\mathbf{x}x附近做一阶泰勒展开：

e(x+Δx)≈e(x)+JΔx \mathbf{e}(\mathbf{x} + \Delta \mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J \Delta \mathbf{x} e(x+Δx)≈e(x)+JΔx

其中 J=∂e∂xJ = \frac{\partial \mathbf{e}}{\partial \mathbf{x}}J=∂x∂e是雅可比矩阵。

将线性化残差代入目标函数：

E(x+Δx)≈12∥e+JΔx∥2 E(\mathbf{x} + \Delta \mathbf{x}) \approx \frac{1}{2} \|\mathbf{e} + J \Delta \mathbf{x}\|^2 E(x+Δx)≈21∥e+JΔx∥2

对 Δx\Delta \mathbf{x}Δx求导并令导数为 0，可得正规方程：

(J⊤J) Δx=−J⊤e (J^\top J)\, \Delta \mathbf{x} = -J^\top \mathbf{e} (J⊤J)Δx=−J⊤e

这是高斯--牛顿方程 。

我们可以求出：
Δx=−(J⊤J)−1J⊤e \Delta \mathbf{x} = -(J^\top J)^{-1} J^\top \mathbf{e} Δx=−(J⊤J)−1J⊤e

然后更新参数：
x←x⊞Δx \mathbf{x} \leftarrow \mathbf{x} \boxplus \Delta \mathbf{x} x←x⊞Δx

其中 ⊞\boxplus⊞表示在李群上的更新操作（例如姿态在 SO(3) 上指数映射更新）。

当优化相机位姿 T= $R∣t$ T = $R\|t$ T= $R∣t$ 时，

我们在线性化时只考虑它的小扰动 δξ∈se(3)\delta \boldsymbol{\xi} \in \mathfrak{se}(3)δξ∈se(3)，

对应一个 6 维向量（3 平移 + 3 旋转）：

T′=exp⁡(δξ∧)T T' = \exp(\delta \boldsymbol{\xi}^\wedge) T T′=exp(δξ∧)T

代入重投影函数：
ei=xiobs−π(KTXi) \mathbf{e}{i} = \mathbf{x}^{obs}{i} - \pi(K T X_i) ei=xiobs−π(KTXi)

线性化后：
ei(T′)≈ei(T)+Ji δξ \mathbf{e}_i(T') \approx \mathbf{e}_i(T) + J_i \, \delta \boldsymbol{\xi} ei(T′)≈ei(T)+Jiδξ

其中 Ji=∂ei∂δξJ_i = \frac{\partial \mathbf{e}_i}{\partial \delta \boldsymbol{\xi}}Ji=∂δξ∂ei是单个特征点关于位姿的雅可比矩阵。

对一个点 Pc=(Xc,Yc,Zc)P_c = (X_c, Y_c, Z_c)Pc=(Xc,Yc,Zc)，其投影为：
{u=fxXcZc+cxv=fyYcZc+cy \begin{cases} u = f_x \frac{X_c}{Z_c} + c_x \\ v = f_y \frac{Y_c}{Z_c} + c_y \end{cases} {u=fxZcXc+cxv=fyZcYc+cy

对相机坐标 $P_c$ 的偏导为：

∂(u,v)∂(Xc,Yc,Zc)= $fxZc0-fxXcZc20fyZc-fyYcZc2$ \frac{\partial (u,v)}{\partial (X_c, Y_c, Z_c)} = \begin{bmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{bmatrix} ∂(Xc,Yc,Zc)∂(u,v)= $Zcfx00Zcfy-Zc2fxXc-Zc2fyYc$

然后再乘以 PcP_cPc关于姿态扰动δξ\delta \boldsymbol{\xi}δξ 的偏导：
∂Pc∂δξ= $I−\[Pc$ ×] \frac{\partial P_c}{\partial \delta \boldsymbol{\xi}} = \begin{bmatrix} I & - $P_c$ _\times \end{bmatrix} ∂δξ∂Pc= $I−\[Pc$ ×]

综合得：
Ji=∂ei∂δξ=−∂(u,v)∂(Xc,Yc,Zc)⋅∂Pc∂δξ J_i = \frac{\partial \mathbf{e}_i}{\partial \delta \boldsymbol{\xi}} = - \frac{\partial (u,v)}{\partial (X_c, Y_c, Z_c)} \cdot \frac{\partial P_c}{\partial \delta \boldsymbol{\xi}} Ji=∂δξ∂ei=−∂(Xc,Yc,Zc)∂(u,v)⋅∂δξ∂Pc

这个 JiJ_iJi 就是一个 2×62\times62×6的雅可比矩阵。

高斯--牛顿法适用于误差较小、收敛性较好的情况。

如果误差较大或非线性较强，常使用 Levenberg--Marquardt（LM）法：

(H+λI)Δx=b (H + \lambda I) \Delta \mathbf{x} = b (H+λI)Δx=b

其中 λ\lambdaλ为阻尼因子，用来平衡收敛速度与稳定性。 ORB-SLAM 和 g2o 都在实际中使用了 LM 的形式。