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) \\$

📌 为何必须用 SVD，而不能用 QR？

QR 分解适用于非齐次 最小二乘问题（$\mathbf{b} \neq \mathbf{0}$），其核心是通过正交变换将问题转化为上三角系统求解。但在齐次情形 $\mathbf{A}\tilde{\mathbf{X}} = \mathbf{0}$ 下，QR 无法直接揭示矩阵的零空间结构。只有 SVD 能显式给出所有奇异值及其对应的奇异向量，从而可靠地提取出使 $\|\mathbf{A}\tilde{\mathbf{X}}\|$ 最小的单位向量------这正是 DLT 所需的解。

最后，需对解进行去齐次化 （dehomogenization）。这是因为 $\tilde{\mathbf{X}} = $\\tilde{X}_x, \\tilde{X}_y, \\tilde{X}_z, \\tilde{X}_w$ ^\top$ 是齐次坐标，仅定义射影空间中的方向，而实际 3D 点位于欧氏空间。根据齐次坐标的定义，其对应的欧氏坐标为：

\ $\\mathbf{X}_{\\text{DLT}} = \\frac{1}{\\tilde{X}_w} \[\\tilde{X}_x, \\tilde{X}_y, \\tilde{X}_z$ ^\top \tag{9} \]

若 $\tilde{X}_w \approx 0$，说明点在无穷远处，通常应舍弃；若 $\tilde{X}_w < 0$，则可能对应负深度（点位于相机后方），需结合相机位姿进行符号校正。

3.3 DLT 的缺陷分析

尽管 DLT 实现简单、计算高效，但它存在几个根本性缺陷，限制了其精度：

优化目标不合理 ：DLT 最小化的是 $\|\mathbf{A} \tilde{\mathbf{X}}\|^2$，即代数残差 。该残差混合了像素坐标、深度和尺度，没有明确的几何或物理意义 。相比之下，我们真正关心的是重投影误差（单位：像素），如式 (4) 所示。
符号模糊性 ：由于 $\mathbf{A} (-\tilde{\mathbf{X}}) = -\mathbf{A} \tilde{\mathbf{X}} = \mathbf{0}$，若 $\tilde{\mathbf{X}}$ 是解，则 $-\tilde{\mathbf{X}}$ 也是解。这导致 DLT 可能输出负深度 的点（位于相机后方），必须通过深度符号校正 （检查 $(\mathbf{R}_i \mathbf{X} + \mathbf{t}_i)_z > 0$）才能得到合理结果。
对噪声高度敏感：代数误差对图像噪声缺乏鲁棒性。即使添加少量像素噪声（如 1 像素），DLT 解也可能严重偏离真值。这是因为 DLT 未考虑透视投影的非均匀性------相同的空间误差在不同深度产生的像素误差不同，但 DLT 对所有方程平等对待。

因此，在实际应用中，DLT 通常仅作为非线性最小二乘优化的初值------它计算快速，但精度有限。要获得高精度的三维点，我们必须回到几何本质：最小化重投影误差。

4 非线性三角化：最小化重投影误差

如第2节所述，三角化的理想目标是最小化重投影误差（式 (4)）：

\ $\\min_{\\mathbf{X} \\in \\mathbb{R}\^3} \\quad S(\\mathbf{X}) = \\sum_{i=1}\^{N} \\left\\\| \\pi(\\mathbf{X}; \\mathbf{R}_i, \\mathbf{t}_i, \\mathbf{K}) - \\mathbf{u}_i \\right\\\|\^2 \\$

该问题是典型的非线性最小二乘问题 。根据《最小二乘问题详解4：非线性最小二乘》和《最小二乘问题详解8：Levenberg-Marquardt方法》中的框架，其求解依赖于对残差函数 $\mathbf{r}_i(\mathbf{X})$ 的一阶泰勒展开 ，而展开的核心正是雅可比矩阵 $\mathbf{J}_i = \frac{\partial \mathbf{r}_i}{\partial \mathbf{X}^\top}$。

尽管现代优化库（如 Ceres）支持自动微分，但手动推导雅可比不仅能加深对几何模型的理解，还能在自定义优化器或性能敏感场景中提供关键优势。下面，我们详细推导该雅可比矩阵。

4.1 重投影函数的显式形式

为简化记号，令第 $i$ 个相机的外参变换为：

