视觉SLAM十四讲|【五】相机与IMU时间戳同步-Toy模板网

这篇具有很好参考价值的文章主要介绍了视觉SLAM十四讲|【五】相机与IMU时间戳同步。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

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

相机成像方程

$\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$
其中，
$K=\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

时间戳同步

假设视觉特征在图像平面上匀速移动，则特征在相机成像平面上的运动速度为
$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)$
设世界坐标系中 $l$ 个地图点坐标为
$f_l^w = [x, y,z]^T$
变换到相机坐标系下则为
$f_l^{c_i} = R_{cb}R_{wb_i}^T(f_l^w - p_{wb_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来表示，但通常大家更喜欢用逆深度表示，因为逆深度优势在于能够建模无穷远点。回顾相机成像方程
$\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$
有
$P=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u \\v \\ 1 \end{bmatrix}$
对于世界坐标系中的某相机观测点 $f_l^{c_i}$ ，可以用相机逆深度成像公式得到，如下所示
$f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix}$
考虑到坐标系转换关系
$f_l^w = R_{wc_i}f_l^{c_i}+p_{wc_i}$
观测点 $f_l^{c_i}$ 也可以通过运动姿态进行推测，有
$\tilde{{f_l^{c_i}}} = R_{c_iw}f_l^w+p_{c_iw}$
我们研究的是投影面内的残差，因此，不考虑时间延迟的残差可以写为如下形式
$r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K \tilde{{f_l^{c_j}}}$
$r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}f_l^w + p_{c_jw})$
$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})$
又因为
$f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix}$
$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})$
$\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}$
令
$\begin{bmatrix} 1 & 0 &0 \\ 0 & 1 & 0 \end{bmatrix}$
有
$r_c = C r_{c3}$
$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})$
现在考虑时间延迟
$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})$
其中
$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)$ 文章来源地址https://www.toymoban.com/news/detail-808079.html