lidar_static_map_benchmark

这是一个独立的 ROS 2 BAG 离线横向基准仓库，用同一份注册点云、位姿、地图坐标系、 canonical footprint、时序静态证据和 PGM 栅格规则比较四条完整处理链：

baseline_ransac：固定阈值逐帧平面 RANSAC 基线。
geometry_huber：局部几何粗筛加 Huber IRLS 地面拟合。
prosac_magsac：质量排序、渐进采样和连续 MAGSAC 风格平面评分。
dbscan_cauchy：非地面候选 DBSCAN、map 帧跨帧关联和 Cauchy IRLS 兜底。

核心算法是 C++17/Eigen/yaml-cpp，ROS 和 matplotlib 不进入核心类。Python 只负责 ROS 2 BAG 读取、缓存、时序栅格、文件输出、可视化、参数编排和报告。没有 ROS 2 BAG 时可以用合成缓存完成构建、测试和报告冒烟验证。

算法边界

Huber 和 Cauchy 是鲁棒损失，不是动态过滤；它们只降低离群点对地面模型的影响。只有 DBSCAN 聚类经过 map frame 的跨帧中心/包围盒关联、位移、速度和驻留时间判断后，才会把满足条件的点标记为动态。PROSAC 的优势依赖候选点质量排序；法向量、平面度与连续性由几何栅格的 3×3 局部协方差估计，不要求缓存包含预算法向量字段。

prosac_magsac 是通用三维平面适配层，不是把普通 RANSAC 改名。当前实现为八个噪声尺度边际化的连续 MAGSAC 风格评分、置信度提前停止和 Eigen 加权精化，尚不是原论文 MAGSAC++ 的完整 sigma-consensus 边际化、置信度更新和优化最小求解器。详细范围见 docs/magsac_scope.md。各模块输入、输出、坐标变换责任和非目标见 docs/modules.md。

这些方法都有明确边界：baseline 不能区分跨帧动态物体；geometry_huber 不能把鲁棒损失当作动态检测，也不能修复错误的位姿或错误地面坐标系；prosac_magsac 对候选质量、噪声尺度和时间预算敏感，不能替代真实传感器标定；dbscan_cauchy 不能仅凭单帧簇名判断动态，稀疏、遮挡或簇合并也可能造成关联失败。四条链都不做导航安全判定、不发布 ROS 速度，地图结果必须由后续 Nav2/安全链继续验证。

安装与构建

Ubuntu 22.04、C++17、CMake、Eigen3、yaml-cpp（安装 PCL 时会自动链接，可选）、Python 3.10+ 和 Python 包 PyYAML numpy matplotlib psutil 是默认路径；BAG 模式还需要已 source 的 ROS 2 Humble、rosbag2_py、rclpy、rosidl_runtime_py 和 sensor_msgs_py。pandas 在 requirements.txt 中列为交互分析依赖，runner 的稳定写出路径使用标准库 CSV。

./scripts/setup.sh
./scripts/build.sh
./scripts/test.sh

Release 和 Debug 构建分别位于 build/release、build/debug，正式性能测试应使用 Release。脚本不会安装系统包，也不会下载第三方库。

BAG 与话题配置

把标准 ROS 2 rosbag2 目录放到 data/bags/。该目录中的 BAG 被 .gitignore 忽略，不会复制、修改或覆盖。SQLite3 默认可用；MCAP 由当前 ROS 2 storage plugin 决定。 ROS 1 单文件 .bag 会明确报错，需先转换。所有话题、frame、时间窗口、点云字段和位姿最大时间误差都在 configs/datasets/greenhouse_mid360.yaml 中配置，没有绝对用户路径。

裁减仍保持 rosbag2 的原始 storage 格式，并复制窗口内的全部话题；源 BAG 只读且不会被覆盖。可以按 BAG 时间戳指定窗口，或根据 odometry 话题自动删除开头的静止段。自动模式使用滚动 1 秒位移超过 5 cm 且持续 3 秒的判据，并默认保留运动起点前 2 秒作为缓冲；这些判据和实际计数会写入输出目录的 crop_manifest.yaml。

source /opt/ros/humble/setup.bash
PYTHONPATH=python:$PYTHONPATH python3 tools/crop_bag.py \
  --bag <source_rosbag2_directory> \
  --output data/bags/<cropped_bag_name> \
  --auto-start-motion

裁减后的目录可以直接交给检查器、缓存构建器和 end-to-end runner；不要把 ROS 1 单文件 .bag 传给该工具。

检查 BAG：

python3 tools/inspect_bag.py --bag data/bags/<bag_name>

