三角化 | Ciallo(∠・ω< )⌒☆

三角化前端中最重要的提供深度信息的步骤。分为线性三角化和非线性三角化，前者足够信任观测，直接用SVD求解目标位置，后者视为最小二乘问题进行优化估计。

线性三角化

首先是根据[相机投影模型](../../../02 数学知识/10 相机投影模型.md?from=from_copylink)，我们可以得到

$$\begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} \sim P_i \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$$

其中波浪线表示左右成比例，相差一个比例系数 $\lambda$

$$\lambda_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = P_i \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$$

注意到这个比例系数的来源，从推导上来看，右侧的转换都是可逆的，并没有丢失深度信息，但是在之后的归一化过程中，我们需要除以左侧的第三个分量来归中，这是保证所有点都能够统一到像素空间，表示为齐次矩阵。这个比例系数就是深度信息，我们的目的也就是要估计这个系数 $\lambda$。

这个方程涉及到四个未知量和三个线性方程，我们需要找办法消除这个标量系数 $\lambda$。我们将 $P_i$拆分成三行，每行含义是这个方向上的投影分量

$$P_i = \begin{bmatrix} p_1^T \\ p_2^T \\ p_3^T \end{bmatrix}$$

于是可以将方程拆成如下方程组：

$$\lambda_i u_i = p_1^T X$$

$$\lambda_i v_i = p_2^T X$$

$$\lambda_i = p_3^T X$$

将第三个式子直接带入前两个式子进行消元

$$u_i (p_3^T X) = p_1^T X$$

$$v_i (p_3^T X) = p_2^T X$$

移项得到

$$(u_i p_3^T - p_1^T) X = 0$$

$$(v_i p_3^T - p_2^T) X = 0$$

也就是 $AX=0$的形式。

每一个相机都会贡献两个线性方程，我们堆叠所有相机关于同一点的方程，得到线性齐次方程组 $A X = 0$ ，其中

$$A = \begin{bmatrix} u_1 p_3^T - p_1^T \\ v_1 p_3^T - p_2^T \\ \vdots \\ u_N p_3^T - p_1^T \\ v_N p_3^T - p_2^T \end{bmatrix}$$

根据奇异值分解，我们求解 $V$矩阵的最右侧列向量即为所需 $X$，归一化后就能得到所需坐标。

由于大部分情况下实现不一定能够重合与目标点，甚至是异面，我们采用非线性优化的方式来处理这个问题，用最小二乘原理来对目标位置进行估计。

$$X^* = \arg\min_X \sum_{i=1}^{N} \left\| \begin{bmatrix} u_i \\ v_i \end{bmatrix} - \pi_i(X) \right\|^2$$

其中 $\pi_i(X) $表示点 $X$在相机中的预测坐标，是由变换矩阵计算的像素坐标除以深度信息得到的

$$\pi_i(X) = \begin{bmatrix} \frac{p_1^T X}{p_3^T X} \\ \frac{p_2^T X}{p_3^T X} \end{bmatrix}$$

这个其实就是所谓的重投影误差，后续采用高斯牛顿法迭代即可逼近目标。

非线性三角化需要初始值。通常用线性三角化（DLT）的结果作为初始值。

三角化后，点 $X$的协方差可以用一阶误差传播估计。假设像素噪声协方差为 $Σ_r$：

$$\Sigma_X = (J^T \Sigma_r^{-1} J)^{-1}$$

后续可用于马氏范数加权。