相机模型详解：从世界坐标到图像坐标的完整推导
视觉 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） 真实镜头会引入畸变，一般分为两种： 1. **径向畸变** （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)。 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 的形式。