1. VINS-Fusion与KITTI数据集简介

VINS-Fusion是香港科技大学开源的多传感器融合定位框架，支持单目/双目+IMU、双目+GPS等多种组合。我在实际项目中使用它处理KITTI数据集时，发现其GPS与视觉惯性数据融合的效果令人惊喜——在KITTI 00序列测试中，轨迹误差能控制在1.2米以内（使用evo工具评估）。

KITTI数据集作为自动驾驶领域的标杆，提供了同步的立体图像、激光雷达和GPS/IMU数据。但初次接触时容易被其复杂的目录结构搞懵，这里分享我的理解：

• image_00和image_01对应左右目灰度图像（10Hz）
• oxts文件夹包含GPS/IMU原始数据（100Hz但实际有效频率为10Hz）
• velodyne_points存放激光雷达扫描数据（本文未使用）

提示：处理KITTI数据前务必检查时间戳对齐情况，我曾在时间同步问题上浪费了两天时间。

2. 环境配置与数据准备

2.1 依赖安装

实测在Ubuntu 20.04 + ROS Noetic环境下最稳定，以下是关键依赖：

sudo apt install ros-noetic-cv-bridge ros-noetic-tf ros-noetic-message-filters
pip install evo --upgrade  # 轨迹评估工具

2.2 数据预处理

KITTI的GPS数据需要转换为ENU坐标系。我改写了官方提供的转换脚本：

# gps_converter.py
from geographiclib.geodesic import Geodesic
def llh_to_enu(lat, lon, h, origin):
    geod = Geodesic.WGS84
    g = geod.Inverse(origin[0], origin[1], lat, lon)
    distance = g['s12']
    azimuth = math.radians(g['azi1'])
    east = distance * math.sin(azimuth)
    north = distance * math.cos(azimuth)
    return east, north, h - origin[2]

2.3 配置文件修改

关键参数在kitti_00-02_config.yaml中：

# 外参设置（需根据实际标定修改）
body_T_cam0: !!opencv-matrix
  rows: 4
  cols: 4
  data: [0, 0, 1, 0.5, 
        -1, 0, 0, -0.1,
         0,-1, 0, 0,
         0, 0, 0, 1]

3. 多传感器融合实战

3.1 启动流程

需要四个终端分别运行：

# 终端1: ROS核心
roscore

# 终端2: 可视化
roslaunch vins vins_rviz.launch

# 终端3: 主算法
rosrun vins kitti_gps_test \
  config/kitti_raw/kitti_00-02_config.yaml \
  /path/to/kitti/2011_10_03_drive_0027_sync/

# 终端4: 全局融合
rosrun global_fusion global_fusion_node

3.2 轨迹输出修改

默认不输出轨迹文件，需要修改globalOptNode.cpp：

// 在vio_callback函数中添加：
std::ofstream fout("/output/path/pose.txt", ios::app);
fout << pose_msg->header.stamp << " "
     << global_t.x() << " " << global_t.y() << " " << global_t.z() << " "
     << global_q.x() << " " << global_q.y() << " " << global_q.z() << " " << global_q.w() << endl;

4. 性能评估与问题排查

4.1 使用EVO评估

将结果与KITTI真值对比：

# 转换真值格式
evo_traj kitti KITTI_00_gt.txt --save_as_tum
# 计算绝对位姿误差
evo_ape tum output/pose.txt KITTI_00_gt.tum -a -p

4.2 常见问题解决

问题1：轨迹与GPS数据垂直 这是坐标系转换的正常现象，评估时两者会自动对齐。如果强行修改可能导致优化发散。

问题2：初始化失败 检查IMU数据是否正常：

# 查看IMU数据范围
awk '{print $4,$5,$6}' imu.txt | gnuplot -p -e "plot '-' with lines"

正常加速度范围应在±2g内，角速度±200°/s。

问题3：GPS融合效果差 尝试调整config.yaml中的：

gps_position_weight: 0.1  # 增大该值增强GPS权重

5. 进阶：双目+IMU+GPS融合

5.1 数据同步处理

KITTI的IMU(100Hz)与图像(10Hz)需要手动同步。我写的同步脚本核心逻辑：

double img_time = imageTimeList[i];
auto imu_it = lower_bound(imuTimeList.begin(), imuTimeList.end(), img_time);
if (imu_it != imuTimeList.end()) {
    double dt = *imu_it - img_time;
    if (dt < 0.02) {  // 20ms阈值
        processIMU(*imu_it, imuData[imu_it - imuTimeList.begin()]);
    }
}

5.2 外参标定技巧

使用Kalibr标定相机-IMU外参后，还需转换为VINS格式：

# 转换标定结果
T_cam_imu = np.loadtxt("kalibr_result.txt")
T_body_cam = np.linalg.inv(T_cam_imu)  # VINS使用body系

6. 实战经验分享

在室外大场景测试中，纯视觉里程计30秒后漂移达5米，而加入GPS后即使300米轨迹仍能保持2米内误差。但要注意：

1. GPS信号中断时，系统会自动降级为纯VIO模式
2. 隧道等场景建议增加轮速计融合
3. 城市峡谷区域可设置gps_trust_angle: false避免错误约束

我曾遇到一个棘手问题：融合后轨迹出现周期性抖动。最终发现是IMU温度漂移导致，通过添加在线bias估计解决：

estimate_imu_bias: true  # 在config中启用