Have a Question?
三角化 | Triangulation
1 定义
已知两幅图像间的姿态 T=\left[\begin{array}{l}\mathbf{r}_{1} \\\mathbf{r}_{2} \\\mathbf{r}_{3}\end{array}\right]=[R \quad t] 和三维点在两幅图像上对应的匹配点 \boldsymbol{p}_1 和 \boldsymbol{p}_2 (其归一化坐标为 x_{1}=\left(\begin{array}{c}u_{1} \\v_{1} \\1\end{array}\right), x_{2}=\left(\begin{array}{c}u_{2} \\v_{2} \\1\end{array}\right))。希望求解点 P=\left[\begin{array}{l}X \\Y \\Z \\1\end{array}\right] 的三维位置。
2 求解
本文主要求解推导参见这篇文章。
2.1 叉乘消元求解
设 x_{1}=\left(\begin{array}{c}u_{1} \\v_{1} \\1\end{array}\right), x_{2}=\left(\begin{array}{c}u_{2} \\v_{2} \\1\end{array}\right) 为两个特征点的归一化坐标,
按照对极约束 (Epipolar Constraint)中的定义:
s_{1} \boldsymbol{x}_{1}=s_{2} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{t}\tag{1}
我们希望求解的是特征点的深度 \boldsymbol{s}_1 或者 \boldsymbol{s}_2。
对于公式 (1) 两边左乘 \boldsymbol{x}_{1}^{\wedge}:
s_{1} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{x}_{1}=0=s_{2} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{x}_{1}^{\wedge} \boldsymbol{t}\tag{2}
但是由于噪声的存在,显然未必能够满足公式 (2) 等于0。因此最好的方式一般都是用最小二乘法。
2.2 线性方程求解
在 VINS-Mono 和 ORB-SLAM 中很多都用类似的求解线性方程的方法。它的大概思路如下:
定义三维点 P=\left[\begin{array}{l}X \\Y \\Z \\1\end{array}\right],其归一化坐标表示为 \mathbf{x}=\left[\begin{array}{l}u \\v \\1\end{array}\right]
根据投影方程:
\begin{aligned}& \lambda \mathbf{x}=T P \\&\Rightarrow \lambda \mathbf{x} \times T P=0 \\&\Rightarrow \hat{\mathbf{x}} T P=0\end{aligned}\tag{3}
利用向量叉乘的性质:
\mathbf{a} \times \mathbf{b}=\hat{\mathbf{a}} \mathbf{b}=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\a_{3} & 0 & -a_{1} \\-a_{2} & a_{1} & 0\end{array}\right]\left[\begin{array}{l}b_{1} \\b_{2} \\b_{3}\end{array}\right]\tag{4}
其中 \hat{\mathbf{a}} 表示向量 \mathbf{a} 对应的 反对称矩阵 (Skew-symmetric Matrix)。
得到:
\begin{aligned}\hat{\mathbf{x}} T P &=\left[\begin{array}{ccc}0 & -1 & v \\1 & 0 & -u \\-v & u & 0\end{array}\right]\left[\begin{array}{l}\mathbf{r}_{1} \\\mathbf{r}_{2} \\\mathbf{r}_{3}\end{array}\right] P \\&=\left[\begin{array}{c}-\mathbf{r}_{2}+v \mathbf{r}_{3} \\\mathbf{r}_{1}-u \mathbf{r}_{\mathbf{3}} \\-v \mathbf{r}_{1}+u \mathbf{r}_{2}\end{array}\right] P\end{aligned}\tag{5}
显然第一行 -\times \boldsymbol{u}、第二行 -\times \boldsymbol{v} 再相加即可得到第三行,所以是线性相关的,只需2个方程即可:
\left[\begin{array}{c}-\mathbf{r}_{2}+v \mathbf{r}_{3} \\\mathbf{r}_{1}-u \mathbf{r}_{3}\end{array}\right] P=0\tag{6}
因此已知一个相机的观测可以得到两个方程组,要求的三维点包含3个变量,理论上只需要2个以上相机观测即可进行三角化。
\left[\begin{array}{c}-\mathbf{r}_{2}^{(1)}+v^{(1)} \mathbf{r}_{3}^{(1)} \\\mathbf{r}_{1}^{(1)}-u^{(1)} \mathbf{r}_{3}^{(1)} \\-\mathbf{r}_{2}^{(2)}+v^{(2)} \mathbf{r}_{3}^{(2)} \\\mathbf{r}_{\mathbf{1}}^{(2)}-u^{(2)} \mathbf{r}_{3}^{(2)} \\\vdots\end{array}\right] X=0\tag{7}
上述方程为超定方程没有非0解,适合使用 SVD 求最小二乘解。对于双视图问题,我们令:
\boldsymbol{A}=\left[\begin{array}{c}-\mathbf{r}_{2}^{(1)}+v^{(1)} \mathbf{r}_{3}^{(1)} \\\mathbf{r}_{1}^{(1)}-u^{(1)} \mathbf{r}_{3}^{(1)} \\-\mathbf{r}_{2}^{(2)}+v^{(2)} \mathbf{r}_{3}^{(2)} \\\mathbf{r}_{\mathbf{1}}^{(2)}-u^{(2)} \mathbf{r}_{3}^{(2)}\end{array}\right]_{6 \times 4}\tag{8}
求解 \boldsymbol{A}x=0 就转化成如下问题:
J(x)=\min \|\boldsymbol{A} x\|\tag{9}使用 SVD 分解 A=U D V^{T},其中:U \in R^{6 \times 6}, D \in R^{6 \times 4}, V \in R^{4 \times 4}。由于 D \in R^{6 \times 4},则最小值在 y=[0,0,0,1]^{T} 也就是 V 的最右列得到:
y=V^{T} x \Longrightarrow x=\left[\begin{array}{l}x_0 \\x_1 \\x_2 \\x_3\end{array}\right]=V y\tag{10}
同时由于 P=\left[\begin{array}{l}X \\Y \\Z \\1\end{array}\right] 为齐次坐标,因此进行如下处理使得最后一项为 1:
X=\left[\begin{array}{l}x \\y \\z \\1\end{array}\right]=\left[\begin{array}{l}x_{0} / x_{3} \\x_{1} / x_{3} \\x_{2} / x_{3} \\x_{3} / x_{3}\end{array}\right]\tag{11}
当然如果不是已知归一化平面坐标而是像素坐标 \mathbf{x}^{\prime}=\left[\begin{array}{l}u^{\prime} \\v^{\prime} \\1\end{array}\right] 和内参 K,也是一样的:
\begin{aligned}& \lambda \mathrm{x}^{\prime}=K T P \\\Rightarrow & \lambda \mathbf{x}^{\prime} \times K T P=0 \\\Rightarrow & \hat{\mathbf{x}}^{\prime} K T P=0 \\\Rightarrow & \hat{\mathbf{x}}^{\prime} H P=0\end{aligned}\tag{12}
只不过系数矩阵由 T 变成了 H=K T
以下是 VINS-Mono 中的代码,使用 SVD 求解方程,和上面的过程是一样的:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d) { Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero(); design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0); design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1); design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0); design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1); Eigen::Vector4d triangulated_point; triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>(); point_3d(0) = triangulated_point(0) / triangulated_point(3); point_3d(1) = triangulated_point(1) / triangulated_point(3); point_3d(2) = triangulated_point(2) / triangulated_point(3); } |
参考材料
[1] https://www.cnblogs.com/FangLai-you/p/11276148.html
[2] https://www.cnblogs.com/Jessica-jie/p/7730731.html
[3] https://zhuanlan.zhihu.com/p/103694374
[4] 《视觉 SLAM 十四讲》
[5] https://blog.csdn.net/michaelhan3/article/details/89483148
[6] https://note.youdao.com/ynoteshare1/index.html?id=5e98f487c40ef22f90e1177f29271be5&type=note
[7] https://blog.csdn.net/weixin_43795395/article/details/95677116