视觉SLAM十四讲|【六】基于特征匀速模型的重投影误差计算形式

视觉SLAM十四讲|【六】基于特征匀速模型的重投影误差计算形式

基本推导方法

无时间戳延迟时，残差计算流程：
世界坐标系中的第$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}$$
观察上式，易知转换的过程为先从世界坐标系转换到body坐标系，再从body坐标系转换到相机坐标系

再投影到图像平面，并计算重投影残差：
$$r_c=[x_l^i/z_l^i-u_l^i,y_l^i/z_l^i-v_l^i{]}^T$$
$u_l^i,v_l^i$为相机成像像素位置。
考虑时间戳延迟时，特征坐标进行补偿
$$Z_l^i(t_d)=[u_l^i,v_l^i{]}^T+t_d{V}_l^i$$
其中
$$V_l^k=([u_l^{k+1},v_l^{k+1}{]}^T-[u_l^k,v_l^k{]}^T)/(t_{k+1}-t_k)$$

当 vins 中对特征采用逆深度参数时，基于特征匀速模型的重投影误差计算形式

回顾相机投影矩阵，我们有
[ u v 1 ] = [ − f x 0 u 0 0 f y v 0 0 0 1 ] [ x / z y / z 1 ] \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} -f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x/z \\ y/z \\ 1 \end{bmatrix} ​uv1​ ​= ​−fx​00​0fy​0​u0​v0​1​ ​ ​x/zy/z1​ ​
令 [ − f x 0 u 0 0 f y v 0 0 0 1 ] = K \begin{bmatrix} -f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{bmatrix}=K ​−fx​00​0fy​0​u0​v0​1​ ​=K, [ u v 1 ] = p c \begin{bmatrix} u \\v \\1 \end{bmatrix}=p^c ​uv1​ ​=pc, [ x y z ] = p w \begin{bmatrix} x \\ y \\ z \end{bmatrix}=p^w ​xyz​ ​=pw, $z=\frac{1}{\lambda }$
有
$$p_c=\frac{1}{\lambda }{K}^{-1}{p}^c$$
其中。$p^c$为特征点在相机归一化平面内的成像坐标。$p_c$为相机坐标系下特征点的坐标。

将观测$Z_i$用相机内参转换到相机坐标系下

标记$i$时刻对特征点的观测$Z_i$在相机坐标系下的坐标值为$p_c^i$
$$p_c^i=\frac{1}{\lambda }{K}^{-1}{p}_i^c$$

根据相机位姿，反推观测点$Z$在世界坐标系下的位置

$$p_b^i=R_{bc}{p}_c^i+t_{bc}$$
$$p_w^i=R_{wb}^i{p}_b^i+t_{wb}^i$$
$$p_w^i=R_{wb}^i(R_{bc}{p}_c^i+t_{bc})+t_{wb}^i$$
从$i$时刻到$j$时刻，观测点$Z$在相机坐标系下的位置如下
$$p_w^i=p_w^j$$
$$p_c^j=R_{cb}{p}_b^j+t_{cb}$$
$$p_b^j=R_{bw}^j{p}_w^j+t_{bw}^j$$
$$p_c^j=R_{cb}(R_{bw}^j{p}_w^j+t_{bw}^j)+t_{cb}$$
由此可得在相机成像坐标系下的坐标
$$p_j^c=\lambda K{p}_c^j$$
$$p_j^c=\lambda K{R}_{cb}(R_{bw}^j{p}_w^j+t_{bw}^j)+t_{cb}$$
$$p_j^c=\lambda K{R}_{cb}(R_{bw}^j{p}_w^i+t_{bw}^j)+t_{cb}$$
有因为
$$p_w^i=R_{wb}^i(R_{bc}{p}_c^i+t_{bc})+t_{wb}^i$$
$$p_j^c=\lambda K{R}_{cb}(R_{bw}^j(R_{wb}^i(R_{bc}{p}_c^i+t_{bc})+t_{wb}^i)+t_{bw}^j)+t_{cb}$$
其中
$$p_c^i=\frac{1}{\lambda }{K}^{-1}{p}_i^c$$
$$p_j^c=\lambda K{R}_{cb}(R_{bw}^j(R_{wb}^i(R_{bc}（\frac{1}{\lambda }{K}^{-1}{p}_i^c）+t_{bc})+t_{wb}^i)+t_{bw}^j)+t_{cb}$$
残差为
r = [ 1 0 0 0 1 0 ] ( p j c − [ u j v j 1 ] ) r = \begin{bmatrix}1 & 0 & 0\\ 0 & 1 & 0 \end{bmatrix} (p_j^c -\begin{bmatrix} u_j \\ v_j \\ 1 \end{bmatrix}) r=[10​01​00​](pjc​− ​uj​vj​1​ ​)
令$\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]=D$
r = D ( p j c − [ u j v j 1 ] ) r = D(p_j^c -\begin{bmatrix} u_j \\ v_j \\ 1 \end{bmatrix}) r=D(pjc​− ​uj​vj​1​ ​)
考虑时间戳延迟时，特征坐标进行补偿
$$Z_l^j(t_d)=[u_l^j,v_l^j{]}^T+t_d{V}_l^j$$
其中
$$V_l^k=([u_l^{k+1},v_l^{k+1}{]}^T-[u_l^k,v_l^k{]}^T)/(t_{k+1}-t_k)$$
$$r=D{p}_j^c-Z_l^j(t_d)$$