视觉三角化原理

原创已于 2026-07-05 14:46:42 修改 · 289 阅读

3 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

于 2026-07-05 14:46:08 首次发布

一、核心问题

已知：两帧匹配像素、相机内参、两帧相机位姿（外参）

求解：匹配像素对应的三维空间坐标 (P)

难点：针孔投影方程含未知深度尺度 (s)，无法直接线性求解三维点。

二、符号定义

世界坐标系下三维点：
$P = [X,Y,Z]^T$

两帧相机外参：
$T_1(R_1,t_1),\; T_2(R_2,t_2)$

像素匹配点齐次坐标：
$p_1,\; p_2$

归一化平面坐标（去内参）：
$x1=K−1p1,x2=K−1p2,x=[x,y,1]Tx_1=K^{-1}p_1,\quad x_2=K^{-1}p_2,\quad x=[x,y,1]^T$

三、针孔投影方程（带尺度/深度）

相机坐标系下空间点 = 深度 × 归一化方向射线：
${s1x1=R1T(P−t1)s2x2=R2T(P−t2)\begin{cases} s_1 x_1 = R_1^T(P - t_1) \\ s_2 x_2 = R_2^T(P - t_2) \end{cases}$

关键理解：
$x_1,x_2$ ：仅代表射线方向，无深度远近信息
$s_1,s_2$ ：尺度因子 = 相机深度（未知额外变量）

方程组包含 (P、s_1、s_2) 共5个未知量，无法直接求解，需要通过数学方法消去尺度变量。

四、核心操作：叉乘消尺度（三角化精髓）

叉乘核心意义与目的

视觉三角化中，叉乘（反对称矩阵运算）不用于求向量垂直，核心作用是消元去尺度。投影方程中的深度尺度 (s) 是多余未知量，利用反对称矩阵固有性质，可无条件消去 (s)，仅保留三维点 (P) 作为唯一未知量，构造纯线性方程组。

1. 反对称矩阵性质

对任意三维向量 (a)，反对称矩阵恒满足：
$x×a=0x_\times a = 0$
$x×(sa)=s(x×a)=0x_\times(sa) = s(x_\times a) = 0$

该性质可无条件消除任意尺度 (s)，是三角化消元的核心依据。

2. 代入消元推导

对第一帧投影式左乘 (x_{1\times})：
$x1×s1x1=x1×R1T(P−t1)x_{1\times} s_1 x_1 = x_{1\times} R_1^T(P-t_1)$

左边直接为0，(s_1) 消失：
$x1×R1TP=x1×R1Tt1x_{1\times} R_1^T P = x_{1\times} R_1^T t_1$

同理第二帧消去 (s_2)：
$x2×R2TP=x2×R2Tt2x_{2\times} R_2^T P = x_{2\times} R_2^T t_2$

五、构造线性方程组 AP=b

单帧消尺度后可得到3个方程，秩为2，仅存在2组线性无关约束。

两帧共得到4个独立约束，对应3个未知三维坐标 (P(X,Y,Z))，构成超定线性方程组。

提取两帧全部线性无关约束，堆叠得到超定方程组：
$[x1×[0,:]R1Tx1×[1,:]R1Tx2×[0,:]R2Tx2×[1,:]R2T]4×3P=[x1×R1Tt1[0]x1×R1Tt1[1]x2×R2Tt2[0]x2×R2Tt2[1]]4×1\begin{bmatrix} x_{1\times}[0,:]R_1^T \\ x_{1\times}[1,:]R_1^T \\ x_{2\times}[0,:]R_2^T \\ x_{2\times}[1,:]R_2^T \end{bmatrix}_{4\times3} P = \begin{bmatrix} x_{1\times}R_1^T t_1[0] \\ x_{1\times}R_1^T t_1[1] \\ x_{2\times}R_2^T t_2[0] \\ x_{2\times}R_2^T t_2[1] \end{bmatrix}_{4\times1}$

简化记为：
$A P = b$

超定方程抗噪原理

理想无噪声场景下，3个未知量仅需3个方程即可唯一求解。但真实图像存在像素噪声，不存在能严格满足4条约束的三维点，方程组无严格解析解。

因此只能求解全局误差最小的最优解。多出的1组冗余约束可平均、抵消随机像素误差，让三角化结果更鲁棒，这是超定方程天然的抗噪优势。

六、最优求解：SVD 齐次解法（工程标准）

超定方程可通过最小二乘解析解：
$P=(A^TA)^{-1}A^T b$

求解，但该方式数值极不稳定，这也是SLAM/VIO工程中统一改用SVD求解的核心原因。

1. 最小二乘数值不稳定的根本原因

最小二乘需要构造正规矩阵 (A^TA)，原始矩阵 (A) 自带图像噪声与浮点截断误差，矩阵平方运算会二次放大误差，严重损失数值精度。

同时三角化场景极易出现矩阵病态（近奇异）：相机平移极小、特征深度差异小时，(A^TA) 行列高度线性相关，行列式趋近于0。

此时求逆会无限放大微小噪声，导致三维点求解严重偏移、失效。

2. SVD核心优势

SVD无需构造正规矩阵、无需求逆，全程为稳定正交变换，完美适配病态矩阵与含噪数据，是视觉三角化的工业级稳健解法。

补充：为什么要构造增广矩阵？

原始方程 (AP=b) 为非齐次方程，而SVD最擅长求解齐次方程零空间最优解，数值稳定性极强。

为适配SVD求解范式，先做恒等变形：
$A P - b = 0$

拼接构造4×4增广矩阵，完成齐次化统一求解格式：
$[A−b][P1]=0\begin{bmatrix}A & -b\end{bmatrix} \begin{bmatrix}P \\ 1\end{bmatrix} = 0$

核心目的：

适配SVD零空间求解，彻底规避最小二乘求逆的数值病态问题；
整合全部约束信息，一步求解全局最优解，无需迭代与求逆。

对齐次化增广矩阵做SVD分解：
$[A−b]=UΣVT\begin{bmatrix}A & -b\end{bmatrix} = U\Sigma V^T$

取分解后矩阵 (V) 的最后一列（对应最小奇异值，为零空间最优解）：
$v = [v_1,v_2,v_3,v_4]^T$

归一化得到三维点：
$\left[ \frac{v_1}{v_4},\; \frac{v_2}{v_4},\; \frac{v_3}{v_4} \right]^T$

七、深度合法性校验（必做）

求解的三维点必须在相机前方，任意帧深度为负则解无效，直接丢弃该特征：
${s1=e3TR1T(P−t1)>0s2=e3TR2T(P−t2)>0\begin{cases} s_1 = e_3^T R_1^T(P-t_1) > 0 \\ s_2 = e_3^T R_2^T(P-t_2) > 0 \end{cases}$