使用slam评估工具evo评估fast-lio2精度

运行fast-lio2后，先输入下面的代码准备记录/Odometry

rostopic echo -p /Odometry > odometry_data.csv

然后再播包开始记录数据，记录结束后直接Ctrl+c结束即可。
此时需要将odometry_data.csv转化为tum格式的数据，运行下面Python程序进行转换

import csv

def convert_to_tum(input_file, output_file):
    with open(input_file, 'r') as infile, open(output_file, 'w') as outfile:
        reader = csv.reader(infile)

        # 跳过首行标题
        next(reader)

        # 读取第一行并提取首个时间戳
        first_row = next(reader)
        first_timestamp_ns = float(first_row[0])
        first_timestamp_std = first_timestamp_ns / 1e9
        first_timestamp_s = first_timestamp_std - first_timestamp_std
        print("timestamp0 = ", first_timestamp_s)
        tx = float(first_row[5])  # 第6列：位置 x
        ty = float(first_row[6])  # 第7列：位置 y
        tz = float(first_row[7])  # 第8列：位置 z
        qx = float(first_row[8])  # 第9列：四元数 x
        qy = float(first_row[9])  # 第10列：四元数 y
        qz = float(first_row[11])  # 第12列：四元数 z
        qw = float(first_row[12])  # 第13列：四元数 w
        outfile.write(f"{first_timestamp_s} {tx} {ty} {tz} {qx} {qy} {qz} {qw}\n")

        for row in reader:
            # 解析每一列的数据
            timestamp = float(row[0])  # 从第0列获取时间戳
            timestamp = timestamp / 1e9
            # print("timestamp = ", timestamp)
            timestamp = timestamp - first_timestamp_std

            tx = float(row[5])  # 第6列：位置 x
            ty = float(row[6])  # 第7列：位置 y
            tz = float(row[7])  # 第8列：位置 z
            qx = float(row[8])  # 第9列：四元数 x
            qy = float(row[9])  # 第10列：四元数 y
            qz = float(row[11])  # 第11列：四元数 z
            qw = float(row[12])  # 第12列：四元数 w

            # 写入TUM格式：时间戳 tx ty tz qx qy qz qw
            outfile.write(f"{timestamp} {tx} {ty} {tz} {qx} {qy} {qz} {qw}\n")

if __name__ == "__main__":
    input_file0 = "/home/lj/lj_ws/src/FAST_LIO/PCD/odometry_voxel_data.txt"  # 从rostopic保存的文件
    output_file0 = "/home/lj/lj_ws/src/FAST_LIO/PCD/trajectory_voxel.txt"    # 生成的TUM格式文件

    convert_to_tum(input_file0, output_file0)

***此处我记得好像也需要把groundtruth值转化为tum格式的数据，就是下面的estTraj.txt需要时tum格式的数据

绘制对比图像命令

evo_traj tum estTraj.txt --ref realTraj.txt -p -a

打印和真实参数对比命令

evo_ape tum estTraj.txt realTraj.txt -a