\ $\\mathbf{X}_c\^{(i)} = \\mathbf{R}_i \\mathbf{X} + \\mathbf{t}_i = \\begin{bmatrix} x_c \\\\ y_c \\\\ z_c \\end{bmatrix} \\$

则重投影函数（式 (2)）可写为：

\ $\\pi(\\mathbf{X}) = \\begin{bmatrix} u \\\\ v \\end{bmatrix}= \\begin{bmatrix} f_x \\frac{x_c}{z_c} + c_x \\\\ f_y \\frac{y_c}{z_c} + c_y \\end{bmatrix} \\$

残差为：

\ $\\mathbf{r}_i(\\mathbf{X}) = \\begin{bmatrix} r_u \\\\ r_v \\end{bmatrix}= \\begin{bmatrix} f_x \\frac{x_c}{z_c} + c_x - u_i \\\\ f_y \\frac{y_c}{z_c} + c_y - v_i \\end{bmatrix} \\$

4.2 雅可比矩阵推导

我们需要计算：

\ $\\mathbf{J}_i = \\frac{\\partial \\mathbf{r}_i}{\\partial \\mathbf{X}\^\\top} = \\begin{bmatrix} \\frac{\\partial r_u}{\\partial X} \& \\frac{\\partial r_u}{\\partial Y} \& \\frac{\\partial r_u}{\\partial Z} \\\\ \\frac{\\partial r_v}{\\partial X} \& \\frac{\\partial r_v}{\\partial Y} \& \\frac{\\partial r_v}{\\partial Z} \\end{bmatrix} \\in \\mathbb{R}\^{2 \\times 3} \\$

利用链式法则：

\ $\\frac{\\partial r_u}{\\partial \\mathbf{X}} = \\frac{\\partial r_u}{\\partial x_c} \\frac{\\partial x_c}{\\partial \\mathbf{X}} + \\frac{\\partial r_u}{\\partial z_c} \\frac{\\partial z_c}{\\partial \\mathbf{X}} \\$

首先计算中间偏导：

\ $\\frac{\\partial r_u}{\\partial x_c} = \\frac{f_x}{z_c}, \\quad \\frac{\\partial r_u}{\\partial z_c} = -f_x \\frac{x_c}{z_c\^2} \\$

\ $\\frac{\\partial r_v}{\\partial y_c} = \\frac{f_y}{z_c}, \\quad \\frac{\\partial r_v}{\\partial z_c} = -f_y \\frac{y_c}{z_c\^2} \\$

而：

\ $\\frac{\\partial x_c}{\\partial \\mathbf{X}} = \\mathbf{r}_{i1}\^\\top, \\quad \\frac{\\partial y_c}{\\partial \\mathbf{X}} = \\mathbf{r}_{i2}\^\\top, \\quad \\frac{\\partial z_c}{\\partial \\mathbf{X}} = \\mathbf{r}_{i3}\^\\top \\$

其中 $\mathbf{r}_{ij}^\top$ 是 $\mathbf{R}_i$ 的第 $j$ 行。

因此，最终雅可比矩阵为：

\ $\\boxed{ \\mathbf{J}_i = \\begin{bmatrix} \\frac{f_x}{z_c} \\mathbf{r}_{i1}\^\\top - \\frac{f_x x_c}{z_c\^2} \\mathbf{r}_{i3}\^\\top \\\\ \\frac{f_y}{z_c} \\mathbf{r}_{i2}\^\\top - \\frac{f_y y_c}{z_c\^2} \\mathbf{r}_{i3}\^\\top \\end{bmatrix} } \\tag{10} \\$

或等价地写成：

\ $\\mathbf{J}_i = \\begin{bmatrix} \\frac{f_x}{z_c\^2} (z_c \\mathbf{r}_{i1}\^\\top - x_c \\mathbf{r}_{i3}\^\\top) \\\\ \\frac{f_y}{z_c\^2} (z_c \\mathbf{r}_{i2}\^\\top - y_c \\mathbf{r}_{i3}\^\\top) \\end{bmatrix} \\$

雅可比矩阵描述了3D 点微小扰动如何影响图像观测 。在这里可以看到，分母 $z_c^2$ 表明：深度越大（$z_c$ 越大），图像对 3D 扰动越不敏感------这正是透视投影的非均匀性体现，也是 DLT 忽略的关键信息。

