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

最新推荐文章于 2025-04-02 00:00:00 发布

影子鱼Alexios

最新推荐文章于 2025-04-02 00:00:00 发布

阅读量5.8k

点赞数 21

文章标签：机器人机器学习

本文链接：https://blog.csdn.net/qq_43443531/article/details/135514803

版权

本文详细介绍了视觉SLAM中的关键概念，包括相机与惯性测量单元（IMU）的时间戳同步方法，以及如何通过逆深度参数化处理相机成像方程中的运动和延迟效应。着重讨论了特征点在不同坐标系下的转换和重投影残差的计算，以及考虑时间延迟后的误差补偿过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

视觉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)$