从零手写VIO第六章作业（三角化理论推导及代码仿真）

一、三角化

1. 理论推导

由于噪声影响，通常来说$D$是一个满秩矩阵，我们应该求最小二乘解$y$

2.最小二乘解$y$几何意义

由于噪声的影响，$O_{1}P$和$O_{2}P$不能相交于一点，解出的最小二乘解就是到两条射线距离和最短的点

二、求最小二乘解$y$

三角化问题最终转化为最小二乘问题

$$\begin{array}{r}min\Vert Dy{\Vert }_2^2\\ s.t.\Vert y\Vert =1\end{array}$$

其中
$$\begin{array}{r}\Vert Dy{\Vert }_2^2=y^T{D}^{\top }Dy\end{array}$$

对D进行SVD分解
$$\begin{array}{r}{D}_{2n\ast 4}={\mathrm{U}}_{2n\ast 2n}{\Sigma }_{2n\ast 4}{\mathrm{V}}_{4\ast 4}^{\mathrm{T}}\end{array}$$

则有
$$\begin{array}{r}{\left(D^{T}D\right)}_{4\ast 4}=V{\Sigma }^{\top }\Sigma V^{\top }=\sum _{i=1}^4{\sigma }_i^2{v}_i{v}_i^T\end{array}$$

D满秩且${\sigma }_1^2>{\sigma }_2^2>{\sigma }_3^2>{\sigma }_4^2$

因为$v_1,v_2,v_3,v_4$为一组标准正交基，且$y$维数为四

设$y={\sum }_{i=1}^4{k}_i{v}_i$，因为$\Vert y{\Vert }_2^2=1$，故有${\sum }_{i=1}^4{k}_i^2=1$

则
$$\begin{array}{rl}{y}^T{D}^{T}Dy& ={\left(\sum _{i=1}^4{k}_i{v}_i\right)}^{\top }\left(\sum _{i=1}^4{\sigma }_i^2{v}_i{v}_i^{\top }\right)\left(\sum _{i=1}^4{k}_i{v}_i\right)\\ & =\sum _{i=1}^4{k}_i^2{\sigma }_i^2⩾{\sigma }_4^2\end{array}$$

当且仅当$y=v_4$时等号成立，原问题取最小值
故路标点$y$的齐次坐标归一化后的坐标即为SVD分解后最小奇异值对应的向量$v_4$。
最终求得路标点坐标$y=k\ast v_4$，其中$k\ast (v_4{)}_4=1$(将归一化形式恢复成齐次坐标形式)

三、三角化仿真

1. 代码实现

注意要对$D^{T}D$进行SVD分解，不是对$D$进行SVD分解。
投影矩阵是
$$\mathbf{P}=\left[{\mathbf{R}}_{cw},{\mathbf{t}}_{cw}\right]$$
表示从世界坐标系到camera系投影，别搞错了。

2. 仿真结果

四、奇异值比例与观测噪声之间的关系

\quad 若不存在噪声，则矩阵$D^{T}D$秩为3，即${\sigma }_4=0$，但实际场景通常是存在噪声的，我们要判断${\sigma }_4<<{\sigma }_3$是否成立。此部分我们将讨论观测噪声与$\frac{{\sigma }_4}{{\sigma }_3}$之间的关系。

1. 观测噪声建模

在三角化时我们都是用归一化平面坐标，首先要确定像素平面误差与归一化平面误差之间的关系。

根据观测方程有
$$\begin{array}{rl}u=f_x\frac{X}{Z}+c_x& \\ v=f_y\frac{Y}{Z}+c_y& \end{array}$$
以横坐标为例，若像素平面观测误差为$\delta u$个像素，设归一化平面差$\sigma$个单位
$$\begin{array}{rl}\delta u=f_x(\frac{X}{Z}+\sigma )-f_x(\frac{X}{Z})=f_x\sigma & \end{array}$$

解得
$$\begin{array}{rl}\sigma =\frac{\delta u}{f_x}& \end{array}$$
其中
$$\begin{array}{r}{f}_x=\frac{f}{dx}\end{array}$$

$f$为相机焦距，$f_x$为相机在$u$轴上的归一化焦距，$dx$为相机一个像素单元的长度。

现以NiKon D700相机为例进行求解其内参：
焦距 $f=35mm$ 最高分辨率：4256×2832 传感器尺寸：36.0×23.9 mm

根据以上定义可以有：

$$\begin{array}{r}{u}_0=4256/2=2128\\ v_0=2832/2=1416\\ dx=\frac{36.0}{4256}(mm/pixel)\\ dy=\frac{23.9}{2832}(mm/pixel)\\ f_x=f/dx=4137.8\\ f_y=f/dy=4147.3\end{array}$$

由此看来$f_x、f_y$通常是$1e^3$量级。

所以我们设归一化平面观测误差建模$\text{e}$服从$\mathcal{N}(0,\frac{\delta u}{1000})$分布。

2. 实验

仿真代码

假设第三帧和第四帧可观测到路标点P

实验结果

标准差	奇异值比例	标准差	奇异值比例
1/1000	0.0000495412	6/1000	0.0271588
2/1000	0.000788281	7/1000	0.036579
3/1000	0.00312873	8/1000	0.0472432
4/1000	0.0069819	9/1000	0.0590794
5/1000	0.0190476	10/1000	0.0720082

五、奇异值比例与观测帧数之间的关系

为了尽可能消除随机性，得到奇异值比例与观察测帧数更普遍的结论，固定标准差为1/1000，对每一个观测帧数都测试100次，奇异值比值取平均数。

代码

实验结果

可以看出比值总体呈上升趋势，因为观测帧数多，误差累积大，如一、2所示，空间中某点到各条射线的距离和将增大。