Extrinsic Online Calibration on AirSLAM

15 Jan 2025

---

AirSLAM 在公开数据集上的效果很不错，但在笔者自己录制的数据集（螺旋快速运动）上效果很差，具体如下：

• AirSLAM 使用 imu 和相机，定位漂移很大
• 而相同的外参在 vins-fusion 上的效果却还不错。
• AirSLAM 仅使用双目，漂移小，较为正常

根据第一条和第三条信息，可以推断出是 imu 和相机之间的协调出了问题，可能的原因是:

• 时间基准不同：公开数据集的 imu 与 图像帧有硬件同步且参考同一个时间基准，而自己录制的数据集未作同步且不是同一个时间基准，而 vins-fusion 在运行过程中在线校准 imu 和图像帧的时间偏移。这是二者存在的一个较为明显的区别，
• 相机与 imu 之间的外参不够准确：虽然 vins-fusion 和 AirSLAM 使用的外参一致，但 vins 有对外参做在线估计校准，而 AirSLAM 没有。

在 为 AirSLAM 增加在线时间校准 中，已经对比了增加在时间校准前后的精度，得到的结论是 时间基准不同不是引起该问题的原因。因此本文将验证第二种可能性：相机与 imu 之间的外参不够准确。

验证的方法是，为 AirSLAM 增加外参在线校准，对比开启该功能与关闭该功能的定位精度，如果提升明显，则说明确实是相机与 imu 之间的外参不准确导致的较大漂移。

结论是，正是外参不准确且没有在线校准，导致了 AirSLAM 在自己录制的快速运动的数据集中出现大幅度漂移。

原理

状态的定义

假设机器人配置多个相机和一个 IMU。

\[\begin{align} \mathcal{X} = [& \mathrm{x}_0, \mathrm{x}_1, \cdots, \mathrm{x}_{n-1}, \\ & L_0, L_1, \cdots, L_{l-1}, \\ & \mathrm{x}_{c_0}, \mathrm{x}_{c_1}, \cdots,\mathrm{x}_{c_{m-1}}] \end{align}\]

其中 $\mathrm{x}_i,\ i = [0, n)$ 是大小为 $n$ 的滑动窗口内的核心状态，包括机体相对于世界系的位置、速度、姿态，以及加速度计和陀螺仪的零偏

\[\mathrm{x}_i = [p_i^w, v_i^w, R_i^w, b_a, b_g]\]

$L_j, j= [0, l)$ 是滑窗内被历史观测二次及以上的路标，如果使用 3D 表示，则 $L_j = [x_j, y_j, z_j]^T$，如果使用逆深度表示，则 $L_j = \lambda_j$。

$x_{c_k}$ 是在线校准的状态，包括第 k 个相机相对于 imu 的位置、姿态、时间偏移。

\[\mathrm{x}_{c_k} = [p_{c_k}^w, R_{c_k}^w, t_{d_k}]\]

构建残差函数

定义这些状态变量后，结合测量值，可构建联合的残差函数：

\[\mathbf{r}(\mathrm{z}, \mathcal{X}) = \left\| \mathbf{e}_p - \mathbf{H}_p \mathcal{X} \right\|^2 + \sum_{i \in \mathcal{B}} \left\| \mathbf{e}_{\mathcal{B}}(\mathrm{z}_i^{i+1}, \mathcal{X}) \right\|_{\mathbf{P}_i^{i+1}}^2 + \sum_{(l,j) \in \mathcal{O}} \left\| \mathbf{e}_{\mathcal{C}}(\mathrm{z}_l^j, \mathcal{X}) \right\|^2_{\mathbf{P}_l^j} + \sum_{k \in \mathcal{C}} \left\| \mathbf{e}_{\mathrm{ext}}(z_{c_k}, \mathrm{x}_{c_k}) \right\|^2_{\mathbf{P}_{c_k}}\]

其中：

• $\left| \mathbf{e}_p - \mathbf{H}_p \mathcal{X} \right|^2$ 是先验因子

• $\sum_{i \in \mathcal{B}} \left| \mathbf{e}{\mathcal{B}}(\mathrm{z}_i^{i+1}, \mathcal{X}) \right|{\mathbf{P}_i^{i+1}}^2$ 是 IMU propagation 因子

• $\sum_{(l,j) \in \mathcal{O}} \left| \mathbf{e}{\mathcal{C}}(\mathrm{z}_l^j, \mathcal{X}) \right|^2{\mathbf{P}_l^j}$ 是视觉重投影因子

• 本篇中最重要的因子则是 外参先验因子：$\sum_{k \in \mathcal{C}} \left| \mathbf{e}{\mathrm{ext}}(z{c_k}, \mathrm{x}{c_k}) \right|^2{\mathbf{P}_{c_k}}$