检查器输出 storage 格式、时间范围、话题、类型、数量、频率、点云字段、frame_id、 odometry/OccupancyGrid/TF 是否存在和推荐 YAML。构建共享缓存：

python3 tools/build_frame_cache.py \
  --bag data/bags/<bag_name> \
  --dataset-config configs/datasets/greenhouse_mid360.yaml

注册点云必须已经在配置 map frame 中；如果 frame_id 不一致，适配器会停止并报告，不会静默套用未经确认的 TF。BAG 缓存使用两遍只读扫描：第一遍读取 odometry 和累计地图，第二遍逐云解码，并在线性平移与四元数 SLERP 后生成 map_to_base。未来 ROS 2 在线节点可复用 C++ GroundObstaclePipeline 接口，但在线节点不在当前阶段实现。

运行

真实 BAG 的完整比较：

python3 tools/run_benchmark.py \
  --bag data/bags/<bag_name> \
  --experiment configs/experiments/greenhouse_mid360.yaml \
  --mode end_to_end

可以用 BAG 的绝对时间戳限制 end-to-end 窗口；窗口参数会进入解析配置和缓存 metadata：

python3 tools/run_benchmark.py \
  --bag data/bags/<bag_name> \
  --experiment configs/experiments/greenhouse_mid360.yaml \
  --mode end_to_end --start-time <unix_seconds> --end-time <unix_seconds>

算法模式只读取同一缓存，不把 BAG 解码和 ROS 反序列化时间混入算法耗时：

python3 tools/run_benchmark.py \
  --cache data/cache/greenhouse_mid360/frames.bin \
  --experiment configs/experiments/greenhouse_mid360.yaml \
  --mode algorithm

可视化开启和关闭是两个独立的实验状态，状态会写入 manifest.json 和 performance_mode.json。正式算法性能应使用关闭逐帧可视化的运行：

python3 tools/run_benchmark.py --cache data/cache/greenhouse_mid360/frames.bin \
  --experiment configs/experiments/greenhouse_mid360.yaml --mode algorithm \
  --visualization off

开启可视化的运行用于 debug 图片和 UI 观察，不能与关闭可视化的性能数字混写。

验证较长 BAG 的输入、坐标和地图产物时，可以临时限制处理帧数；该限制会写入解析后的配置和 manifest，不应把这种结果当作完整 BAG 验收：

python3 tools/run_benchmark.py --cache data/cache/greenhouse_mid360/frames.bin \
  --experiment configs/experiments/greenhouse_mid360.yaml --mode algorithm \
  --visualization off --maximum-frames 100

单算法和参数扫描：

python3 tools/run_single.py --cache data/cache/greenhouse_mid360/frames.bin \
  --pipeline geometry_huber --config configs/pipelines/geometry_huber.yaml
python3 tools/run_parameter_sweep.py --bag data/bags/<bag_name> \
  --experiment configs/experiments/parameter_sweep.yaml
python3 tools/generate_report.py --results results/<experiment>/<run_id>

对已裁减的运动 BAG 做分段横向比较时，使用独立进程运行每个算法，避免跨算法保留大量原始点结果。每个窗口只构建一次缓存，四条链共享该缓存和地图快照。先用一个 30 秒窗口验证资源上限，再运行连续 60 秒窗口；分段 PGM 仅可做窗口内比较，不能直接拼接为全局地图。

source /opt/ros/humble/setup.bash
PYTHONPATH=python:$PYTHONPATH python3 tools/run_segmented_comparison.py \
  --bag data/bags/<motion_trimmed_bag> \
  --start-time <bag_start_seconds> --end-time <bag_end_seconds> \
  --segment-seconds 30 --max-windows 1 --plan-id <pilot_id>

正式运行去掉 --max-windows 1 并改用 --segment-seconds 60。计划目录中的 plan.json、comparison.csv、每算法日志和结果目录保留缓存哈希、窗口边界和原始汇总。为有效运动窗口生成地图和聚合延迟的图像对比表：

PYTHONPATH=python:$PYTHONPATH python3 tools/generate_segmented_visual_report.py \
  --plan results/segmented/<plan_id>/plan.json

基于同一计划生成逐帧性能曲线、阶段耗时堆叠图和算法原理图：

PYTHONPATH=python:$PYTHONPATH python3 tools/generate_segmented_performance_report.py \
  --plan results/segmented/<plan_id>/plan.json

输出 performance_overhead_curves.png 和 algorithm_principles.png。前者使用全窗口逐帧实测耗时，后者按当前 C++ 实现和共享时序栅格后端说明各算法边界。

