# 大规模 LiDAR 建图融合

## 项目概述

本项目实现了一个针对大规模 LiDAR 建图数据的融合框架。主要解决 SLAM 算法（如 Fast-LIVO2）生成的多个独立子图（Submaps）在全局坐标系下的对齐与一致性问题。

本方案利用 GNSS 先验（模拟或真实）进行轨迹级粗配准，结合 ICP 精配准，最终通过构建位姿图并在 GTSAM 中求解，实现全局一致的地图融合。

> **状态**: 阶段性研究原型。目前在HKisland03拆分的4张图上进行了测试。

## 方法

融合策略包含三个主要阶段：

1. **粗配准 (Coarse Alignment)**:
* 利用 **Kabsch 算法 (SVD)** 计算局部里程计轨迹与全局 GNSS 轨迹之间的刚性变换 。
* 解决初始位姿估计及坐标系对齐问题。


2. **精配准 (Fine Registration)**:
* 基于质心距离门控 (Distance Gating) 和体素重叠检测筛选相邻子图。
* 使用 **Point-to-Plane ICP** 优化子图间的相对位姿。


3. **全局优化 (Global Optimization)**:
* 构建因子图 (Factor Graph)，包含 **一元因子 (Unary Factors, GNSS Priors)** 和 **二元因子 (Binary Factors, ICP Constraints)**。
* 利用 **GTSAM (iSAM2 / Levenberg-Marquardt)** 求解全局最优位姿。



## 项目结构 (Structure)

```text
.
├── data/                   # 输入数据 (PCDs 和 Pose 文件)
├── scripts/                # 预处理工具 (坐标修正, 降采样, GNSS模拟)
├── src/
│   ├── global_optimizer.py # 融合流程主入口
│   └── optimizer_modules/  # 核心模块: Loader, Matching (SVD/ICP), Optimization (GTSAM)
└── results/                # 输出结果 (轨迹 *.csv 及 融合点云 *.pcd)

```

## 环境依赖 (Dependencies)

* **Python 3.10**
* **基础库**: `open3d`, `numpy`, `scipy`
* **优化库**: `gtsam`

```bash
conda env create -f environment.yaml
conda activate mapping_fusion

```

## 使用流程 (Workflow)

### 1. 数据预处理 (Preprocessing)

请确保原始子图数据 (PCDs) 和轨迹文件已置于 `data/` 目录。

**坐标系标准化与 GNSS 模拟:**
修正 LiDAR 坐标系（如需）并从 Fast-LIVO2 里程计生成全局先验。

```bash
# 修正坐标轴 (如需) 并生成模拟 GNSS 数据
python scripts/fix_pcd_coords.py data/HK_PCD --map "z,y,-x" --out_dir data/HK_PCD_FIXED
python scripts/stitch_fastlivo2_poses.py --input-dir data/HK_POSE --pattern "*_fixed.txt"

```

**点云降采样:**
对点云进行降采样以加速 ICP 配准过程。

```bash
python scripts/preprocess_pcds.py --pcd-dir data/HK_PCD --out-dir data/HK_PCD_DS --voxel-size 0.5

```

### 2. 执行融合 (Execution)

运行主程序执行分层优化。流程模块化为 5 个阶段 (Load -> SVD -> Overlap -> ICP -> GraphOpt)。

```bash
python src/global_optimizer.py --pcd_dir "data/HK_PCD_DS" --max_stage 5

```

**关键参数:**

* `--max_stage`: 指定终止阶段 (例如 `2` 用于查看粗配准结果，`5` 执行完整优化)。
* `--gating_distance_m`: 空间邻接检测的距离阈值 (默认: 500m)。

### 3. 输出结果 (Output)

结果将保存至 `results/` 目录：

* `global_poses_optimized.csv`: 优化后的全局轨迹 (Timestamp, x, y, z, qw, qx, qy, qz)。
* `Global_Map_Optimized.pcd`: 最终融合的全局点云地图。