涉及到的集合：

• $\mathcal{B}$ 是滑动窗口内的关键帧的 index 集合

• $\mathcal{O}$ 是滑动窗口内路标 index 与 观测到该路标的关键帧 index 对 $(l,j)$ 的集合

• $\mathcal{C}$ 是所有相机的编号的集合

我们的目标是：

\[\mathcal{X}^* = \mathrm{argmin}_{\mathcal{X}}\ \mathbf{r}(\mathrm{z}, \mathcal{X})\]

VINS-Fusion 的残差构造满足上述描述，而 AirSLAM 则需对视觉重投影因子稍作修改。因为在 VINS 中，路标点被滑动窗口内的至少两帧观测到，才会被加入滑窗中；而在 AirSLAM 中，一个路标点同样需要被两帧不同关键帧观测，但只要求至少有一帧在滑窗中即可，其余的观测帧可不位于滑窗内，此时我们可以把视觉因子修改为：

\[\sum_{(l,j) \in \mathcal{C}} \left\| \mathbf{e}_{\mathcal{C}}(\mathrm{z}_l^j, \mathcal{X}_{old}, \mathcal{X}) \right\|^2_{\mathbf{P}_l^j}\]

其中，$\mathcal{X}_{old}$ 是滑窗外的关键帧，作为参数而不是被优化变量，出现在因子当中。此时 下标 $j$ 不再局限于滑窗内的关键帧下标了，也可能是滑窗外不被优化的关键帧下标。

一些因子的具体形式

在本文中，我们关注相机相对于 imu 的位置和姿态，需要把 $\sum_{(l,j) \in \mathcal{C}} \left| \mathbf{e}{\mathcal{C}}(\mathrm{z}_l^j, \mathcal{X}{old}, \mathcal{X}) \right|^2_{\mathbf{P}_l^j} $ 具体化才能作进一步的分析。该残差具体可以写为：

\[\mathbf{e}_{\mathcal{C}}(\mathrm{z}_l^j, \mathcal{X}_{old}, \mathcal{X}) = \mathrm{z}_l^j - \pi \left(R_j^{wT} (L_l - p_j^w) \right)\]

其中，$\pi (\cdot)$ 是相机的投影函数。

实施

对 AirSLAM 增加了外参在线校准，提交在 Commit 0dc8f6d: add online calibration of extrinsic between camera and imu.。

AirSLAM 与 ORB-SLAM 类似，使用 G2O 库作为后端优化的框架。因此，为了增加所述功能，我们需要增加顶点和边，并修改一些现有的边：

• 增加的顶点是 外参 状态变量
• 增加的边是 外参先验
• 修改的边是 视觉重投影，从二元边变成了多元边

增加的核心文件及内容

增加的文件为（外参顶点、外参先验边的定义和实现，可参考已有的代码或 g2o 手册）：

• src/g2o_optimization/edge_extrinsic_prior.cc
• include/g2o_optimization/edge_extrinsic_prior.h
• src/g2o_optimization/vertex_extrinsic.cc
• include/g2o_optimization/vertex_extrinsic.h

提交在上个 commit，但在本次 commit 中使用到的（用到了新定义的 g2o 多元边的功能）：

• src/g2o_optimization/edge_project_point_td.cc
• include/g2o_optimization/edge_project_point_td.h

修改的核心文件及内容

修改的文件：

• src/g2o_optimization/g2o_optimization.cc

主要的修改内容：

• 增加 外参顶点 的初始化
• 视觉重投影边 由二元变成多元，增加了与 外参顶点 的关联
• 将 外参顶点 的优化结果更新到数据结构 camera_list[0] 中，这样其他位置调用到的外参才是实时更新的
• 使用更新后的 外参顶点 和 IMU位姿顶点 来计算 相机相对于世界系的位姿（$R_{wc},\ p_{wc}$），导出到数据结构 MapofPoses poses 中

效果

使用的数据集为 快速螺旋运动的数据集。

对比结果：

是否使用外参在线估计	定位漂移量 [m]
是	0.42
否	5.67

意外收获

外参先验因子很重要，其协方差的调节也很重要，影响到优化结果的一致性或可观性。如果完全放开，则优化结果将不断陷入局部最小值，直观体现为轨迹不断出现折线；若完全锁死，则等价与不在线优化，此时如果外参不准确，将带来巨大漂移。

Realization of Online Temporal Calibration on AirSLAM

15 Jan 2025

---

AirSLAM 在公开数据集上的效果很不错，但在笔者自己录制的数据集上效果很差，具体如下：

• AirSLAM 使用 imu 和相机，定位漂移很大
• 而相同的外参在 vins-fusion 上的效果却还不错。
• AirSLAM 仅使用双目，漂移小，较为正常