4.3 整体优化流程

有了残差 $\mathbf{r}_i(\mathbf{X})$ 和雅可比 $\mathbf{J}_i$，即可构建整体残差向量 $\mathbf{r}(\mathbf{X}) \in \mathbb{R}^{2N}$ 和雅可比矩阵 $\mathbf{J}(\mathbf{X}) \in \mathbb{R}^{2N \times 3}$（将所有 $\mathbf{J}_i$ 垂直堆叠）。

随后，可采用 Gauss-Newton 或 Levenberg-Marquardt 方法迭代求解（详见《最小二乘问题详解4：非线性最小二乘》和《最小二乘问题详解8：Levenberg-Marquardt方法》）：

\ $(\\mathbf{J}\^\\top \\mathbf{J})\\Delta \\mathbf{X} = -\\mathbf{J}\^\\top \\mathbf{r} \\quad \\text{(GN)} \\$

或

\ $(\\mathbf{J}\^\\top \\mathbf{J} + \\lambda \\mathbf{I}) \\Delta \\mathbf{X} = -\\mathbf{J}\^\\top \\mathbf{r} \\quad \\text{(LM)} \\$

5 实例

根据前文的理论推导，我们完整实现了基于 DLT 初值估计与非线性优化的三角化流程。具体代码如下：

cpp 复制代码

#include <ceres/ceres.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iomanip>
#include <iostream>
#include <random>
#include <vector>

constexpr double PI = 3.14159265358979323846;

// 投影函数：将世界坐标 X 投影到图像平面（对应式 (1)）
Eigen::Vector2d Project(const Eigen::Matrix3d& K, const Eigen::Matrix3d& R_i,
                        const Eigen::Vector3d& t_i,
                        const Eigen::Vector3d& X_world) {
  // 相机坐标系下的点: X_c = R_i * X + t_i （式 (1) 中间步骤）
  Eigen::Vector3d X_cam = R_i * X_world + t_i;
  // 像素齐次坐标: s * [u, v, 1]^T = K * X_cam
  Eigen::Vector3d px_hom = K * X_cam;
  // 去齐次化（式 (2)）
  return Eigen::Vector2d(px_hom.x() / px_hom.z(), px_hom.y() / px_hom.z());
}

// DLT 三角化（基于式 (5)-(8)）
Eigen::Vector3d TriangulateDLT(const std::vector<Eigen::Matrix3d>& Rs,
                               const std::vector<Eigen::Vector3d>& ts,
                               const std::vector<Eigen::Vector2d>& observations,
                               const Eigen::Matrix3d& K) {
  size_t N = Rs.size();
  Eigen::MatrixXd A(2 * N, 4);  // 式 (8): A ∈ ℝ^{2N×4}

  for (size_t i = 0; i < N; ++i) {
    // 构造完整的投影矩阵 P_i = K [R_i | t_i] ∈ ℝ^{3×4} （式 (5)）
    Eigen::Matrix<double, 3, 4> P_i;
    P_i.block<3, 3>(0, 0) = K * Rs[i];  // K * R_i
    P_i.col(3) = K * ts[i];             // K * t_i

    double u_i = observations[i].x();  // 观测像素 u_i
    double v_i = observations[i].y();  // 观测像素 v_i

    // 构造 DLT 约束（式 (7)）:
    // (u_i * p_{i3}^T - p_{i1}^T) * X_tilde = 0
    // (v_i * p_{i3}^T - p_{i2}^T) * X_tilde = 0
    A.row(2 * i) = u_i * P_i.row(2) - P_i.row(0);
    A.row(2 * i + 1) = v_i * P_i.row(2) - P_i.row(1);
  }

  // SVD 求解 min ||A X_tilde|| s.t. ||X_tilde||=1 （式 (9) 前）
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV);
  Eigen::Vector4d X_tilde = svd.matrixV().col(3);  // 对应 v_4

  // 齐次坐标 X_tilde = [X, Y, Z, W]^T → 欧氏坐标 = [X/W, Y/W, Z/W]^T
  if (std::abs(X_tilde.w()) < 1e-8) {
    // 点在无穷远处，无法有效三角化；返回原点或可抛异常
    return Eigen::Vector3d::Zero();
  }
  Eigen::Vector3d X_euclid(X_tilde.x() / X_tilde.w(), X_tilde.y() / X_tilde.w(),
                           X_tilde.z() / X_tilde.w());

  // 符号校正：由于 X_tilde 和 -X_tilde 都是齐次解，
  // 去齐次化后对应 X_euclid 和 -X_euclid，需选择使更多相机看到正深度的解
  Eigen::Vector3d X1 = X_euclid;
  Eigen::Vector3d X2 = -X_euclid;

  auto count_positive_depth = [&](const Eigen::Vector3d& X) -> int {
    int cnt = 0;
    for (size_t i = 0; i < N; ++i) {
      double z_cam = (Rs[i] * X + ts[i]).z();  // (R_i X + t_i)_z
      if (z_cam > 0) cnt++;
    }
    return cnt;
  };

  Eigen::Vector3d X_dlt =
      (count_positive_depth(X1) >= count_positive_depth(X2)) ? X1 : X2;

  // 最终保障：至少第一个相机深度为正
  if ((Rs[0] * X_dlt + ts[0]).z() <= 0) {
    X_dlt = -X_dlt;
  }

  return X_dlt;
}

