ARTICLE · INTELLIGENCE

战地情报 · 详情页

来自尧图项目组的一线实战观察与深度解析

VINS-Fusion 移植 ROS2 Jazzy 踩坑(二):数据与评测

VINS-Fusion 移植 ROS2 Jazzy 踩坑(二):数据与评测

上一篇解决了编译期的问题。本篇走完剩下的链路:EuRoC 的 .bag 进 Jazzy、轨迹输出、真值对齐、算 ATE。这段路的坑大多不报错。环境同上一篇:宿主机 Ubuntu 22.04(8C8G),ROS2 Jazzy 跑在 Docker 里,底座 zinuok/VINS-Fusion-ROS2(Humble)。仓库:stronghasaki/VINS-ROS2_jazzy。坑编号接上一篇,从坑 5 开始。

坑 5:Jazzy 没有 rosbag1,EuRoC 的 .bag 播不了

现象:ros2 bag play MH_01_easy.bag报错不识别;apt 源里也没有 rosbag1 的包可装。

根因:ROS2 从 Jazzy 起彻底移除 rosbag1 支持(此前的 rosbag_v2 兼容包只出到 Humble)。EuRoC 官方发布的是 2016 年的 ROS1 .bag。

解法:不装 ROS1,用纯 Python 的 rosbags 库转格式:

pip3installrosbags rosbags-convert--srcMH_01_easy.bag--dstMH_01_easy_ros2

转换后先ros2 bag info核对 /cam0 /cam1 /imu0 三个 topic 的消息数与原 bag 一致。

原理:.bag 是 ROS1 自定义格式,rosbag2 是 sqlite3 + CDR。rosbags 库直接解析 ROS1 序列化字节并重写为 CDR,全程不需要 ROS1 环境。

坑 6:只挂载 install/ 时,launch 静默不启动

现象:Docker 里ros2 launch vins euroc.launch.py无任何报错,但输出目录里的轨迹文件是上一次的旧数据。

根因:编译用了colcon build --symlink-install,此时install/vins/lib/vins/vins_node是指向/ws/build/...的符号链接。容器只挂载了 install/,链接目标不存在;launch 对"可执行文件不存在"的处理是静默跳过该节点。

解法:挂载成对出现:

dockerrun...-v$PWD/../build:/ws/build-v$PWD/../install:/ws/install...

排查命令:ls -la install/vins/lib/vins/看链接指向。

原理:symlink-install 把构建产物和安装产物绑在一起,容器挂载、打包、跨机拷贝时任一单独存在都是坏的。要么成对携带,要么构建时不用 --symlink-install。

坑 7:上游 vio.csv 时间戳被截成整数秒,路径不存在时静默失败

现象:evo 按--t_max_diff 0.02对齐时全部匹配失败;更早还发现输出文件根本没生成,VINS 也不报错。

根因:两个问题叠加,都在上游写 vio.csv 的代码里。一是foutC.precision(0)把时间戳截成整数秒(如 1403636584),真值是纳秒精度,0.02s 的匹配窗口下必然错位;二是配置里写~/output/时 C++ 的 ofstream 不展开 ~,目录不存在时静默失败。

解法:本仓库把输出改成标准 TUM 格式(t tx ty tz qx qy qz qw,微秒精度时间戳),读配置处加了展开 ~ + mkdir -p 兜底。

坑 8:真值四元数列序,EuRoC 是 wxyz,TUM 是 xyzw,一列之差旋转评测全毁

现象:自写的真值转换脚本转出 TUM 后,evo_ape -va警告invalid (must be unit quaternions)。诡异的是平移 ATE 看着完全正常(0.118 m,与 ROS1 原版同数量级),旋转评测(-r angle_deg)的误差却是 66° 量级,全程无报错。

根因:官方state_groundtruth_estimate0/data.csv的实际格式是第 1 行#注释头 + 逗号分隔,列序$1=timestamp, $2..4=px py pz, $5=qw $6=qx $7=qy $8=qz, $9..=vx vy vz,四元数 wxyz,第 9 列开始是线速度。转换脚本两层错误:一是把$6 $7 $8 $9当作 xyzw 输出——实际把 vx(线速度)当成了 qw,“四元数"模长 0.846;二是注释里假设"无逗号、3 行注释头”,对官方文件用 awk 默认空白分隔会把整行吞成 $1,字段全空——这个脚本在官方真值上根本跑不出结果。

awk 'NR>3 {printf "%.9f %s %s %s %s %s %s %s\n", $1/1e9, $2, $3, $4, $6, $7, $8, $9}' data.csv

看起来是跳过 qw 取 xyz,实际把 vx(线速度)当成了 qw 塞进 TUM 第 8 列。正确写法是$6 $7 $8 $5。

file_interface.py 的 read_tum_trajectory_file 读入后只做列 roll(xyzw→wxyz),不归一化、不校验模长;模长校验只在 check() 里。

evo 的平移 APE 和 Umeyama 对齐只用位置坐标,四元数列不进这条路径。A/B 实测两种列序的平移 RMSE 逐位相同(0.1176 m)。四元数只在旋转评测和一切用到真值姿态的场景生效——用修正真值是 mean 2.77° / max 5.46°,用错误真值是 mean 66.3° / max 155.3°。评测数字被污染的方向取决于该指标用到数据的哪部分。