根据第一条和第三条信息，可以推断出是 imu 和相机之间的协调出了问题，可能的原因是:

• 时间基准不同：公开数据集的 imu 与 图像帧有硬件同步且参考同一个时间基准，而自己录制的数据集未作同步且不是同一个时间基准，而 vins-fusion 在运行过程中在线校准 imu 和图像帧的时间偏移。这是二者存在的一个较为明显的区别，
• 相机与 imu 之间的外参不够准确：虽然 vins-fusion 和 AirSLAM 使用的外参一致，但 vins 有对外参做在线估计校准，而 AirSLAM 没有。

本篇主要验证是否是未进行时间偏移估计导致的 AirSLAM 效果较差。笔者在 airslam 上增加了时间偏移估计，思路参考 vins-fusion，github 上 fork 的仓库内对应的提交为 Commit b16d3ea: add time composition between camera and imu data.。

最终的实验结果证明，自己的数据集中，相机的时间基准比 imu 的时间基准快 1.6 ms，影响较小，即使不进行在线时间偏移估计，也不会导致轨迹的大幅度漂移，因此导致 AirSLAM 在自己录制的数据集上漂移较大的并不是时间基准偏移，而是另有其因（相机与 imu 之间的外参不够准确，且无在线校准）。

如何将时间偏移加入到优化框架中

对于多个传感器时间戳的质量，可以分成三类：

• 既同步，又有相同时间基准
• 不同步，但有相同时间基准
• 既不同步，又无相同时间基准

其中，无相同的时间基准对 VIO 或 SLAM 系统的影响最大

接下来我们需要搞清楚，是哪些数据组合喂给了前端：

• 接收到一帧新图像
• 根据这帧图像，以及上一帧图像的时间戳，收集在这段时间内的 imu 数据

如果存在时间基准的偏移，那么我们就会把图像相对于自身时间基准的时间戳，当成是相对于 imu 时间基准的时间戳，然后收集打包两个时间戳之间的 imu 数据

根据下图可以看到:

• (1) 应当对应的 imu 数据，时间区间为 $[t_2, t_4]$
• (2) 实际打包给前端的 imu 数据，时间区间未 $[t_1, t_3]$

二者不再是同一个区间。在统一的时间基准下（以 imu 的时间基准为参考），图像帧对应的时间区间是 $[t_2, t_4]$，而打包的 imu 却是在 $[t_1, t_3]$

 td
|->|
   |----x-----x----->
        |<--->|      (1)
|--ooooooooooooo->
     |<--->|         (2) 
|----x-----x----->
|----t1-t2-t3-t4---->

解决思路的核心是用现有的信息预测其他时间的数据，类似插值，之后将修正后的数据带入原有的优化表达式，由于修正的数据的表达式中含有时间差，因此优化器会对 时间差 做优化，从而估计出时间差。

那什么是现有的数据呢？$t_2$、$t_4$ 两帧图像中的特征点位置，以及它们的速度就是现有的信息。需要预测的便是 $t_1$、$t_3$ 时刻的特征点位置。

某个投影约束的表达式为：

\[r_i(Z,X) = z_i - \pi(T_{cw}\ ^wp)\]

其中，约束的 id 是 $i$，特征点位置为 $z_i$，$pi$ 把相机系下的 3D 点投影到像素或归一化平面上。

对特征点进行修正后，代入残差表达式：

\[r_i(Z,X) = (z_i - V_i t_d ) - \pi(T_{cw}\ ^wp)\]

其中，特征点的运动速度为 $V_i$。在 $t_d$ 时间内特征点被假设为匀速运动。

这样我们便得到了最初版本的，包含时间偏移的残差表达式。但这还不是最终用在程序里的表达式。

最值得思索的是:

• 用已有的特征点去估计 imu batch 对应的起始或结束时刻的特征点位置

• 在应用的时候是以迭代的形式使用，使得算法能够处理较大时间偏移，因为匀速运动的假设会随着迭代的进行愈加正确。

