Triangulate

• 考虑某路标点 $y$ 在若干个关键帧 $k = 1, · · · , n$ 中看到。

• $\mathbf{y} \in \mathbb{R}^{4}$ ，取齐次坐标。每次观测为 $\mathbf{x}_{k}=\left[u_{k}, v_{k}, 1\right]^{\top}$，取归一化平 面坐标 (这样可以忽略掉内参)。

• 记投影矩阵$\mathbf{P}_{k}=\left[\mathbf{R}_{k}, \mathbf{t}_{k}\right] \in \mathbb{R}^{3 \times 4}$，为 World 系到 Camera 系 投影关系：

  $$\forall k, \lambda_{k} \mathbf{x}_{k}=\mathbf{P}_{k} \mathbf{y}$$

其中 $\lambda_{k}$ 为观测点的深度值 (末知) 。

我们以一个通用方程来描述，这里我们把 $\lambda$ 放到了左边，但是效果是一样的。

$$\forall k, \lambda_{k} \mathbf{x}_{k}=\left[\mathbf{R}_{k}, \mathbf{t}_{k}\right]\mathbf{y}$$

$$\lambda _k\left[\begin{array}{l} u_k\\ v_k\\ 1 \end{array}\right]=\left[\begin{array}{cccc} p_{1} & p_{2} & p_{3} & p_{4} \\ p_{5} & p_{6} & p_{7} & p_{8} \\ p_{9} & p_{10} & p_{11} & p_{12} \end{array}\right]\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right]$$

根据上面式子中的第三行

$$\lambda_k =P^{\top}_{k,3}\mathbf{y}$$

$P^{T}_{k,3}$ 为 $P_k$ 的第 3 行。

将 $\lambda_k =P^{T}_{k,3}\mathbf{y}$ 带入前两行得到

$$\begin{array}{l} u_{k} \mathbf{P}_{k, 3}^{\top} \mathbf{y}=\mathbf{P}_{k, 1}^{\top} \mathbf{y} \\ v_{k} \mathbf{P}_{k, 3}^{\top} \mathbf{y}=\mathbf{P}_{k, 2}^{\top} \mathbf{y} \end{array}$$

每次观测将提供两个这样的方程，视 $\mathrm{y}$ 为末知量，并将 $\mathrm{y}$ 移到等 式一侧:

$$\left[\begin{array}{c} u_{1} \mathbf{P}_{1,3}^{\top}-\mathbf{P}_{1,1}^{\top} \\ v_{1} \mathbf{P}_{1,3}^{\top}-\mathbf{P}_{1,2}^{\top} \\ \vdots \\ u_{n} \mathbf{P}_{n, 3}^{\top}-\mathbf{P}_{n, 1}^{\top} \\ v_{n} \mathbf{P}_{n, 3}^{\top}-\mathbf{P}_{n, 2}^{\top} \end{array}\right] \mathbf{y = 0} \rightarrow \mathbf{D y}=\mathbf{0}$$

于是， $y$ 为 $\mathrm{D}$ 零空间中的一个非零元素。

由于 $\mathbf{D} \in \mathbb{R}^{2 n \times 4}$ ，在观测次于大于等于两次时，很可能 $\mathbf{D}$ 满秩， 无零空间。 寻找最小二乘解:

$$\min _{\mathbf{y}}\|\mathbf{D} \mathbf{y}\|_{2}^{2}, \quad \text { s.t. }\|\mathbf{y}\|=1$$

解法: 对 $\mathbf{D}^{\top} \mathbf{D}$ 进行 SVD:

$$\mathbf{D}^{\top} \mathbf{D}=\sum_{i=1}^{4} \sigma_{i}^{2} \mathbf{u}_{i} \mathbf{u}_{j}^{\top}$$

其中 $\sigma_{i}$ 为奇异值，且由大到小排列， $\mathbf{u}_{i}, \mathbf{u}_{j}$ 正交。此时,取 $y = u_4$ ，那么该问题的目标函数值为 $σ_4$ 。判断该解有效性的条件: $σ_4 ≪ σ_3$ 。若该条件成立,认为三角化有效,否则认为三角化无效。实用当中,还需要加上 $y$ 投影正负号的判定作为依据

TIPS

一般我们通过两对点进行三角化

$$\left[\begin{array}{c} u_{1} \mathbf{P}_{1,3}^{\top}-\mathbf{P}_{1,1}^{\top} \\ v_{1} \mathbf{P}_{1,3}^{\top}-\mathbf{P}_{1,2}^{\top} \\ u_{2} \mathbf{P}_{2, 3}^{\top}-\mathbf{P}_{2, 1}^{\top} \\ v_{2} \mathbf{P}_{2, 3}^{\top}-\mathbf{P}_{2, 2}^{\top} \end{array}\right] \mathbf{y = 0} \rightarrow \mathbf{D y}=\mathbf{0}$$

则有

$$A\mathbf{y}=0$$

SVD求解,右奇异矩阵的最后一行就是最终的解。