解法:修正列序($6 $7 $8 $5),-F','加首字段纯数字判数据行,并加模长自检——转换时验证数据首行四元数模长,偏离 1 直接报错退出:

# 自检:真值数据首行 (qw,qx,qy,qz) 模长应在 1 附近 awk -F',' 'NR==2 && $1 ~ /^[0-9]+$/ { q=sqrt($5*$5+$6*$6+$7*$7+$8*$8); if (q<0.9 || q>1.1) {print "FATAL: quaternion norm " q ", column layout mismatch" > "/dev/stderr"; exit 1} }' "$DS" # TUM 的 qw 取真值的 $5 awk -F',' '$1 ~ /^[0-9]+$/ {printf "%.9f %s %s %s %s %s %s %s\n", $1/1e9, $2, $3, $4, $6, $7, $8, $5}' "$DS" > "$gt_tum"

完整脚本见仓库 scripts/eval_euroc.sh。

需要交代的事:上一篇给出的 ATE 0.118 m 出自这套有问题的评测链路,当时不可复现。已按修正脚本用官方真值重测:平移 ATE RMSE 0.1176 m(mean 0.101 / max 0.322),与原发布一致(原因如上,四元数列不影响平移指标);旋转 APE mean 2.77° / max 5.46°。README 结果表已更新并补了旋转指标。

原理:SLAM 评测有两套流传最广的四元数约定,TUM(xyzw)和 ASL/EuRoC(wxyz,ROS 的 geometry_msgs 也是 wxyz),两套约定并行十几年。

坑 9:evo 装不上

容器内:Jazzy 镜像基于 noble(Python 3.12),pip3 install evo被 externally-managed-environment 拦截。镜像内一次性安装加 --break-system-packages 即可:

pip3install--break-system-packages --ignore-installed evo

宿主机:Ubuntu 22.04 上pip3 install --user evo后运行报:

ValueError: numpy.dtype size changed, may indicate binary incompatibility. Expected 96 from C header, got 88

根因是 22.04 系统 scipy 针对 numpy 1.x 编译,pip 拉进了 numpy 2.x,~/.local 优先级高于系统包,numpy 换新、scipy 没换。解法pip3 install --user --upgrade scipy(1.15+ 的 wheel 兼容 numpy 2.x)。

evo_res -p先报ImportError: cannot import name 'docstring' from 'matplotlib'——pip 的 matplotlib 3.10 wheel 不再带 mpl_toolkits/init.py(命名空间包),apt 装在 /usr/lib 的 mpl_toolkits 是有init.py 的常规包,Python 的 import 规则是常规包优先于命名空间包,mplot3d 就一直解析到系统旧版和新版 matplotlib 拼不上;给 ~/.local 的 mpl_toolkits 补空init.py 不可靠。修完这层还报Failed to import any of the following Qt binding modules——evo 的 ~/.evo/settings.json 里 plot_backend 默认 qtagg,服务器没 Qt,改成 agg 即可。

原理:numpy.dtype size changed 是 numpy 1.x/2.x ABI 断裂的标志性报错。评测环境要么全装容器里,要么全用 venv(22.04 先 apt install python3.10-venv)。

修正后的完整流程

# 转换(坑 5)rosbags-convert MH_01_easy.bag--dstMH_01_easy_ros2# metadata 补丁:rosbags 转出的 metadata 是旧 schema,不补的话# ros2 bag play 直接报 yaml-cpp bad conversioncdMH_01_easy_ros2cpmetadata.yaml metadata.yaml.orig-rosbagssed-e"s|offered_qos_profiles: ''|offered_qos_profiles: []|"metadata.yaml.orig-rosbags\|sed's|\( serialization_format: cdr\)|\1\n type_description_hash: ""|'>metadata.yamlcd- ros2 bag info MH_01_easy_ros2# 核对消息数,能正常输出才往下走# 跑(坑 6:build/ 和 install/ 成对挂载)# src 必须挂到 /ws/src/src:install/ 内 launch/config 软链接指向 /ws/src/src/**,# 挂成 /ws/src 会报 FileNotFoundErrordockerrun--rm--networkhost\-v$PWD/../src:/ws/src/src-v$PWD/../build:/ws/build-v$PWD/../install:/ws/install\-v/path/to/MH_01_easy_ros2:/bags:ro-v$HOME/vins_out:/root\vins-jazzy-basebash-c"source /opt/ros/jazzy/setup.bash && \ source /ws/install/local_setup.bash && \ (ros2 launch vins euroc.launch.py &) && sleep 8 && \ ros2 bag play /bags && sleep 30"# 评测(坑 7/8/9:TUM 输出 + 带列序自检的 eval_euroc.sh)./scripts/eval_euroc.sh$HOME/vins_out/output/vio.csv

eval_euroc.sh 会生成 gt_mh01.tum 和 vio.csv.ape.zip,多次实验用evo_res *.ape.zip汇总对比——回环开关、参数改动、ROS1 vs ROS2。

下一篇

开回环 pose_graph 的对比、实机 RealSense 的 QoS 与时间戳问题、MH_02–MH_05 基线。大概率写实机篇。

仓库:https://github.com/stronghasaki/VINS-ROS2_jazzy (Docker 一键复现 + 评测脚本)

RELATED READING

延伸阅读

更多一线实战笔记与深度复盘,助您持续精进