扫描使用固定随机种子，默认对每个单参数取值运行全部 5 个种子。--maximum-runs 只用于缩短探索性运行；截断运行不满足完整首轮扫描验收。

没有真实 BAG 时：

python3 tools/generate_synthetic.py --frames 40
python3 tools/run_benchmark.py --cache data/cache/synthetic/frames.bin \
  --experiment configs/experiments/smoke_test.yaml --mode algorithm

输出

运行目录为 results/<experiment>/<run_id>/，包含 manifest.json、解析后的配置、 bag_inspection.json、四个算法目录、comparison/、performance/、summary.csv、 summary.md 和完全离线的 report.html。每个算法目录包含：

map_raw_obstacles.pgm、map_temporal.pgm、map_temporal_swept.pgm、map_final.pgm 及 map_final.yaml，并提供 <algorithm>.pgm/.yaml 同内容入口；
固定 occupied 黑、free 白、unknown 灰的 map_final_preview.png 和 map_layers.png；
occupancy_confidence.png、observation_count.png、observation_span.png、viewpoint_count.png；
frame_metrics.csv、runtime_summary.json、runtime_summary.csv、含高度均值/方差的 temporal_evidence.csv 和受 stride 限制的 debug 帧。
map_diagnostics.json，记录地图边界、时序证据、完整轨迹数量和越界障碍候选。

PGM 是 ROS map_server 可读的 P5 三值图：occupied=0、free=254、unknown=205。若 BAG 含 OccupancyGrid，源 occupied 会被单独保存在 baseline_octomap_occupied.pgm，方法图只允许当前方法通过时序证据重新写 occupied。没有 baseline 时，完整 odometry 轨迹会以真实 canonical 多边形栅格化 swept footprint，不能用一个圆代替车辆 footprint。

性能汇总包括 P50/P90/P95/P99、最大帧延迟、FPS、RSS、CPU、50/80/100 ms 超时帧，图表位于 performance/。当前 runner 的 CPU/RSS 是宿主 Python 控制进程的采样，正式硬件对比仍应固定线程数量并单独记录机器状态。

实时性参考与已知限制

MID360 10 Hz、单帧 100 ms、P50<30 ms、P95<50 ms、P99<80 ms、maximum<100 ms 只是第一轮工程参考目标，不是已验证结论。真实验收还需要目标 BAG、固定 Release 构建、独立重复实验和硬件环境记录。

当前近似和非目标包括：未接入原始 ROS 在线节点；局部协方差法向会在极稀疏栅格边缘退回中性质量分；MAGSAC 为明确记录的多尺度风格化适配；DBSCAN 使用带距离自适应半径的空间哈希邻域而不是 PCL KD-tree；BAG 适配器要求注册点云已经在 map frame；地图指标是几何和运行时指标，不代表导航闭环安全性。算法失败逐帧保留在 CSV，绝不静默退化到另一条链。

带 baseline OccupancyGrid 的 BAG 会使用请求时间窗口结束前的最新累计栅格，避免把启动阶段的小地图错误地用于后续运动点云。缓存同时保存窗口内所有 odometry 位姿，用于完整 canonical footprint swept 清除。若障碍候选全部落在选定地图边界之外，运行会失败并提示检查地图快照与点云坐标，而不是生成空的 occupied 层。

参考目录

项目只创建于当前目录下的 lidar_static_map_benchmark/，不修改同级的 agt_navigation_v2 或其他参考仓库。车辆 geometry 只存在本仓库的实验配置中；后续若接入项目运行时，应改为读取经批准的 canonical platform profile，而不是复制第二份安全几何。

Name		Name	Last commit message	Last commit date
configs		configs
data		data
docs		docs
include/lidar_static_map_benchmark		include/lidar_static_map_benchmark
python/lidar_static_map_benchmark		python/lidar_static_map_benchmark
results		results
scripts		scripts
src		src
tests		tests
tools		tools
.gitignore		.gitignore
CMakeLists.txt		CMakeLists.txt
LICENSE		LICENSE
README.md		README.md
requirements.txt		requirements.txt

Navigation Menu

Repository navigation

About

Repository files navigation

lidar_static_map_benchmark

算法边界

安装与构建

BAG 与话题配置

运行

输出

实时性参考与已知限制

参考目录

About

Releases

Packages

Contributors

Languages

Latest commit

History

Folders and files

Repository files navigation

lidar_static_map_benchmark

算法边界

安装与构建

BAG 与话题配置

运行

输出

实时性参考与已知限制

参考目录

About

Resources

Stars

Watchers

Forks

Releases

Packages

Contributors

Languages