视觉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 = fx000fy0cxcy1 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= fx000fy0cxcy1
时间戳同步
假设视觉特征在图像平面上匀速移动，则特征在相机成像平面上的运动速度为
V l k = ( [ u l k + 1 v l k + 1 ] − [ u l k v l k ] ) / ( t k + 1 − t k ) V_l^k =( \begin{bmatrix} u_l^{k+1} \\ v_l^{k+1} \end{bmatrix} - \begin{bmatrix} u_l^{k} \\ v_l^{k} \end{bmatrix})/(t_{k+1}-t_k) Vlk=([ulk+1vlk+1]−[ulkvlk])/(tk+1−tk)
设世界坐标系中 l l l个地图点坐标为
f l w = [ x , y , z ] T f_l^w = [x, y,z]^T flw=[x,y,z]T
变换到相机坐标系下则为
f l c i = R c b R w b i T ( f l w − p w b i ) + p c b f_l^{c_i} = R_{cb}R_{wb_i}^T(f_l^w - p_{wb_i}) + p_{cb} flci=RcbRwbiT(flw−pwbi)+pcb再投影到图像平面，并计算重投影残差
r c = [ x l i z l i − u l i , y l i z l i − v l i ] T r_c = [\frac{x_l^i}{z_l^i}-u_l^i, \frac{y_l^i}{z_l^i}-v_l^i]^T rc=[zlixli−uli,zliyli−vli]T
考虑时间延迟对特征坐标的补偿为
z l i ( t d ) = [ u l i , v l i ] T + t d v l i z_l^i(t_d) = [u_l^i, v_l^i]^T + t_d v_l^i zli(td)=[uli,vli]T+tdvli
逆深度参数化方式
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 = fx000fy0cxcy1 XYZ =KP
有
P = 1 λ K − 1 [ u v 1 ] P=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u \\v \\ 1 \end{bmatrix} P=λ1K−1 uv1
对于世界坐标系中的某相机观测点 f l c i f_l^{c_i} flci，可以用相机逆深度成像公式得到，如下所示
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=λ1K−1 ulivli1
考虑到坐标系转换关系
f l w = R w c i f l c i + p w c i f_l^w = R_{wc_i}f_l^{c_i}+p_{wc_i} flw=Rwciflci+pwci
观测点 f l c i f_l^{c_i} flci也可以通过运动姿态进行推测，有
f l c i ~ = R c i w f l w + p c i w \tilde{{f_l^{c_i}}} = R_{c_iw}f_l^w+p_{c_iw} flci~=Rciwflw+pciw
我们研究的是投影面内的残差，因此，不考虑时间延迟的残差可以写为如下形式
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= uljvlj1 −λ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= uljvlj1 −λK(Rcjwflw+pcjw)
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= uljvlj1 −λK(Rcjw(Rwciflci+pwci)+pcjw)
又因为
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=λ1K−1 ulivli1
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= uljvlj1 −λK(Rcjw(Rwci(λ1K−1 ulivli1 )+pwci)+pcjw)
[ 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} [uljvlj]=[100100] uljvlj1
令
C = [ 1 0 0 0 1 0 ] C = \begin{bmatrix} 1 & 0 &0 \\ 0 & 1 & 0 \end{bmatrix} C=[100100]
有
r c = C r c 3 r_c = C r_{c3} rc=Crc3
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 uljvlj1 −λCK(Rcjw(Rwci(λ1K−1 ulivli1 )+pwci)+pcjw)
现在考虑时间延迟
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( uljvlj1 +vjtd)−λCK(Rcjw(Rwci(λ1K−1 ulivli1 )+pwci)+pcjw)
其中
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+1vk+11 − ukvk1 )/(tk+1−tk)