1. VINS-Mono中的视觉重投影
（1）理论分析
如下图所示，由于VINS-Mono中滑窗里面维护的位姿是IMU坐标系下的位姿，因此对于当前第 i i i帧中的特征点，一方面先通过逆深度投影到第 i i i帧的相机归一化坐标系中，然后通过相机与IMU之间的外参投影到第 i i i帧IMU坐标系中，再通过IMU坐标系之间的转换得到它在第 j j j帧IMU坐标系中的投影（实际上是先转换到世界坐标系中，然后再从世界坐标系转换到第 j j j帧IMU坐标系中），然后通过相机与IMU之间的外参将其转换到第 j j j帧相机坐标系中，最后通过逆深度投影到第 j j j帧归一化坐标系上。另一方面，通过光流追踪法可获得第 i i i帧归一化坐标系中的特征点在第 j j j帧归一化坐标系中的位置，通过这两个点可计算出重投影误差。
通过以上分析可知，VINS-Mono中视觉重投影约束有：第 i i i帧和第 j j j帧的(IMU)位姿，相机与IMU之间的外参以及3D地图点的逆深度。
上述转换过程可通过如下公式进行描述：
[ x c j y c j z c j 1 ] = T b c − 1 T w b j − 1 T w b i T b c [ 1 λ u c i 1 λ v c i 1 λ 1 ] 第 i 帧归一化坐标系 → 第 j 帧相机坐标系 \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帧相机坐标系
xcjycjzcj1
=Tbc−1Twbj−1TwbiTbc
λ1uciλ1vciλ11
第i帧归一化坐标系 → 第j帧相机坐标系
其中， ( u i , v i ) (u_i,v_i) (ui,vi)是通过特征点提取或追踪以及去畸变得到的第 i i i帧归一化坐标系下的特征点坐标， [ 1 λ u c i 1 λ v c i 1 λ 1 ] \left[\begin{array}{cc} \frac{1}{\lambda}u_{c_i} \\ \frac{1}{\lambda}v_{c_i} \\ \frac{1}{\lambda} \\ 1 \end{array} \right]
λ1uciλ1vciλ11
表示第 i i i帧的相机坐标系下的三维空间点， 1 λ \frac{1}{\lambda} λ1为对应的逆深度， [ x c j y c j z c j 1 ] \left[\begin{array}{cc} x_{c_j} \\ y_{c_j} \\ z_{c_j} \\ 1 \end{array} \right]
xcjycjzcj1
表示第 j j j帧的相机坐标系下的三维空间点。
上述公式的具体含义如下：
T b c [ 1 λ u c i 1 λ v c i 1 λ 1 ] 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] Tbc
λ1uciλ1vciλ11
表示将第 i i i帧相机坐标系下的三维空间点转换到第 i i i帧IMU的body坐标系下， T w b i T b c [ 1 λ u c i 1 λ v c i 1 λ 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] TwbiTbc
λ1uciλ1vciλ11
表示第 i i i帧对应的body坐标系下的三维空间点转换到世界坐标系下， T w b j − 1 T w b i T b c [ 1 λ u c i 1 λ v c i 1 λ 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] Twbj−1TwbiTbc
λ1uciλ1vciλ11
表示第 i i i帧对应的世界坐标系下的三维空间点转换到第 j j j帧body坐标系下， T b c − 1 T w b j − 1 T w b i T b c [ 1 λ u c i 1 λ v c i 1 λ 1 ] 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] Tbc−1Twbj−1TwbiTbc
λ1uciλ1vciλ11
表示第 j j j帧body坐标系下的点转换到第 j j j帧相机坐标系下，由此实现将第 i i i帧相机坐标系下的点转换到第 j j j帧相机坐标系下。另外，优化的状态量是在IMU的body坐标系下的。
简而言之，先将第 i i i帧的相机坐标系下转换到body坐标系下，然后body坐标系之间有一个相互转换（从第 i i i帧转换到第 j j j帧，其中要转换到世界坐标系下作为中转），最后再从body坐标系下转换到第 j j j帧相机坐标系下。
此时（第 j j j帧），基于逆深度的重投影误差（归一化平面上的估计值 减去 观测值）为：
r c = [ x c j z c j − u c j y c j z c j − v c 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] rc=
zcjxcj−ucjzcjycj−vcj
其中， ( x c j , y c j , z c j ) (x_{c_j},y_{c_j},z_{c_j}) (xcj,ycj,zcj)为经过上述公式估计得到的第 j j j帧相机坐标系下的三维空间点坐标， ( u c j , v c j ) (u_{c_j},v_{c_j}) (ucj,vcj)表示在第 j j j帧下观测到的（通过特征提取或光流追踪以及去畸变）相机归一化平面上的点坐标。
将上述公式使用旋转和平移表示，具体过程如下：
T b c − 1 T w b j − 1 T w b i T b c = [ R b c T − R b c T t b c 0 1 ] [ R w b j T − R w b j T t w b j 0 1 ] [ R w b i t w b i 0 1 ] [ R b c t b c 0 1 ] = [ R b c T R w b j T − R b c T R w b j T t w b j − R b c T t b c 0 1 ] [ R w b i R b c R w b i t b c + t w b i 0 1 ] = [ R b c T R w b j T R w b i R b c R b c T R w b j T ( R w b i t b c + t w b i ) − R b c T R w b j T t w b j − R b c T t b c 0 1 ] = [ R b c T R w b j T R w b i R b c R b c T [ R w b j T ( R w b i t b c + t w b i − t w b j ) − t b c ] 0 1 ] [ x c j y c j z c j ] = R b c T R w b j T R w b i R b c p c i + R b c T [ R w b j T ( R w b i t b c + t w b i − t w b j ) − t b c ] 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}] Tbc−1Twbj−1TwbiTbc=[RbcT0−RbcTtbc1][RwbjT0−RwbjTtwbj1
最低0.47元/天 解锁文章
502
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
