1 简介

最近在基于GVINS进行开发，需要使用自采数据集进行测试验证；由于网上相关资料较少，导致自己也走了一些弯路，所以一方面为了给大家一个参考，另一方面也记录一下采集过程以便后续查找，所以在利用该文章来记录一下。

2 数据集采集过程

2.1 GVINS数据集介绍

GVINS数据集包含有GNSS、IMU、Camera三种传感器的数据，其中IMU和Camera数据比较常规，都是之前多次采集过的；但是对于GNSS就稍微复杂一些了，根据GVINS官方给的数据集（sports_field.bag）为例进行说明吧。

GVINS的数据集都是以rosbag的形式保存的，根据上图可以看到包含有GNSS、IMU、Camera三种传感器的数据，下面对各种数据进行说明：

消息名称	对应传感器	详情
cam0/image_view	Camera	相机0
cam1/image_view	Camera	相机1
external_trigger	-	外部触发信号
imu0	IMU	IMU数据
ublox_driver/ephem	GNSS	星历数据
ublox_driver/glo_ephem	GNSS	Glonass星历数据
ublox_driver/iono_params	GNSS	电离层参数
ublox_driver/range_meas	GNSS	原始观测数据
ublox_driver/receiver_lla	GNSS	GNSS位置解
ublox_driver/receiver_pvt	GNSS	GNSS详细位置解
ublox_driver/time_pulse_info	GNSS	PPS秒脉冲

根据以上展示可以发现，GNSS部分的数据是很复杂的，第一次接触该部分数据时由于网上资料较少，加上gnss_comm里面有相关代码（如下图所示），我还以为是要自己将之前采集到的数据手动转换为rosbag，还为此花了几天的时间来将之前采集到的数据一步步转换为对应的消息类型和rosbag，浪费了宝贵的时间，这也是目前在该数据集采集过程中遇到的最大的一个弯路。

2.2 ublox_driver使用说明

峰回路转在于发现了GVINS团队还提供了ublox_driver工具来帮助大家比较方便地采集GNSS数据并保存为rosbag格式，有了该工具，采集GNSS数据就简单得多了，在此记录一下使用过程。

2.2.1 ublox_driver介绍

根据Github页面介绍，该工具是参考UBX-18010854文档专为Ublox ZED-F9P模块设计的，可能也兼容于其他8系列或9系列的Ublox接收机。安装该工具的具体过程就不赘述了，在网页上有详细介绍，只要之前成功编译安装过VINS、GVINS，那么这个工具的安装就没有什么问题。真正需要注意的是对ZED-F9P模块进行参数设置。

2.2.2 ZED-F9P模块参数设置

ublox_driver Github页面上也说明了，在使用该工具之前需要利用u-center来对ZED-F9P模块进行设置，目前u-center只有Windows版本，所以需要在Windows上进行设置，以下为详细步骤：

打开u-center并连接接收机；
点击Tools-Receiver Configuration；
在弹出页面中选择配置文件路径，这里使用ublox_driver Github页面提供的文件即可，具体文件为config/ucenter_config_f9p_gvins.txt；填写正确路径，然后点击Transfer file -> GNSS，如果没有报错就表示正常设置了该配置文件；
然后打开View-Message View，查看对应消息是否打开；

这里需要做两件事情：第一件事是确保UBX-RXM-RAWX, UBX-RXM-SFRBX 和 UBX-NAV-PVT处于激活状态；只要上一步的配置文件设置没问题，这里就可以看到各个消息正常输出，以UBX-RXM-RAWX为例，如下图所示。这里如果运行正常的化是有数据输出的，只不过我这是在实验室没有接天线，所以没有数据输出，但是这个消息选项是点亮的（黑色，而不是灰色）。

第二件事情是将修改后的配置进行保存，还是在该Message View页面，选取UBX-CFG(Config)-CFG(Configuration)，进入该页面，确保左侧选择了``Save current configuration”，右侧选取了BBR、FLASH，然后点击左下角的Send按钮。这样刚才的配置就会在设备断电重启后继续使用该配置。

至此，即可将该模块接入Ubuntu系统中，运行以下命令来启动该工具并查看对应的topic：

