视觉SLAM中的三角化

2023-03-26 21:09:53 来源：博客园

视觉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\)求解,右奇异矩阵的最后一行就是最终的解。

关键词：