最小二乘问题详解12：三角化中的非线性优化

1 引言

在前两篇文章《最小二乘问题详解10：PnP问题求解》和《最小二乘问题详解11：基于李代数的PnP优化》中，我们分别通过常规思想与李代数思想，深入探讨了计算机视觉中 SFM（Structure from Motion）系统的核心子问题之一——PnP 问题。该问题建模于针孔成像原理，本质上是利用单视图中的2D-3D对应关系求解相机位姿，常被归入单视图几何的范畴。
然而，仅靠单视图无法恢复场景的三维结构：深度信息在投影过程中永久丢失。要重建真实世界，必须引入多视图几何（Multi-view Geometry）。而在多视图框架下，最基础、最关键的优化问题之一，便是三角化（Triangulation）——即：在已知多个相机位姿的前提下，通过多视角下的同名点观测，反推空间中对应 3D 点的位置。
三角化看似简单，却是 SfM 和 SLAM 系统中“结构恢复”的基础，本文要讲解就是三角化问题的非线性优化求解方法。
2 问题建模

2.1 三角化定义

在计算机视觉中，三角化（Triangulation）是指已知多个相机的位姿和同一空间点在各图像中的观测位置，求解该点在世界坐标系下的三维坐标。其名称源于几何直观：从两个（或多个）相机光心向对应的图像点作射线，这些射线理论上应交于空间中的同一点——形成一个“三角形”。在理想无噪声情况下，两射线精确相交；但在实际中，由于位姿误差、特征匹配噪声等，射线往往不交，此时需通过优化找到“最佳”交点。
需要注意的是，三角化假设相机位姿已知（通常由 PnP 或 SfM 前端提供），因此它是一个纯结构恢复问题，与 PnP（已知结构求位姿）互为对偶。
2.2 成像模型回顾

回顾一下《最小二乘问题详解10：PnP问题求解》和《最小二乘问题详解11：基于李代数的PnP优化》中提到的针孔相机成像模型。设某空间点在世界坐标系下的位置为：

\[\mathbf{X} = [X, Y, Z]^\top \in \mathbb{R}^3\]
对于第 \(i\) 个相机，其位姿由旋转矩阵 \(\mathbf{R}_i \in SO(3)\) 和平移向量 \(\mathbf{t}_i \in \mathbb{R}^3\) 描述（即世界到相机的变换）。
该点在第 \(i\) 幅图像上的投影像素坐标 \(\mathbf{u}_i = [u_i, v_i]^\top\) 满足：

\[s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix}= \mathbf{K}_i \left( \mathbf{R}_i \mathbf{X} + \mathbf{t}_i \right)\tag{1}\]
其中：

• \(\mathbf{K}_i\) 为第 \(i\) 个相机的内参矩阵（通常假设已标定且恒定，记为 \(\mathbf{K}\)）；
  
• \(s_i\) 为未知尺度因子（深度）。
  

去齐次化后，得到重投影函数：

\[\pi(\mathbf{X}; \mathbf{R}_i, \mathbf{t}_i, \mathbf{K}) = \begin{bmatrix}f_x \cdot \dfrac{r_{i1}^\top \mathbf{X} + t_{ix}}{r_{i3}^\top \mathbf{X} + t_{iz}} + c_x \\f_y \cdot \dfrac{r_{i2}^\top \mathbf{X} + t_{iy}}{r_{i3}^\top \mathbf{X} + t_{iz}} + c_y\end{bmatrix}\tag{2}\]
其中 \(\mathbf{r}_{ij}^\top\) 表示 \(\mathbf{R}_i\) 的第 \(j\) 行。
2.3 优化目标函数

设某空间点被 \(N \geq 2\) 个视角观测到，对应图像坐标为 \(\{\mathbf{u}_1, \mathbf{u}_2, \dots, \mathbf{u}_N\}\)，相机位姿为 \(\{(\mathbf{R}_1, \mathbf{t}_1), \dots, (\mathbf{R}_N, \mathbf{t}_N)\}\)。我们的目标是找到一个 \(\mathbf{X} \in \mathbb{R}^3\)，使得其在所有视角下的重投影结果尽可能接近观测值。由此定义残差向量：

