VINS-Mono+Fusion源码解析系列（十九）：视觉重投影约束

最新推荐文章于 2024-03-04 01:39:18 发布

LDST_CSDN

最新推荐文章于 2024-03-04 01:39:18 发布

阅读量513

点赞数 1

文章标签：自动驾驶计算机视觉人工智能

本文链接：https://blog.csdn.net/LDST_CSDN/article/details/130674865

版权

本文详细解析了VINS-Mono中视觉重投影的理论与实践，包括从第i帧到第j帧的转换过程，以及重投影误差的计算。重点介绍了为何采用逆深度来描述3D点，并探讨了视觉重投影误差对待优化量的雅可比矩阵的计算方法。

摘要由CSDN通过智能技术生成

1. VINS-Mono中的视觉重投影

（1）理论分析

如下图所示，由于VINS-Mono中滑窗里面维护的位姿是IMU坐标系下的位姿，因此对于当前第 $i$ 帧中的特征点，一方面先通过逆深度投影到第 $i$ 帧的相机归一化坐标系中，然后通过相机与IMU之间的外参投影到第 $i$ 帧IMU坐标系中，再通过IMU坐标系之间的转换得到它在第 $j$ 帧IMU坐标系中的投影（实际上是先转换到世界坐标系中，然后再从世界坐标系转换到第 $j$ 帧IMU坐标系中），然后通过相机与IMU之间的外参将其转换到第 $j$ 帧相机坐标系中，最后通过逆深度投影到第 $j$ 帧归一化坐标系上。另一方面，通过光流追踪法可获得第 $i$ 帧归一化坐标系中的特征点在第 $j$ 帧归一化坐标系中的位置，通过这两个点可计算出重投影误差。
在这里插入图片描述
通过以上分析可知，VINS-Mono中视觉重投影约束有：第 $i$ 帧和第 $j$ 帧的(IMU)位姿，相机与IMU之间的外参以及3D地图点的逆深度。

上述转换过程可通过如下公式进行描述：
$\left[\begin{array}{cc} x_{c_j} \\ y_{c_j} \\ z_{c_j} \\ 1 \end{array} \right] = T_{bc}^{-1}T_{wb_j}^{-1}T_{wb_i}T_{bc}\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right] \ \ \ \ \ \ \ \ \ \ 第i帧归一化坐标系 \ \ \rightarrow \ \ 第j帧相机坐标系$
其中， $u_i,v_i)$ 是通过特征点提取或追踪以及去畸变得到的第 $i$ 帧归一化坐标系下的特征点坐标， $\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]$ 表示第 $i$ 帧的相机坐标系下的三维空间点， $\frac{1}{\lambda}$ 为对应的逆深度， $\left[\begin{array}{cc} x_{c_j} \\ y_{c_j} \\ z_{c_j} \\ 1 \end{array} \right]$ 表示第 $j$ 帧的相机坐标系下的三维空间点。
上述公式的具体含义如下：

$T_{bc}\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]$ 表示将第 $i$ 帧相机坐标系下的三维空间点转换到第 $i$ 帧IMU的body坐标系下， $T_{wb_i}T_{bc}\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]$ 表示第 $i$ 帧对应的body坐标系下的三维空间点转换到世界坐标系下， $T_{wb_j}^{-1}T_{wb_i}T_{bc}\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]$ 表示第 $i$ 帧对应的世界坐标系下的三维空间点转换到第 $j$ 帧body坐标系下， $T_{bc}^{-1}T_{wb_j}^{-1}T_{wb_i}T_{bc}\left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]$ 表示第 $j$ 帧body坐标系下的点转换到第 $j$ 帧相机坐标系下，由此实现将第 $i$ 帧相机坐标系下的点转换到第 $j$ 帧相机坐标系下。另外，优化的状态量是在IMU的body坐标系下的。
简而言之，先将第 $i$ 帧的相机坐标系下转换到body坐标系下，然后body坐标系之间有一个相互转换（从第 $i$ 帧转换到第 $j$ 帧，其中要转换到世界坐标系下作为中转），最后再从body坐标系下转换到第 $j$ 帧相机坐标系下。

此时（第 $j$ 帧），基于逆深度的重投影误差（归一化平面上的估计值减去观测值）为：
$r_c=\left[\begin{array}{cc} \frac{x_{c_j}}{z_{c_j}}-u_{c_j} \\ \frac{y_{c_j}}{z_{c_j}}-v_{c_j} \end{array} \right]$
其中， $x_{c_j},y_{c_j},z_{c_j})$ 为经过上述公式估计得到的第 $j$ 帧相机坐标系下的三维空间点坐标， $u_{c_j},v_{c_j})$ 表示在第 $j$ 帧下观测到的（通过特征提取或光流追踪以及去畸变）相机归一化平面上的点坐标。

将上述公式使用旋转和平移表示，具体过程如下：
$T_{bc}^{-1}T_{wb_j}^{-1}T_{wb_i}T_{bc} = \left[\begin{array}{cc} R_{bc}^T & -R_{bc}^Tt_{bc} \\ 0 & 1\end{array} \right] \left[\begin{array}{cc} R_{wb_j}^T & -R_{wb_j}^Tt_{wb_j} \\ 0 & 1\end{array} \right] \left[\begin{array}{cc} R_{wb_i} & t_{wb_i} \\ 0 & 1\end{array} \right] \left[\begin{array}{cc} R_{bc} & t_{bc} \\ 0 & 1\end{array} \right] \ \ \ \ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \left[\begin{array}{cc} R_{bc}^TR_{wb_j}^T & -R_{bc}^TR_{wb_j}^Tt_{wb_j}-R_{bc}^Tt_{bc} \\ 0 & 1\end{array} \right] \left[\begin{array}{cc} R_{wb_i}R_{bc} & R_{wb_i}t_{bc}+t_{wb_i} \\ 0 & 1\end{array} \right] \ \ \ \ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \left[\begin{array}{cc} R_{bc}^TR_{wb_j}^TR_{wb_i}R_{bc} & R_{bc}^TR_{wb_j}^T(R_{wb_i}t_{bc}+t_{wb_i})-R_{bc}^TR_{wb_j}^Tt_{wb_j}-R_{bc}^Tt_{bc} \\ 0 & 1\end{array} \right] \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \left[\begin{array}{cc} R_{bc}^TR_{wb_j}^TR_{wb_i}R_{bc} & R_{bc}^T[R_{wb_j}^T(R_{wb_i}t_{bc}+t_{wb_i}-t_{wb_j})-t_{bc}] \\ 0 & 1\end{array} \right] \\ \left[\begin{array}{cc} x_{c_j} \\ y_{c_j} \\ z_{c_j} \end{array} \right] = R_{bc}^TR_{wb_j}^TR_{wb_i}R_{bc}p_{c_i} + R_{bc}^T[R_{wb_j}^T(R_{wb_i}t_{bc}+t_{wb_i}-t_{wb_j})-t_{bc}]$