以FAST_LIO为例，讲解bag数据集只录制了激光雷达和IMU话题下时，如何运用evo工具

2301_79152004

已于 2025-03-23 16:21:27 修改

阅读量867

点赞数 11

文章标签：人工智能 ubuntu bash 机器人

于 2025-03-23 16:20:03 首次发布

本文链接：https://blog.youkuaiyun.com/2301_79152004/article/details/146456354

版权

一、FAST-LIO的核心功能与输出

1. FAST-LIO是一种基于 紧耦合迭代卡尔曼滤波（iEKF） 的激光雷达-惯性里程计（LIO）框架，其核心功能包括：

直接点云配准：无需提取特征点，直接将原始点云与全局地图匹配。
增量k-d树（ikd-Tree）：高效维护动态地图，支持实时插入和删除点云。
IMU与LiDAR融合：通过反向传播补偿运动畸变，实现高精度位姿估计。

2.FAST-LIO 的ROS节点会发布以下关键话题（在lasermapping.cpp文件中）：

 /*** ROS subscribe initialization ***/
    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
        nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
        nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
    ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
    ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered", 100000);
    ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered_body", 100000);
    ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_effected", 100000);
    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
            ("/Laser_map", 100000);
    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
            ("/Odometry", 100000);
    ros::Publisher pubPath          = nh.advertise<nav_msgs::Path> 
            ("/path", 100000);

/Odometry：融合后的里程计信息（包含位姿、速度和协方差）。
/path：轨迹路径，用于可视化。
/cloud_registered：配准后的点云地图。

这些话题是算法处理传感器数据后的输出结果，可直接用于EVO评估。

二、从FAST-LIO中获取里程计数据的步骤

1. 运行FAST-LIO处理bag文件

FAST-LIO需要订阅LiDAR和IMU的原始话题，通过以下命令运行：

roslaunch fast_lio mapping_velodyne.launch
rosbag play your_bag.bag /velodyne/points:=/velodyne_points /imu/data:=/IMU_data

关键参数配置：在config/velodyne.yaml中设置LiDAR和IMU的话题名称、外参标定、噪声参数等。
输出话题：算法处理后，/Odometry会持续发布位姿信息。

在这里，rosbag play命令对话题进行了重映射，其中 /velodyne/points和/imu/data话题是FAST_LIO中config/velodyne.yaml文件对于激光雷达和IMU话题的设置，而/velodyne_points和/IMU_data是录制的bag数据集中激光雷达和IMU的话题名称。

2. 记录算法输出的里程计话题

通过ROS记录FAST-LIO的里程计话题：

rosbag record -O fastlio_odom.bag /Odometry

3. 提取轨迹数据供EVO使用

将bag文件中的里程计话题转为TUM格式：

evo_traj bag fastlio_odom.bag /odometry/imu --save_as_tum --output odometry_imu.tum

生成的文件（odometry_imu.tum）包含时间戳、位置和四元数姿态，可直接用于EVO评估。

其他关于evo工具的使用可以参考下面这篇博客，主要注意数据格式

rosbag命令 | EVO工具的使用_evo bag 绘制轨迹-优快云博客

三、FAST-LIO生成里程计的代码解析

1. 主函数流程

FAST-LIO的主函数（laserMapping.cpp）流程如下：

ROS节点初始化：读取参数（如LiDAR类型、IMU噪声协方差、外参矩阵）。
传感器数据订阅：通过回调函数获取LiDAR点云和IMU数据。
预处理与IMU积分：对点云降采样，通过IMU数据预测位姿。
状态估计：使用iEKF融合点云残差和IMU预测，更新状态。
地图更新：将配准后的点云插入ikd-Tree，维护全局地图。

2. 核心代码片段

void publish_odometry(const ros::Publisher & pubOdomAftMapped)
{
    odomAftMapped.header.frame_id = "camera_init";
    odomAftMapped.child_frame_id = "body";
    odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time);
    set_posestamp(odomAftMapped.pose);
    pubOdomAftMapped.publish(odomAftMapped);
    auto P = kf.get_P();
    for (int i = 0; i < 6; i ++)
    {
        int k = i < 3 ? i + 3 : i - 3;
        odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3);
        odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4);
        odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5);
        odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0);
        odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1);
        odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2);
    }

    static tf::TransformBroadcaster br;
    tf::Transform                   transform;
    tf::Quaternion                  q;
    transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \
                                    odomAftMapped.pose.pose.position.y, \
                                    odomAftMapped.pose.pose.position.z));
    q.setW(odomAftMapped.pose.pose.orientation.w);
    q.setX(odomAftMapped.pose.pose.orientation.x);
    q.setY(odomAftMapped.pose.pose.orientation.y);
    q.setZ(odomAftMapped.pose.pose.orientation.z);
    transform.setRotation( q );
    br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body" ) );
}

优化后的位姿数据来自于 kf（卡尔曼滤波器）的状态估计，发布为/Odometry话题。

四、EVO评估的关键注意事项

真值轨迹对齐：若需计算绝对位姿误差（APE），需提供高精度真值（如运动捕捉系统或RTK-GPS数据）。
时间同步：确保FAST-LIO的里程计时间戳与真值对齐，可通过evo_ape的-t参数调整时间偏移。
参数调优：FAST-LIO的性能受以下参数影响：
- IMU噪声协方差（gyr_cov, acc_cov）：需根据传感器标定结果调整。
- 地图分辨率（cube_len, filter_size_map）：影响ikd-Tree的构建效率。