sudo usermod -aG dialout $USER
source devel/setup.bash
roslaunch ublox_driver ublox_driver.launch
rostopic echo /ublox_driver/receiver_lla

2.2.3 模块固件版本不匹配问题

在2.2.2节步骤3中可能会出现固件版本不匹配的问题，即该工具提供的配置文件固件与当前使用的ublox模块的固件版本不一致，在导入配置文件时会报错：The version you are about to download does not correspond the version of the GPS receiver.。根据弹出框中的内容显示，ublox_driver提供的配置文件是基于f10c36固件版本的，网上查询之后发现该版本对应的固件版本号为HPG-1.13。在Ublox官方中可以找到并下载该固件，此处附上对应的下载链接。下载完该固件之后，在u-center中进行固件升级，如下图所示：

选取下载的固件，然后点击左下角的GO即可进行固件升级，然后继续重复上节的操作步骤即可。

2.3 数据采集

数据采集过程这里就不多赘述了，安装好相机、IMU、GNSS模块和天线之后，开启各个模块的数据采集程序，然后使用rosbag record -O ${文件名} ${ROS_topic}即可进行数据的保存。

3 数据处理过程

保存完数据之后并不是都可以直接兼容GVINS程序的，我这里在测试过程中就遇到了一些问题，在这里记录一下。

3.1 IMU消息类型不兼容

由于之前编写的IMU数据采集程序使用了自定义的数据格式，而非GVINS中使用的sensor_msgs/Imu格式（如下图所示），因此，需要对自采数据集中的IMU消息类型进行转换。

这里可以采用两种方式进行转换：方式一是将整个bag包中的IMU消息进行类型转换，然后保存为新的rosbag包；方式二是在回放rosbag时对IMU消息进行实时转换。因为包含图像的rosbag比较大，所以为了方便起见，这里采用方式二实时转换的方法。具体转换方法为使用以下python脚本进行实时转换：

#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import Imu
from EXAMPLE.msg import example_imu		# 根据自己数据集的实际消息类型进行修改

def callback(data):
    standard_imu = Imu()
    
    # 时间戳修改，后文会有说明。
    gps_time_float = data.accel_gps_time
    standard_imu.header.stamp = rospy.Time.from_sec(gps_time_float)
    standard_imu.header.frame_id = "imu_link"
	
    # 将自采数据中的加速度计、陀螺仪数值赋给standard_imu。
    # 这里的data.linear_acceleration.x、data.angular_velocity.x等需要根据自采数据集的实际情况进行修改。
    # 注意这里涉及到坐标轴的转换！
    standard_imu.linear_acceleration.x = data.linear_acceleration.x
    standard_imu.linear_acceleration.y = -data.linear_acceleration.y
    standard_imu.linear_acceleration.z = -data.linear_acceleration.z

    standard_imu.angular_velocity.x = data.angular_velocity.x
    standard_imu.angular_velocity.y = -data.angular_velocity.y
    standard_imu.angular_velocity.z = -data.angular_velocity.z

    pub.publish(standard_imu)


if __name__ == '__main__':
    rospy.init_node('imu_format_converter')

    # 根据实际情况进行修改。
    rospy.Subscriber("/EXAMPLE/EXAMPLE", example, callback)

    pub = rospy.Publisher("/imu_converted", Imu, queue_size=10)

    rospy.spin()

值得注意的是，这里运行该脚本需要基于EXAMPLE环境。

在回放rosbag数据前先使用python img_bridge.py运行该脚本即可实现对自采数据集的IMU消息进行实时转换。当然，也可以对GVINS代码进行修改，但那种方式相对会更复杂一些，我这里就没有去尝试了。

同样地，如果图像消息也与GVINS不同的话也要做对应的修改，不过这种情况应该比较少见，一般图像的保存类型都为默认的sensor_msgs/Image。

3.2 坐标轴转换

这个问题属于比较常规的问题，但是为了后续便于查找还是记录一下。GVINS遵循ROS社区的标准约定，主要涉及两个核心坐标系，IMU坐标系和相机坐标系。

IMU坐标系 (Body Frame)

作为系统的基准坐标系，GVINS默认其为”前左上“：

