视觉SLAM中的三角化
-
考虑某路标点 \(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\)求解,右奇异矩阵的最后一行就是最终的解。