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

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

相机成像方程

$$Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=KP$$
其中，
$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

时间戳同步

假设视觉特征在图像平面上匀速移动，则特征在相机成像平面上的运动速度为
$$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\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=KP$$
有
$$P=\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$
对于世界坐标系中的某相机观测点$f_l^{c_i}$，可以用相机逆深度成像公式得到，如下所示
$$f_l^{c_i}=\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}{u}_l^i\\ v_l^i\\ 1\end{array}\right]$$
考虑到坐标系转换关系
$$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_{c3}=\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]-\lambda K\widetilde{f_l^{c_j}}$$
$$r_{c3}=\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]-\lambda K(R_{c_{j}w}{f}_l^w+p_{c_{j}w})$$
$$r_{c3}=\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]-\lambda K(R_{c_{j}w}(R_{w{c}_i}{f}_l^{c_i}+p_{w{c}_i})+p_{c_{j}w})$$
又因为
$$f_l^{c_i}=\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}{u}_l^i\\ v_l^i\\ 1\end{array}\right]$$
$$r_{c3}=\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]-\lambda K(R_{c_{j}w}(R_{w{c}_i}(\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}{u}_l^i\\ v_l^i\\ 1\end{array}\right])+p_{w{c}_i})+p_{c_{j}w})$$
$$\left[\begin{array}{c}{u}_l^j\\ v_l^j\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]$$
令
$$C=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$$
有
$$r_c=C{r}_{c3}$$
$$r_c=C\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]-\lambda CK(R_{c_{j}w}(R_{w{c}_i}(\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}{u}_l^i\\ v_l^i\\ 1\end{array}\right])+p_{w{c}_i})+p_{c_{j}w})$$
现在考虑时间延迟
$$r_c=C(\left[\begin{array}{c}{u}_l^j\\ v_l^j\\ 1\end{array}\right]+v_j{t}_d)-\lambda CK(R_{c_{j}w}(R_{w{c}_i}(\frac{1}{\lambda }{K}^{-1}\left[\begin{array}{c}{u}_l^i\\ v_l^i\\ 1\end{array}\right])+p_{w{c}_i})+p_{c_{j}w})$$
其中
$$v_j=(\left[\begin{array}{c}{u}_{k+1}\\ v_{k+1}\\ 1\end{array}\right]-\left[\begin{array}{c}{u}_k\\ v_k\\ 1\end{array}\right])/(t_{k+1}-t_k)$$