x轴：载体前进方向；
y轴：载体左侧；
z轴：载体上方；
重力矢量：静止平放时，加速度计测得的z轴数值应改为 $+9.8m/s^2$ 。

以我的自采数据集为例，在实验过程中IMU的坐标轴为”前右下“，在静止平放时z轴的数值为 $-9.8 m/s^2$ ；所以，我在进行IMU消息转换时也将坐标轴转换为”前左上“了，如3.1节中的代码所示。

相机坐标系 (Camera Frame)

遵循计算机视觉的传统定义”右下前“：

x轴：相机右侧；
y轴：相机下方；
z轴：相机前方（视线方向）。

由此，也可以得到从相机坐标系到IMU坐标系的标准旋转矩阵 $\mathrm{R}^b_c$ ：

$\mathrm{R}^b_c = \begin{bmatrix} 0&0&1 \\ -1&0&0 \\ 0&-1&0 \end{bmatrix}$

当然，需要根据数据采集过程中各设备的具体摆放来进行对应的坐标轴转换。

3.3 IMU和图片时间戳对齐

在处理了以上两个问题之后，我在GVINS上测试仍不成功，遵循Gemini的建议现在VINS上测试一下IMU与相机是否可以正常工作，发现仍然无法正常运行，一直在等待IMU数据（Wait IMU …），我也使用了rostopic info ${IMU_TOPIC}进行查看，发现IMU消息的发布和订阅都没有问题；进一步分析之后发现原来是时间戳不对应的关系。现在梳理一下三个传感器的时间戳以及对应yaml文件的配置。

首先，查看GVINS官方数据集中各传感器的时间戳，如下图所示，可以发现，IMU和相机的时间戳都是在header.stamp中，且是机器内部时钟（记为MCU时钟）；而GNSS数据的时间戳是在每一个卫星的观测中出现的，在time中以GPS时周数+周内秒形式记录的。这种差异是由于数据采集时没有使用时间硬同步造成的，IMU和相机只能使用MCU时间。

此外，GVINS的配置文件中关于时钟的设定为：

1
2
3

gnss_local_online_sync: 1                       # if perform online synchronization betwen GNSS and local time
local_trigger_info_topic: "/external_trigger"   # external trigger info of the local sensor, if `gnss_local_online_sync` is 1
gnss_local_time_diff: 18.0                      # difference between GNSS and local time (s), if `gnss_local_online_sync` is 0

这里设定了gnss_local_online_sync为1，即需要对GNSS时间和MCU时间进行校准。

接下来进一步观察我的自采数据集三个传感器的时间戳，如下图所示。

我在采集数据时使用了时间硬同步，所有传感器均同步到了GPS时；从上图可以看到，自采数据集中，IMU数据除了在header.stamp中记录了MCU时间外，还记录了GPS时，而相机和GNSS消息中都是记录了GPS时，所以我在3.1节的IMU消息转换代码中使用了以下代码进行时间修改以确保三个传感器的时间戳同步：

1 2	gps_time_float = data.accel_gps_time standard_imu.header.stamp = rospy.Time.from_sec(gps_time_float)

由此，三个传感器的时间戳就都同步到了GPS时，对应的yaml配置文件也修改为：

1
2
3

gnss_local_online_sync: 0                       # if perform online synchronization betwen GNSS and local time
local_trigger_info_topic: "/external_trigger"   # external trigger info of the local sensor, if `gnss_local_online_sync` is 1
gnss_local_time_diff: 0                      # difference between GNSS and local time (s), if `gnss_local_online_sync` is 0

将gnss_local_online_sync修改为0，同时把gnss_local_time_diff也设置为0，之后VINS就可以正常运行了，说明IMU和图片的时间戳是对齐成功了的，但是GVINS中还是没有GNSS的结果，需要进一步查看代码分析原因。

3.4 VINS和GNSS时间戳对齐

查看代码之后发现，没有GNSS结果输出的原因还是在于时间戳没有对齐。在函数std::vector<ObsPtr> msg2meas(const GnssMeasMsgConstPtr &gnss_meas_msg)中，每个GNSS观测消息的时间是通过obs->time = gpst2time(obs_msg.time.week, obs_msg.time.tow);定义的，而gpst2time的定义如下所示：

