# fastlio_pgo

**Repository Path**: ccpdead/fastlio_pgo

## Basic Information

- **Project Name**: fastlio_pgo
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 1
- **Created**: 2026-01-14
- **Last Updated**: 2026-03-04

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# FASTLIO2 ROS2
## 主要工作
1. 重构[FASTLIO2](https://github.com/hku-mars/FAST_LIO) 适配ROS2
2. 添加回环节点，基于位置先验+ICP进行回环检测，基于GTSAM进行位姿图优化
3. 添加重定位节点，基于由粗到细两阶段ICP进行重定位
4. 增加一致性地图优化，基于[BLAM](https://github.com/hku-mars/BALM) (小场景地图) 和[HBA](https://github.com/hku-mars/HBA) (大场景地图)

## 环境依赖
1. Ubuntu 22.04
2. ROS2 Humble

## 编译依赖
```text
pcl
Eigen
sophus
gtsam
livox_ros_driver2
```

## 详细说明
### 1.编译 LIVOX-SDK2
```shell
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j
sudo make install
```

### 2.编译 livox_ros_driver2
```shell
mkdir -r ws_livox/src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
cd ws_livox/src/livox_ros_driver2
source /opt/ros/humble/setup.sh
./build.sh humble
```

### 3.编译 Sophus
```shell
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout 1.22.10
mkdir build && cd build
cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
make
sudo make install
```

**新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错**


## 实例数据集
```text
链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb 
--来自百度网盘超级会员v7的分享
```

## 部分脚本

### 1.激光惯性里程计 
```shell
ros2 launch fastlio2 lio_launch.py
ros2 bag play your_bag_file
```

### 2.里程计加回环
#### 启动回环节点
```shell
ros2 launch pgo pgo_launch.py
ros2 bag play your_bag_file
```
#### 保存地图
```shell
ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}"
```

### 3.里程计加重定位
#### 启动重定位节点
```shell
ros2 launch localizer localizer_launch.py
ros2 bag play your_bag_file // 可选
```
#### 设置重定位初始值
```shell
ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}"
```
#### 检查重定位结果
```shell
ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}"
```

### 4.一致性地图优化
#### 启动一致性地图优化节点
```shell
ros2 launch hba hba_launch.py
```
#### 调用优化服务
```shell
ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}"
```
**如果需要调用优化服务，保存地图时需要设置save_patches为true**

## 特别感谢
1. [FASTLIO2](https://github.com/hku-mars/FAST_LIO)
2. [BLAM](https://github.com/hku-mars/BALM)
3. [HBA](https://github.com/hku-mars/HBA)
## 性能相关的问题
该代码主要使用timerCB作为频率触发主函数，由于ROS2中的timer、subscriber以及service的回调实际上运行在同一个线程上，在电脑性能不是好的时候，会出现调用阻塞的情况，建议使用线程并发的方式将耗时的回调独立出来(如timerCB)来提升性能

**总体结构**

```
原始传感器数据 (激光 + IMU)
        ↓
    ┌───────────────────────┐
    │  FASTLIO2 (前端LIO)   │ ← 激光惯性里程计
    └───────────────────────┘
        ↓
    位姿轨迹 (6DoF)
        ↓
    ┌──────────────────────────────────────┐
    │    后端优化层（选择执行）              │
    ├──────────────────────────────────────┤
    │ ① PGO (回环检测+位姿图优化)          │
    │ ② Localizer (重定位)                 │
    │ ③ HBA (一致性地图优化)               │
    └──────────────────────────────────────┘
        ↓
    优化后的地图 + 位姿轨迹
```

## 工作流程

### 1. fastlio2模块（LIO里程计-前端）
```
Livox Mid360 激光 + IMU 数据
        ↓
  ┌─────────────────────┐
  │ 数据预处理          │
  │ - IMU去畸变         │
  │ - 激光去运动畸变    │
  └─────────────────────┘
        ↓
  ┌─────────────────────┐
  │ IESKF滤波           │
  │ (扩展卡尔曼滤波)    │
  │ - 融合 IMU 预测     │
  │ - 激光更新           │
  └─────────────────────┘
        ↓
  ┌─────────────────────┐
  │ IKD-Tree (增量树)   │
  │ - 动态管理点云      │
  │ - 快速搜索邻近点    │
  └─────────────────────┘
        ↓
  输出：位姿 + 地图点云
```

| 文件 | 功能 |
|------|------|
| map_builder.cpp | 核心 LIO 算法 |
| imu_processor.cpp | IMU 数据处理 |
| lidar_processor.cpp | 激光数据处理 |
| ieskf.cpp | 扩展卡尔曼滤波器 |
| ikd_Tree.cpp | 增量 KD 树 |


### 2. pgo模块（回环检测+位姿图优化-后端）
```
FASTLIO2 输出：轨迹 T1 → T2 → T3 → ... → Tn
        ↓
  ┌──────────────────────────┐
  │ 回环检测                  │
  │ 基于：                    │
  │ • 位置先验 (半径搜索)     │
  │ • ICP 点云配准验证        │
  └──────────────────────────┘
        ↓
  检测到回环：Ti ≈ Tj (i << j)
        ↓
  ┌──────────────────────────┐
  │ GTSAM 位姿图优化          │
  │ - 添加回环约束            │
  │ - 优化所有位姿            │
  │ - 最小化总体误差          │
  └──────────────────────────┘
        ↓
  输出：优化后的轨迹 + 一致性地图
```


| 文件 | 功能 |
|------|------|
| pgo_node.cpp | PGO 主节点 |
| simple_pgo.cpp | 位姿图优化算法 |
| commons.cpp | 辅助函数 |