视觉SLAM中的三角化

Posted 2023-03-27 weihao-ysgs

视觉SLAM中的三角化

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

• \\(\\mathbfy \\in \\mathbbR^4\\) ，取齐次坐标。每次观测为 \\(\\mathbfx_k=\\left[u_k, v_k, 1\\right]^\\top\\) ，取归一化平 面坐标 (这样可以忽略掉内参)。

• 记投影矩阵 \\(\\mathbfP_k=\\left[\\mathbfR_k, \\mathbft_k\\right] \\in \\mathbbR^3 \\times 4\\) ，为 World 系到 Camera 系 投影关系：

  \\[\\forall k, \\lambda_k \\mathbfx_k=\\mathbfP_k \\mathbfy \\]

其中 \\(\\lambda_k\\) 为观测点的深度值 (末知) 。

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

\\[\\forall k, \\lambda_k \\mathbfx_k=\\left[\\mathbfR_k, \\mathbft_k\\right]\\mathbfy \\]

\\[\\lambda _k\\left[\\beginarrayl u_k\\\\ v_k\\\\ 1 \\endarray\\right]=\\left[\\beginarraycccc p_1 & p_2 & p_3 & p_4 \\\\ p_5 & p_6 & p_7 & p_8 \\\\ p_9 & p_10 & p_11 & p_12 \\endarray\\right]\\left[\\beginarrayl X \\\\ Y \\\\ Z \\\\ 1 \\endarray\\right] \\]

根据上面式子中的第三行

\\[\\lambda_k =P^\\top_k,3\\mathbfy \\]

\\(P^T_k,3\\) 为 \\(P_k\\) 的第 \\(3\\) 行。

将 \\(\\lambda_k =P^T_k,3\\mathbfy\\) 带入前两行得到

\\[\\beginarrayl u_k \\mathbfP_k, 3^\\top \\mathbfy=\\mathbfP_k, 1^\\top \\mathbfy \\\\ v_k \\mathbfP_k, 3^\\top \\mathbfy=\\mathbfP_k, 2^\\top \\mathbfy \\endarray \\]

每次观测将提供两个这样的方程，视 \\(\\mathrmy\\) 为末知量，并将 \\(\\mathrmy\\) 移到等 式一侧:

\\[\\left[\\beginarrayc u_1 \\mathbfP_1,3^\\top-\\mathbfP_1,1^\\top \\\\ v_1 \\mathbfP_1,3^\\top-\\mathbfP_1,2^\\top \\\\ \\vdots \\\\ u_n \\mathbfP_n, 3^\\top-\\mathbfP_n, 1^\\top \\\\ v_n \\mathbfP_n, 3^\\top-\\mathbfP_n, 2^\\top \\endarray\\right] \\mathbfy = 0 \\rightarrow \\mathbfD y=\\mathbf0 \\]

于是， \\(y\\) 为 \\(\\mathrmD\\) 零空间中的一个非零元素。

由于 \\(\\mathbfD \\in \\mathbbR^2 n \\times 4\\) ，在观测次于大于等于两次时，很可能 \\(\\mathbfD\\) 满秩， 无零空间。
寻找最小二乘解:

\\[\\min _\\mathbfy\\|\\mathbfD \\mathbfy\\|_2^2, \\quad \\text s.t. \\|\\mathbfy\\|=1 \\]

解法: 对 \\(\\mathbfD^\\top \\mathbfD\\) 进行 \\(SVD\\):

\\[\\mathbfD^\\top \\mathbfD=\\sum_i=1^4 \\sigma_i^2 \\mathbfu_i \\mathbfu_j^\\top \\]

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

TIPS

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

\\[\\left[\\beginarrayc u_1 \\mathbfP_1,3^\\top-\\mathbfP_1,1^\\top \\\\ v_1 \\mathbfP_1,3^\\top-\\mathbfP_1,2^\\top \\\\ u_2 \\mathbfP_2, 3^\\top-\\mathbfP_2, 1^\\top \\\\ v_2 \\mathbfP_2, 3^\\top-\\mathbfP_2, 2^\\top \\endarray\\right] \\mathbfy = 0 \\rightarrow \\mathbfD y=\\mathbf0 \\]

则有

\\[A\\mathbfy=0 \\]

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