Have a Question?

三角化 | Triangulation

You are here:

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] 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

Add a Comment

您的电子邮箱地址不会被公开。 必填项已用*标注

Table of Contents