\[\mathbf{r}_i(\mathbf{X}) = \pi(\mathbf{X}; \mathbf{R}_i, \mathbf{t}_i, \mathbf{K}) - \mathbf{u}_i \in \mathbb{R}^2\tag{3}\]
最终的非线性最小二乘问题为：

\[\min_{\mathbf{X} \in \mathbb{R}^3} \quad \sum_{i=1}^{N} \left\| \mathbf{r}_i(\mathbf{X}) \right\|^2= \min_{\mathbf{X}} \quad \sum_{i=1}^{N} \left\| \pi(\mathbf{X}; \mathbf{R}_i, \mathbf{t}_i, \mathbf{K}) - \mathbf{u}_i \right\|^2\tag{4}\]
可以看到，与 PnP 问题相比，三角化问题的优化还相对简单一点：PnP 优化变量是 \(\mathbf{T} \in SE(3)\)，需李代数处理；三角化优化变量是 \(\mathbf{X} \in \mathbb{R}^3\)，在普通欧氏空间即可求解，无需流形。当然，由于 \(\pi(\cdot)\) 中存在分母（ \(r_{i3}^\top \mathbf{X} + t_{iz}\) ），整体为非线性、非凸函数，需借助迭代优化方法求解。
3 线性三角化：DLT 方法

尽管三角化的本质是非线性的（见式 (4)），但在实际应用中，我们常先使用一种线性近似方法快速获得初值，这就是直接线性变换（Direct Linear Transform, DLT）。DLT 的核心思想是将透视投影方程转化为齐次线性方程组，并通过 SVD 求解。
3.1 齐次方程的构造

回顾成像方程 (1)：

\[s_i \mathbf{u}_i^{\text{hom}} = \mathbf{K} (\mathbf{R}_i \mathbf{X} + \mathbf{t}_i)\]
其中 \(\mathbf{u}_i^{\text{hom}} = [u_i, v_i, 1]^\top\)。令 \(\mathbf{P}_i = \mathbf{K} [\mathbf{R}_i \mid \mathbf{t}_i] \in \mathbb{R}^{3 \times 4}\) 为第 \(i\) 个相机的投影矩阵，并将世界点表示为齐次坐标 \(\tilde{\mathbf{X}} = [X, Y, Z, 1]^\top \in \mathbb{R}^4\)，则上式可简写为：

\[s_i \mathbf{u}_i^{\text{hom}} = \mathbf{P}_i \tilde{\mathbf{X}}\tag{5}\]
由于等式两边相差一个未知尺度 \(s_i\)，我们可以利用叉积消去尺度：

\[\mathbf{u}_i^{\text{hom}} \times (\mathbf{P}_i \tilde{\mathbf{X}}) = \mathbf{0}\tag{6}\]
展开叉积（设 \(\mathbf{p}_{i1}^\top, \mathbf{p}_{i2}^\top, \mathbf{p}_{i3}^\top\) 为 \(\mathbf{P}_i\) 的三行）：

\[\begin{bmatrix}v_i (\mathbf{p}_{i3}^\top \tilde{\mathbf{X}}) - (\mathbf{p}_{i2}^\top \tilde{\mathbf{X}}) \\(\mathbf{p}_{i1}^\top \tilde{\mathbf{X}}) - u_i (\mathbf{p}_{i3}^\top \tilde{\mathbf{X}}) \\u_i (\mathbf{p}_{i2}^\top \tilde{\mathbf{X}}) - v_i (\mathbf{p}_{i1}^\top \tilde{\mathbf{X}})\end{bmatrix}= \mathbf{0}\]
注意到第三行是前两行的线性组合（因叉积秩为2），因此只需取前两行作为独立约束：

\[\begin{aligned}(u_i \mathbf{p}_{i3}^\top - \mathbf{p}_{i1}^\top) \tilde{\mathbf{X}} &= 0 \\(v_i \mathbf{p}_{i3}^\top - \mathbf{p}_{i2}^\top) \tilde{\mathbf{X}} &= 0\end{aligned}\tag{7}\]
对每个视角 \(i\)，我们得到两个线性方程。若共有 \(N\) 个视角，则可堆叠成一个 \(2N \times 4\) 的设计矩阵 \(\mathbf{A}\)：