gtime_t gpst2time(uint32_t week, double tow)
    {
        gtime_t t = epoch2time(gpst0);
        // std::cout << "1970-1980 sec: " << t.time << std::endl;
        if (tow < -1E9 || tow > 1E9) tow = 0.0;
        t.time += 86400*7*week + (int)tow;
        t.sec = tow - (int)tow;
        return t;
    }

这里的gpst0是GPS时的起点时间，即1980-1-6日00:00:00；进一步观察epoch2time的定义：

/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args   : double *ep       I   day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
gtime_t epoch2time(const double *ep)
{
    const int doy[] = {1,32,60,91,121,152,182,213,244,274,305,335};
    gtime_t time = {0};
    int days, sec, year=(int)ep[0], mon=(int)ep[1], day=(int)ep[2];

    if (year < 1970 || year > 2099 || mon < 1 || mon > 12) return time;

    /* leap year if year%4==0 in 1901-2099 */
    days = (year-1970)*365 + (year-1969)/4 + doy[mon-1] + day-2 + (year%4==0&&mon>=3?1:0);
    sec = (int)floor(ep[5]);
    time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec;
    time.sec = ep[5] - sec;
    return time;
}

该函数作用是计算一个给定日期距离Unix操作系统计时起点（1970-1-1日00:00:00）的总时间。结合以上两个函数发现，GVINS中的GNSS观测消息的时间转换为：观测时间距离1970-1-1日00:00:00的总时间。那么要进行时间戳对齐操作的话就有两种选择，一种是改GNSS时间，一种是改VINS时间；由于GNSS时间会与后续的星历等参数相关联，因此，为了简便起见，这里就对VINS时间进行修改；类似于3.1节中的方法，这里还是采用了实时转换的方式，将IMU和图像的时间戳加上一个固定偏差；对于本数据集为1778371200，其中，gtime_t t = epoch2time(gpst0);得到的时间为315964800，再加上$86400\times7\times Week$即可得到该时间偏差。

为简单起见，这里将IMU消息转换以及IMU/图片时间戳转换放到一个脚本中进行实时转换，列下代码作为参考：

#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import Imu, Image
from byh_uav.msg import uav_imu
import numpy as np

OFFSET = 1778371200.0

def imu_callback(data):
    standard_imu = Imu()
    
    # 1778371200: Total seconds from 1970 to current week.
    gps_time_float = data.accel_gps_time + OFFSET

    standard_imu.header.stamp = rospy.Time.from_sec(gps_time_float)
    standard_imu.header.frame_id = "imu_link"

    # For 20260511 test.
    standard_imu.linear_acceleration.x = -data.linear_acceleration.y
    standard_imu.linear_acceleration.y = -data.linear_acceleration.x
    standard_imu.linear_acceleration.z = -data.linear_acceleration.z

    standard_imu.angular_velocity.x = -data.angular_velocity.y
    standard_imu.angular_velocity.y = -data.angular_velocity.x
    standard_imu.angular_velocity.z = -data.angular_velocity.z

    pub_imu.publish(standard_imu)

def cam0_callback(data):
    data.header.stamp = rospy.Time.from_sec(data.header.stamp.to_sec() + OFFSET)
    pub_cam0.publish(data)

def cam1_callback(data):
    data.header.stamp = rospy.Time.from_sec(data.header.stamp.to_sec() + OFFSET)
    pub_cam1.publish(data)


if __name__ == '__main__':
    rospy.init_node('imu_img_format_converter', anonymous=True)

    rospy.Subscriber("/EXAMPLE/EXAMPLE", example, imu_callback)
    rospy.Subscriber("/EXAMPLE/EXAMPLE", Image, cam0_callback)
    rospy.Subscriber("/EXAMPLE/EXAMPLE", Image, cam1_callback)

    pub_imu = rospy.Publisher("/imu_converted", Imu, queue_size=100)
    pub_cam0 = rospy.Publisher("/cam0_converted", Image, queue_size=10)
    pub_cam1 = rospy.Publisher("/cam1_converted", Image, queue_size=10)

    rospy.spin()

在播放自采数据集前开一个窗口运行该脚本即可，然后，GVINS终于可以成功运行了。

Echo's blog

GVINS数据集采集