视觉SLAM十四讲|【五】相机与IMU时间戳同步

视觉SLAM十四讲|【五】相机与IMU时间戳同步

相机成像方程

Z [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] = K P Z \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}= KP Z ​uv1​ ​= ​fx​00​0fy​0​cx​cy​1​ ​ ​XYZ​ ​=KP
其中，
K = [ f x 0 c x 0 f y c y 0 0 1 ] K=\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= ​fx​00​0fy​0​cx​cy​1​ ​

时间戳同步

假设视觉特征在图像平面上匀速移动，则特征在相机成像平面上的运动速度为
$$V_l^k=(\left[\begin{array}{c}{u}_l^{k+1}\\ v_l^{k+1}\end{array}\right]-\left[\begin{array}{c}{u}_l^k\\ v_l^k\end{array}\right])/(t_{k+1}-t_k)$$
设世界坐标系中$l$个地图点坐标为
$$f_l^w=[x,y,z{]}^T$$
变换到相机坐标系下则为
$$f_l^{c_i}=R_{cb}{R}_{w{b}_i}^T(f_l^w-p_{w{b}_i})+p_{cb}$$再投影到图像平面，并计算重投影残差
$$r_c=[\frac{x_l^i}{z_l^i}-u_l^i,\frac{y_l^i}{z_l^i}-v_l^i{]}^T$$
考虑时间延迟对特征坐标的补偿为
$$z_l^i(t_d)=[u_l^i,v_l^i{]}^T+t_d{v}_l^i$$

逆深度参数化方式

SLAM中特征点的参数化表示有很多，最直接的是用三维坐标XYZ来表示，但通常大家更喜欢用逆深度表示，因为逆深度优势在于能够建模无穷远点。回顾相机成像方程
Z [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] = K P Z \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}= KP Z ​uv1​ ​= ​fx​00​0fy​0​cx​cy​1​ ​ ​XYZ​ ​=KP
有
P = 1 λ K − 1 [ u v 1 ] P=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u \\v \\ 1 \end{bmatrix} P=λ1​K−1 ​uv1​ ​
对于世界坐标系中的某相机观测点$f_l^{c_i}$，可以用相机逆深度成像公式得到，如下所示
f l c i = 1 λ K − 1 [ u l i v l i 1 ] f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix} flci​​=λ1​K−1 ​uli​vli​1​ ​
考虑到坐标系转换关系
$$f_l^w=R_{w{c}_i}{f}_l^{c_i}+p_{w{c}_i}$$
观测点$f_l^{c_i}$也可以通过运动姿态进行推测，有
$$\widetilde{f_l^{c_i}}=R_{c_{i}w}{f}_l^w+p_{c_{i}w}$$
我们研究的是投影面内的残差，因此，不考虑时间延迟的残差可以写为如下形式
r c 3 = [ u l j v l j 1 ] − λ K f l c j ~ r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K \tilde{{f_l^{c_j}}} rc3​= ​ulj​vlj​1​ ​−λKflcj​​~​
r c 3 = [ u l j v l j 1 ] − λ K ( R c j w f l w + p c j w ) r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}f_l^w + p_{c_jw}) rc3​= ​ulj​vlj​1​ ​−λK(Rcj​w​flw​+pcj​w​)
r c 3 = [ u l j v l j 1 ] − λ K ( R c j w ( R w c i f l c i + p w c i ) + p c j w ) r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}(R_{w c_i}f_l^{c_i}+p_{wc_i})+ p_{c_jw}) rc3​= ​ulj​vlj​1​ ​−λK(Rcj​w​(Rwci​​flci​​+pwci​​)+pcj​w​)
又因为
f l c i = 1 λ K − 1 [ u l i v l i 1 ] f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix} flci​​=λ1​K−1 ​uli​vli​1​ ​
r c 3 = [ u l j v l j 1 ] − λ K ( R c j w ( R w c i ( 1 λ K − 1 [ u l i v l i 1 ] ) + p w c i ) + p c j w ) r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw}) rc3​= ​ulj​vlj​1​ ​−λK(Rcj​w​(Rwci​​(λ1​K−1 ​uli​vli​1​ ​)+pwci​​)+pcj​w​)
[ u l j v l j ] = [ 1 0 0 0 1 0 ] [ u l j v l j 1 ] \begin{bmatrix} u_l^j\\ v_l^j \end{bmatrix} = \begin{bmatrix} 1 & 0 &0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} [ulj​vlj​​]=[10​01​00​] ​ulj​vlj​1​ ​
令
$$C=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$$
有
$$r_c=C{r}_{c3}$$
r c = C [ u l j v l j 1 ] − λ C K ( R c j w ( R w c i ( 1 λ K − 1 [ u l i v l i 1 ] ) + p w c i ) + p c j w ) r_{c} = C\begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda CK(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw}) rc​=C ​ulj​vlj​1​ ​−λCK(Rcj​w​(Rwci​​(λ1​K−1 ​uli​vli​1​ ​)+pwci​​)+pcj​w​)
现在考虑时间延迟
r c = C ( [ u l j v l j 1 ] + v j t d ) − λ C K ( R c j w ( R w c i ( 1 λ K − 1 [ u l i v l i 1 ] ) + p w c i ) + p c j w ) r_{c} = C(\begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} + v_jt_d) - \lambda CK(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw}) rc​=C( ​ulj​vlj​1​ ​+vj​td​)−λCK(Rcj​w​(Rwci​​(λ1​K−1 ​uli​vli​1​ ​)+pwci​​)+pcj​w​)
其中
v j = ( [ u k + 1 v k + 1 1 ] − [ u k v k 1 ] ) / ( t k + 1 − t k ) v_j =(\begin{bmatrix} u_{k+1} \\ v_{k+1} \\ 1\end{bmatrix} - \begin{bmatrix} u_{k} \\ v_{k} \\ 1\end{bmatrix})/(t_{k+1}-t_k) vj​=( ​uk+1​vk+1​1​ ​− ​uk​vk​1​ ​)/(tk+1​−tk​)