\[\mathbf{A} \tilde{\mathbf{X}} = \mathbf{0}, \quad\mathbf{A} =\begin{bmatrix}u_1 \mathbf{p}_{13}^\top - \mathbf{p}_{11}^\top \\v_1 \mathbf{p}_{13}^\top - \mathbf{p}_{12}^\top \\\vdots \\u_N \mathbf{p}_{N3}^\top - \mathbf{p}_{N1}^\top \\v_N \mathbf{p}_{N3}^\top - \mathbf{p}_{N2}^\top\end{bmatrix}\in \mathbb{R}^{2N \times 4}\tag{8}\]
3.2 求解与归一化

方程 \(\mathbf{A} \tilde{\mathbf{X}} = \mathbf{0}\) 构成一个齐次线性系统。在《最小二乘问题详解2：线性最小二乘求解》中，我们已系统讨论了线性最小二乘问题的一般形式 \(\min \|\mathbf{A}\mathbf{x} - \mathbf{b}\|^2\) 及其求解方法。然而，DLT 所面对的是该框架下的一个特殊情形：\(\mathbf{b} = \mathbf{0}\)。由于齐次方程的解在尺度上不确定（若 \(\tilde{\mathbf{X}}\) 是解，则任意缩放 \(\lambda \tilde{\mathbf{X}}\) 也是解），直接最小化 \(\|\mathbf{A} \tilde{\mathbf{X}}\|\) 会退化为无意义的平凡解 \(\tilde{\mathbf{X}} = \mathbf{0}\)。因此，我们必须施加单位范数约束 \(\|\tilde{\mathbf{X}}\| = 1\)，以在单位球面上寻找使 \(\|\mathbf{A} \tilde{\mathbf{X}}\|\) 最小的非零向量——即具有单位长度的最优方向。
这一目标等价于：

\[\min_{\|\tilde{\mathbf{X}}\| = 1} \|\mathbf{A} \tilde{\mathbf{X}}\|^2\]
对 \(\mathbf{A} \in \mathbb{R}^{2N \times 4}\) 进行奇异值分解：

\[\mathbf{A} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top, \quad \boldsymbol{\Sigma} = \mathrm{diag}(\sigma_1, \sigma_2, \sigma_3, \sigma_4), \quad \sigma_1 \geq \sigma_2 \geq \sigma_3 \geq \sigma_4 \geq 0\]
由于 \(\mathbf{V} = [\mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3, \mathbf{v}_4]\) 是正交矩阵，其列向量构成 \(\mathbb{R}^4\) 的一组标准正交基。因此，任何满足 \(\|\tilde{\mathbf{X}}\| = 1\) 的候选解均可唯一表示为：

\[\tilde{\mathbf{X}} = \sum_{i=1}^4 \alpha_i \mathbf{v}_i, \quad \text{其中} \quad \sum_{i=1}^4 \alpha_i^2 = 1\]
由于 \(\mathbf{U}\) 是正交矩阵，满足 \(\|\mathbf{U} \boldsymbol{\Sigma}\| = \|\boldsymbol{\Sigma}\|\)，因此范数计算中 \(\mathbf{U}\) 可被消去。此时有：

\[\|\mathbf{A} \tilde{\mathbf{X}}\|^2 = \left\| \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top \tilde{\mathbf{X}} \right\|^2 = \left\| \boldsymbol{\Sigma} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \alpha_4 \end{bmatrix} \right\|^2 = \sum_{i=1}^4 \sigma_i^2 \alpha_i^2\]
为使该式最小，在 \(\sum \alpha_i^2 = 1\) 约束下，应将全部权重分配给最小奇异值对应的分量，即取 \(\alpha_4 = 1\)，其余 \(\alpha_i = 0\)。因此，最优解为：

\[\tilde{\mathbf{X}} = \mathbf{v}_4 = \mathbf{V}(:, 4)\]
<blockquote>

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！