// Ceres 残差块：重投影误差（对应式 (3)）
struct ReprojectionError {
  ReprojectionError(const Eigen::Vector2d& u_obs, const Eigen::Matrix3d& K,
                    const Eigen::Matrix3d& R_i, const Eigen::Vector3d& t_i)
      : u_obs_(u_obs), K_(K), R_i_(R_i), t_i_(t_i) {}

  template <typename T>
  bool operator()(const T* const X_world, T* residuals) const {
    // X_world: 优化变量，对应式 (4) 中的 X ∈ ℝ^3
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> X(X_world);

    // 转换到相机坐标系: X_cam = R_i * X + t_i
    Eigen::Matrix<T, 3, 3> R_i_T = R_i_.template cast<T>();
    Eigen::Matrix<T, 3, 1> t_i_T = t_i_.template cast<T>();
    Eigen::Matrix<T, 3, 1> X_cam = R_i_T * X + t_i_T;

    // 投影到像素平面（式 (2)）
    Eigen::Matrix<T, 3, 3> K_T = K_.template cast<T>();
    Eigen::Matrix<T, 3, 1> px_hom = K_T * X_cam;
    T u_proj = px_hom[0] / px_hom[2];  // f_x * x_c / z_c + c_x
    T v_proj = px_hom[1] / px_hom[2];  // f_y * y_c / z_c + c_y

    // 残差 = 投影值 - 观测值（式 (3)）
    residuals[0] = u_proj - T(u_obs_.x());
    residuals[1] = v_proj - T(u_obs_.y());

    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector2d& u_obs,
                                     const Eigen::Matrix3d& K,
                                     const Eigen::Matrix3d& R_i,
                                     const Eigen::Vector3d& t_i) {
    return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>(
        new ReprojectionError(u_obs, K, R_i, t_i));
  }

 private:
  Eigen::Vector2d u_obs_;    // 观测像素坐标 [u_i, v_i]^T
  Eigen::Matrix3d K_, R_i_;  // 内参、旋转
  Eigen::Vector3d t_i_;      // 平移
};