\[r_i(Z,X) = (z_i - V_i (t_d - t_d') ) - \pi(T_{cw}\ ^wp)\]

实验

给同步的数据集人为增加时间偏移，作为真值。使用的数据集是 MH_05_difficult.bag。

对数据集中 imu 的时间戳偏移不同程度，并分别运行里程计。里程计漂移在本文定义为，里程计给出的终点位置与终点真值的距离。时间偏移估计值的初始值均设置为 0 ms。结果如下：

实际时间基准偏移 [s]	偏移估计值 [s]	里程计漂移(w) [m]	里程计漂移(w/o) [m]
0.030	0.035	0.095	0.382
0.060	0.068	0.363	1.523
0.100	0.108	0.614	2.867

下图展示了 100 ms 偏移情况下，时间偏移估计值是如何收敛到真实偏移附近的：

需要注意的是，在 rosbag 中，有两个和时间相关的量，一个是和 topic、msg 同级的 t，另一个是在消息内部的 msg.header.stamp。二者是不同的，t 表示的是在 rosbag 中消息被记录或者被发布的时刻，而 msg.header.stamp 才是传感器数据的时间戳。

下面是将已作时间同步的数据集人为进行偏移一定时间的程序：

#!/usr/bin/env python          
                               
import rosbag                  
import rospy
from std_msgs.msg import Header

# Time offset (30ms)
time_offset = 0.10  # 30ms = 0.03s

def adjust_timestamp_in_rosbag(input_bag_file, output_bag_file, topic_name):
    # Open the original bag file and the new bag file
    with rosbag.Bag(input_bag_file, 'r') as in_bag, rosbag.Bag(output_bag_file, 'w') as out_bag:
        for topic, msg, t in in_bag.read_messages():
            # If it's the specified topic, modify the timestamp
            if topic == topic_name:         
                new_time = msg.header.stamp + rospy.Duration(time_offset)
                # Update the timestamp in the message (if the message contains a Header)
                if hasattr(msg, 'header') and hasattr(msg, 'header'):
                    msg.header.stamp = new_time     
                else:
                    rospy.logwarn(f"Message {topic} does not have a Header, skipping timestamp adjustment")

            # Write the modified message to the new bag file
            out_bag.write(topic, msg, t)    

        rospy.loginfo(f"Timestamps have been offset by {time_offset} seconds. Output file: {output_bag_file}")

if __name__ == '__main__':
    # Specify the input and output bag file paths, and the topic name to adjust
    input_bag_file = '../rosbags/MH_05_difficult.bag'  # Input ROS bag file
    output_bag_file = '../rosbags/' + 'MH_05_difficult_offset' + str(int(time_offset*1000)) + "ms.bag" # Output ROS bag file
    topic_name = '/imu0'  # The topic name to adjust timestamps for

    # Call the function to adjust the timestamps
    adjust_timestamp_in_rosbag(input_bag_file, output_bag_file, topic_name)

其他测试

为了直观地观察 imu 和图像时间戳的分布情况，写了一个脚本将其绘制出来。

MH_05_difficult.bag 的相机和 imu 同步做的很好：

对 MH_05_difficult.bag 中的 imu 时间戳增加 3 ms：

自己搭建的 fast-drone-xi35 相机和 imu 未作同步，时间偏移量倒是不大，约为 1.6 ms。

python 脚本：

#!/usr/bin/env python

import rosbag
import matplotlib.pyplot as plt
from collections import defaultdict
from std_msgs.msg import Header

def visualize_rosbag_timestamps(bag_file):
    """
    Visualize timestamps of all topics in a ROS bag file using msg.header.stamp.
    """
    # Dictionary to store timestamps for each topic
    topic_timestamps = defaultdict(list)

    # Read the bag file
    with rosbag.Bag(bag_file, 'r') as bag:
        for topic, msg, t in bag.read_messages():
            # Check if the message has a header with stamp field
            if hasattr(msg, 'header') and hasattr(msg.header, 'stamp'):
                # Convert the header.stamp (ros::Time) to seconds
                timestamp = msg.header.stamp.to_sec()
                # Store the timestamp for the topic
                topic_timestamps[topic].append(timestamp)
            else:
                # If no header.stamp, use the default timestamp (from rosbag)
                topic_timestamps[topic].append(t.to_sec())

    # Plot timestamps for each topic
    plt.figure(figsize=(10, 6))
    for topic, timestamps in topic_timestamps.items():
        plt.plot(timestamps, [topic] * len(timestamps), 'o', label=topic)

    # Configure the plot
    plt.title("Timestamps of Messages in ROS Bag (Using header.stamp)")
    plt.xlabel("Time (seconds)")
    plt.ylabel("Topics")
    plt.yticks(range(len(topic_timestamps)), topic_timestamps.keys())
    plt.legend(loc='upper right', fontsize='small')
    plt.grid(True)
    plt.tight_layout()

    # Show the plot
    plt.show()


if __name__ == '__main__':
    # Specify the ROS bag file to analyze
    bag_file = '../rosbags/MH_05_difficult_offset3ms.bag'  # Replace with your ROS bag file path

    # Visualize timestamps
    visualize_rosbag_timestamps(bag_file)

PL-CVIO: Point-Line Cooperative Visual-Inertial Odometry

01 Dec 2024

---

PL-CVIO: Point-Line Cooperative Visual-Inertial Odometry

使用了点和线的多机协同里程计

Total views. cowboys. Hits