int main() {
  // === 相机内参 K（式 (1)）===
  double f_x = 800.0, f_y = 800.0, c_x = 320.0, c_y = 240.0;
  Eigen::Matrix3d K;
  K << f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1;

  // === 真实3D点 X_gt（世界坐标）===
  Eigen::Vector3d X_gt(1.2, -0.5, 3.0);

  // === 相机位姿 {R_i, t_i} ===
  std::vector<Eigen::Matrix3d> Rs;
  std::vector<Eigen::Vector3d> ts;

  // 相机1: 单位位姿
  Rs.push_back(Eigen::Matrix3d::Identity());
  ts.push_back(Eigen::Vector3d::Zero());

  // 相机2: 绕Y轴旋转30度，平移(0.5, 0, 0)
  double angle = PI / 6.0;
  Eigen::AngleAxisd rot(angle, Eigen::Vector3d::UnitY());
  Rs.push_back(rot.toRotationMatrix());
  ts.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));

  // 相机3: 绕Y轴旋转 -20 度，平移 (-0.3, 0.1, 0.2)
  Rs.push_back(Eigen::AngleAxisd(-PI / 9.0, Eigen::Vector3d::UnitY())
                   .toRotationMatrix());
  ts.push_back(Eigen::Vector3d(-0.3, 0.1, 0.2));

  // === 生成带噪声观测 {u_i} ===
  std::vector<Eigen::Vector2d> observations_clean;
  for (size_t i = 0; i < Rs.size(); ++i) {
    Eigen::Vector2d proj = Project(K, Rs[i], ts[i], X_gt);
    observations_clean.push_back(proj);
  }

  std::mt19937 gen(42);
  std::normal_distribution<double> noise(0.0, 2);  // 2像素高斯噪声
  std::vector<Eigen::Vector2d> observations;
  for (const auto& obs : observations_clean) {
    double u_noisy = obs.x() + noise(gen);
    double v_noisy = obs.y() + noise(gen);
    observations.emplace_back(u_noisy, v_noisy);
  }

  std::cout << std::fixed << std::setprecision(6);
  std::cout << "=== Ground Truth 3D Point X_gt ===" << std::endl;
  std::cout << "[" << X_gt.x() << ", " << X_gt.y() << ", " << X_gt.z() << "]"
            << std::endl;

  // === 1. DLT 初值 ===
  Eigen::Vector3d X_dlt = TriangulateDLT(Rs, ts, observations, K);
  std::cout << "\n=== DLT Estimate X_dlt ===" << std::endl;
  std::cout << "[" << X_dlt.x() << ", " << X_dlt.y() << ", " << X_dlt.z() << "]"
            << std::endl;

  // === 2. 非线性优化（最小化式 (4)）===
  double X_opt[3] = {X_dlt.x(), X_dlt.y(), X_dlt.z()};  // 初值

  ceres::Problem problem;
  for (size_t i = 0; i < Rs.size(); ++i) {
    problem.AddResidualBlock(
        ReprojectionError::Create(observations[i], K, Rs[i], ts[i]), nullptr,
        X_opt);
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 20;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << "\n" << summary.BriefReport() << "\n";

  Eigen::Vector3d X_est(X_opt[0], X_opt[1], X_opt[2]);
  std::cout << "=== Nonlinear Optimization Estimate X_est ===" << std::endl;
  std::cout << "[" << X_est.x() << ", " << X_est.y() << ", " << X_est.z() << "]"
            << std::endl;

  // === 3. 评估 ===
  double dlt_error = (X_gt - X_dlt).norm();
  double opt_error = (X_gt - X_est).norm();

  double total_reproj_err_sq = 0.0;
  for (size_t i = 0; i < Rs.size(); ++i) {
    Eigen::Vector2d proj = Project(K, Rs[i], ts[i], X_est);
    double err = (proj - observations[i]).norm();
    total_reproj_err_sq += err * err;
  }
  double reproj_rmse = std::sqrt(total_reproj_err_sq / Rs.size());

  std::cout << "\n=== Evaluation ===" << std::endl;
  std::cout << "DLT 3D error:      " << dlt_error << " meters" << std::endl;
  std::cout << "Optimized 3D error: " << opt_error << " meters" << std::endl;
  std::cout << "Final reprojection RMSE: " << reproj_rmse << " pixels"
            << std::endl;

  return 0;
}

该实现遵循典型的视觉三维重建范式：先通过 DLT（Direct Linear Transform）快速获得一个闭式初值，再以该初值为起点，利用 Ceres Solver 对重投影误差进行非线性最小二乘优化 ，从而在几何意义上获得更精确的 3D 点估计。整个流程与《最小二乘问题详解9：使用Ceres求解非线性最小二乘》中介绍的通用优化框架完全一致。

值得注意的是，DLT 求解的是齐次坐标下的代数最小二乘问题，其解在符号上具有天然的二义性------即若 $\tilde{\mathbf{X}}$ 是解，则 $-\tilde{\mathbf{X}}$ 同样满足方程。然而，只有其中一个符号对应物理上合理的 3D 点（即在所有或大多数相机前方）。因此，代码中专门加入了符号校正机制 ：通过统计各视角下点的深度（$z$ 分量）是否为正，选择使更多相机看到"正深度"的解，并进一步确保第一个相机的深度为正，以消除歧义。

本实验设置了三个视角：主相机位于原点，第二、第三相机分别绕 Y 轴旋转 ±30°/20° 并施加小幅平移，构成良好的三角化几何结构。同时，我们在理想投影上叠加了标准差为 2 像素的高斯噪声，以模拟实际特征匹配中的观测误差。

程序运行输出如下：

text 复制代码

=== Ground Truth 3D Point X_gt ===
[1.200000, -0.500000, 3.000000]

=== DLT Estimate X_dlt ===
[1.195321, -0.500514, 2.982901]
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  3.862688e+00    0.00e+00    6.30e+02   0.00e+00   0.00e+00  1.00e+04        0    9.30e-05    2.91e-04
   1  3.171789e+00    6.91e-01    4.62e-01   0.00e+00   1.00e+00  3.00e+04        1    1.12e-04    5.51e-04

Ceres Solver Report: Iterations: 2, Initial cost: 3.862688e+00, Final cost: 3.171789e+00, Termination: CONVERGENCE
=== Nonlinear Optimization Estimate X_est ===
[1.195772, -0.498440, 2.983621]

=== Evaluation ===
DLT 3D error:      0.017735 meters
Optimized 3D error: 0.016988 meters
Final reprojection RMSE: 1.454141 pixels

可以看到，此处 DLT 的 3D 误差略大于优化结果（0.0177 > 0.0169），表明非线性优化确实带来了改进。不过，DLT 的初值已经非常接近真值，非线性优化带来的 3D 位置改进相对微小（约 4%）。这是因为当前三角化条件良好（视角夹角适中、基线合理、噪声水平较低）。另外，从输出可见，优化后的重投影 RMSE 已成功降低，说明优化器确实更好地拟合了带噪观测数据。

在真实应用中，三角化精度高度依赖于多视图几何构型。当面临小基线、大噪声、弱纹理或极端视角等挑战性场景时，DLT 的代数误差会显著放大，导致深度估计严重失真；而基于重投影误差的非线性优化则能通过合理的几何约束保持鲁棒性与精度，优势将更加明显。

6. 问题

尽管三角化（包括 DLT 和非线性优化）是多视图几何中的核心工具，但在实际应用中其可靠性受到多重因素的制约：

视角夹角过小（小基线）：当两个相机光心距离很近或观察方向几乎平行时，反投影光线近乎平行，导致深度方向上的不确定性急剧增大（即"三角化病态"）。此时即使使用非线性优化，3D 估计也会对噪声极度敏感，误差可能高达数米。
观测点位于图像边缘或遮挡区域：特征匹配在这些区域本身不可靠，容易引入大偏差观测，从而误导优化过程。
纹理缺失或重复纹理区域：在这些区域，特征点定位不准，等效于引入了大噪声输入，同样破坏了三角化的前提条件。
动态物体或非刚性场景：这类场景违反了"静态点在多视图中一致"的基本假设，使得三角化结果无意义。

基于以上且不局限于以上的现实工程环境，单纯依赖两视图或多视图三角化可能失效。实践中常采用以下策略来提高三角化的鲁棒性和精度：

设置三角化角度阈值（如 > 1°~2°），仅对满足几何条件的点进行三角化。这有助于筛选出那些具有足够立体信息的点，避免处理病态情况。
结合先验信息，例如单目深度估计、立体匹配或 RGB-D 传感器提供的深度图。这些额外的信息可以在三角化之前提供可靠的初始估计，增强系统的稳定性。
引入鲁棒核函数（如 Huber、Cauchy），以抑制异常观测的影响。这种方法能够在一定程度上减轻大偏差观测对最终解的影响，提高整体的鲁棒性。
将三角化嵌入更大的优化框架，如全局 Bundle Adjustment（BA）。通过联合优化相机位姿与 3D 点，提升整个系统的几何一致性，进一步减少累积误差。
对于无法三角化的点，延迟初始化或直接放弃，避免错误的3D点污染地图。这种做法可以防止由于不准确的三角化结果带来的连锁错误，确保系统输出的高质量。

因此，虽然三角化是视觉 SLAM 与 SfM 中的基础模块，但其成功应用高度依赖于合理的场景条件与系统级设计。未来的研究方向可以进一步探索基于学习的深度初始化方法、概率三角化（考虑观测协方差）或多传感器融合策略，以突破传统几何方法的局限，提高系统的鲁棒性和精度。

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