# snowland-slam

**Repository Path**: hoops/snowland-slam

## Basic Information

- **Project Name**: snowland-slam
- **Description**: C++实现的开源SLAM框架。用于算法对比测试（非工业使用）。
- **Primary Language**: Unknown
- **License**: Apache-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2026-06-15
- **Last Updated**: 2026-07-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: slam, OpenCV

## README

# Snowland SLAM

一个基于 C++20 的模块化、插件式视觉 SLAM 框架，面向教育、研究和算法对比。

## 特性

- **模块化架构** -- 所有组件（前端、后端、特征提取器、求解器、相机）均为可替换插件
- **多种优化后端** -- Eigen（手写实现）、g2o 和 Ceres Solver，涵盖 Gauss-Newton、Levenberg-Marquardt 和 Dogleg 算法
- **丰富的算法对比** -- 同一问题通过不同实现求解（如 Eigen vs OpenCV 的单应矩阵估计）
- **多数据集支持** -- KITTI 里程计、nuScenes、本地图像文件夹和实时摄像头输入
- **交互模式** -- 批量自动运行、逐帧步进和运动 API 演示

## 依赖

| 组件               | 是否必需  | 说明                              |
|--------------------|----------|------------------------------------------|
| C++20 编译器        | 是       | MSVC 2022+ 或 GCC 11+ / Clang 14+        |
| CMake 3.16+        | 是       | 构建系统                             |
| OpenCV 4.x         | 是       | 图像读写、特征提取、可视化       |
| Eigen3             | 是       | 线性代数（含自动回退搜索）         |
| g2o                | 可选     | 图优化（Bundle Adjustment）   |
| Ceres Solver       | 可选     | 非线性最小二乘                  |
| glog               | 可选     | Google 日志库                   |
| Open3D             | 可选     | 3D 点云可视化                    |
| PCL                | 可选     | 点云库（轨迹/地图显示）          |
| vcpkg              | 推荐     | Windows 包管理器         |

## 快速开始

### 1. 通过 vcpkg 安装依赖

```bash
git clone https://github.com/microsoft/vcpkg.git
cd vcpkg && bootstrap-vcpkg.bat

# 必需
vcpkg install opencv:x64-windows eigen3:x64-windows

# 可选（推荐）
vcpkg install g2o:x64-windows ceres:x64-windows
```

### 2. 构建

```bash
cd snowland-slam
cmake -B build -S . -DCMAKE_TOOLCHAIN_FILE=<vcpkg-root>/scripts/buildsystems/vcpkg.cmake
cmake --build build --config RelWithDebInfo
```

Visual Studio 用户也可以使用项目提供的 `CMakePresets.json`。

### 3. 运行

```bash
# 批量处理（默认：../data/images 中的图像）
./build/bin/snowland_slam

# KITTI 数据集
./build/bin/snowland_slam --kitti /path/to/kitti/sequences/00

# nuScenes 数据集
./build/bin/snowland_slam --nuscenes /path/to/nuscenes

# 逐帧交互模式
./build/bin/snowland_slam --step

# 运动 API 演示
./build/bin/snowland_slam --motion
```

#### 跟踪方法与算法选择

| 参数              | 可选值                                          | 默认值      | 说明                    |
|------------------|------------------------------------------------|------------|------------------------|
| `--method`       | `feature` / `flow`                             | `feature`  | 整体跟踪策略              |
| `--pnp`          | `ransac` / `dlt` / `p3p` / `minreproj`          | `ransac`   | 3D-2D PnP 位姿估计算法    |
| `--est2d2d`      | `essential` / `homography`                     | `essential`| 2D-2D 回退（仅光流模式）   |
| `--flow-backend` | `opencv` / `eigen` / `direct`                  | `opencv`   | 光流跟踪器（仅光流模式）   |
| `--triangulate`  | `opencv` / `eigen` / `direct`                  | `opencv`   | 三角化后端算法            |
| `--kf-interval`  | `<N>`                                          | `1`        | 关键帧间隔（帧数）        |

> `--odometry` 仍可用于向后兼容（`--odometry pnp` 等同 `--method feature`、`--odometry flow` 等同 `--method flow`）。

#### 可视化与评估

| 参数              | 可选值                                          | 默认值      | 说明                    |
| `--vis`          | `all` / `pcl` / `image` / `none`               | `all`      | 可视化模式（PCL 3D / OpenCV 2D） |
| `--gt-compare`   | `on` / `off`                                   | `on`       | 真值轨迹对比评估          |
| `--gt`           | `<file>`                                       | (无)       | 真值轨迹文件（KITTI 格式），覆盖数据集内建 GT |

```bash
# 特征点法（默认）+ RANSAC PnP
./build/bin/snowland_slam --kitti /path/to/kitti/sequences/00

# 特征点法 + DLT PnP
./build/bin/snowland_slam --kitti /path/to/kitti/sequences/00 --pnp dlt

# 特征点法 + 最小重投影误差（无 RANSAC 外点剔除）
./build/bin/snowland_slam --kitti /path/to/00 --pnp minreproj

# 光流法 + 本质矩阵 + Eigen 光流
./build/bin/snowland_slam --kitti /path/to/00 --method flow --flow-backend eigen

# 光流法 + 单应矩阵回退 + Direct 求解器光流
./build/bin/snowland_slam --kitti /path/to/00 --method flow --est2d2d homography --flow-backend direct

# Eigen 三角化 + 仅显示 PCL 3D 地图
./build/bin/snowland_slam --kitti /path/to/00 --triangulate eigen --vis pcl

# 指定外部真值轨迹进行对比
./build/bin/snowland_slam --kitti /path/to/00 --gt /path/to/gt_poses.txt
```

## 项目结构

```
snowland-slam/
├── slam/                          # 核心库
│   ├── system/                    # SLAM 系统组装
│   ├── frontend/                  # 视觉里程计前端
│   ├── backend/                   # 全局优化后端
│   │   ├── ba/                     # 集束调整（Ceres / Eigen / g2o）
│   │   └── kalman_filter/          # 卡尔曼滤波与扩展卡尔曼滤波
│   ├── map/                       # 关键帧与地图点
│   ├── odometry/                  # 位姿估计与三角化
│   │   ├── pose_estimation/       # 位姿估计器层次结构
│   │   │   ├── feature_based/     #  2D-2D / 3D-2D / 3D-3D
│   │   │   │   ├── est2d2d/       #   本质矩阵/单应矩阵（DLT / Eigen / OpenCV）
│   │   │   │   ├── est3d2d/       #   PnP（DLT / P3P / RANSAC / 重投影优化）
│   │   │   │   └── est3d3d/       #   ICP（SVD / Gauss-Newton）
│   │   │   └── image_based/       #  直接法与光流
│   │   └── triangulation/         # 线性三角化（DLT / Eigen / OpenCV）
│   ├── dataset/                   # 数据输入（KITTI、nuScenes、摄像头、文件夹）
│   ├── camera/                    # 相机模型（PinholeCamera、FisheyeCamera）
│   ├── feature/                   # 特征提取与匹配
│   │   ├── extractor/             #  FAST、GFTT、ORB、SIFT
│   │   └── matcher/               #  暴力匹配器
│   ├── motion/                    # 刚体变换（Rotation、RotationTranslation）
│   ├── visualization/             # 可视化（PCL 3D 地图/轨迹 + OpenCV 2D 光流/特征）
│   ├── evaluation/                # 轨迹评估（ATE / RPE、对比绘图）
│   ├── core/                      # 基类与公共定义
│   └── tools/
│       ├── optimization/          # 非线性最小二乘求解器框架
│       │   ├── base_optimizer.h   #  抽象基类：BaseOptimizer
│       │   ├── nonlinear/         #  非线性求解器
│       │   │   ├── gauss_newton/  #    GN：Eigen / g2o / Ceres / Simple
│       │   │   ├── levenberg_marquardt/ # LM：Eigen / g2o / Ceres / Simple
│       │   │   └── dogleg/        #    Dogleg：Eigen / g2o / Ceres / Simple
│       │   └── linear_least_squares/ # 线性最小二乘（Cholesky / QR / SVD）
│       └── convert/               # 零拷贝 cv::Mat <-> Eigen 转换器
├── test/                          # 算法对比测试
│   ├── odometry/pose_estimation/  # 光流、直接法、PnP、ICP、本质矩阵等
│   ├── odometry/triangulation/    # 三角化方法对比
│   ├── backend/ba/                # BA 后端对比
│   ├── backend/kalman_filter/     # 卡尔曼滤波对比
│   └── tools/optimization/        # 求解器基准测试
├── main.cpp                       # 应用程序入口
├── CMakeLists.txt
└── CMakePresets.json
```

## 架构

```
                          +----------------------------+
                          |        System (组装器)       |
                          +----------------------------+
                           /     |      |        \
                          /      |      |         \
                    Frontend   Backend  Visualizer  Dataset
                    (VO 线程)  (BA 线程) (主线程)    (数据源)
                       |         |        |
         +--- Camera --+     BASolver    |    +-- TrajectoryEvaluator
         |             |     /  |  \     |    |    (ATE / RPE)
    Extractor     Feature   Ceres g2o  PCL  Image
       |           Matcher  Eigen      3D    2D
       v              |
    Features     matchBuffer
        \            /
         \          /
        FeatureBasedFrontend
               |
          processFrame()
          ├── track()              -- 投影匹配(相邻帧+历史关键帧) + PnP 位姿估计
          ├── triangulateNewPoints() -- 全局描述子匹配 + map_point_id 传播 + 三角化
          └── needNewKeyframe()    -- 关键帧决策
               |
               v
             Map (共享数据核心)
          +---------+
          |    |    |
       Frame  MapPoint  KeyFrame
```

### 前端：特征匹配设计

| 阶段                      | 方法                    | 作用                          |
|---------------------------|-------------------------|----------------------------------------------|
| `track()` Phase 1         | 投影匹配（last_frame_）  | 用上一帧的已知 MapPoint 建立 3D-2D 对应       |
| `track()` Phase 2         | 投影匹配（历史关键帧）    | 用 feature_match_buffer_ 补充匹配，提高鲁棒性 |
| `track()` PnP             | RANSAC / DLT / P3P      | 从 3D-2D 对应中估计当前帧位姿 (T_wc)          |
| `triangulateNewPoints()`  | 全局描述子匹配 + 三角化   | 单入口：特征关联、map_point_id 传播、3D 点三角化 |

> `FeatureMatcher` 仅用于系统初始化 (`initializeSystem()`) 和 `triangulateNewPoints()`，不再参与 `track()` 的位姿估计。

### 后端引擎

| 组件                              | 类型               | 说明                                        |
|----------------------------------|--------------------|---------------------------------------------------|
| `BABackend` + `CeresBASolver`    | 批量优化            | BABackend 委托 Ceres 求解器执行全局 BA             |
| `BABackend` + `G2oBASolver`      | 批量优化            | BABackend 委托 g2o 求解器执行全局 BA               |
| `BABackend` + `EigenBASolver`    | 批量优化            | BABackend 委托 Eigen 求解器执行全局 BA             |
| `KalmanFilterBackend`            | 递推滤波            | 线性 KF（匀速运动模型）                            |
| `ExtendedKalmanFilterBackend`    | 递推滤波            | EKF-SLAM（用户自定义非线性模型）                    |
| `OpenCVKalmanFilterBackend`      | 递推滤波            | OpenCV 封装的线性卡尔曼滤波                        |
| `OpenCVExtendedKalmanFilterBackend` | 递推滤波         | OpenCV 封装的扩展卡尔曼滤波                        |

**BA 模块重构**：`BABackend` 是单一后端类，内部通过可替换的 `BASolver` 策略执行优化（Ceres / g2o / Eigen）。`BASolver` 是纯算法接口，不继承 `BaseModule`。`BABackend::initialize()` 按可用性自动选择：Ceres > g2o > Eigen。

所有后端继承自 `Backend`，在各自线程中运行，当收到新关键帧时唤醒执行优化。

### 插件系统

所有模块继承自 `BaseModule`（生命周期：`initialize` / `reset`）。算法实现 `AlgorithmPlugin<Input, Output>` 模板，允许同一接口的多种实现共存。

### 优化求解器框架

求解器框架为非线性最小二乘提供一个统一的抽象接口（`BaseOptimizer`）：

```cpp
#include "slam/tools/optimization/nonlinear/gauss_newton/eigen_gauss_newton.h"

auto solver = EigenGaussNewton();
solver.set_initial_guess(x0);
solver.set_residual_function([](const Eigen::VectorXd& x) { return f(x); });
solver.set_jacobian_function([](const Eigen::VectorXd& x) { return J(x); });
solver.solve(100);                          // 最多 100 次迭代
Eigen::VectorXd result = solver.get_result();
```

所有求解器参数（收敛容差、LM 阻尼、Dogleg 信赖域半径）通过共享的 `OptimizationParams` 结构体配置。

可用求解器：

| 求解器                | 后端    | 算法                |
|----------------------|---------|--------------------------|
| `EigenGaussNewton`   | Eigen   | Gauss-Newton             |
| `EigenLM`            | Eigen   | Levenberg-Marquardt      |
| `EigenDogleg`        | Eigen   | Powell's Dogleg          |
| `G2OGaussNewton`     | g2o     | Gauss-Newton             |
| `G2OLM`              | g2o     | Levenberg-Marquardt      |
| `G2ODogleg`          | g2o     | Powell's Dogleg          |
| `CeresGaussNewton`   | Ceres   | Gauss-Newton             |
| `CeresLM`            | Ceres   | Levenberg-Marquardt      |
| `CeresDogleg`        | Ceres   | Powell's Dogleg          |
| `SimpleGaussNewton`  | Eigen   | Gauss-Newton（精简版）   |
| `SimpleLM`           | Eigen   | Levenberg-Marquardt（精简版） |
| `SimpleDogleg`       | Eigen   | Powell's Dogleg（精简版）    |

> 基于 g2o 的求解器需要 `g2o`。基于 Ceres 的求解器需要 `Ceres Solver`。Simple 变体为自包含的 Eigen 实现，除 Eigen 外无其他外部依赖。

## CMake 选项

| 选项              | 条件        | 效果                        |
|---------------------|-------------|-------------------------------|
| `HAS_G2O`           | 检测到 g2o   | 启用基于 g2o 的求解器和 BA 后端    |
| `HAS_CERES`         | 检测到 Ceres | 启用基于 Ceres 的求解器和 BA 后端   |
| `HAS_GLOG`          | 检测到 glog  | 启用 Google 日志支持              |
| `HAS_OPEN3D`        | 检测到 Open3D | 启用 3D 点云可视化              |
| `HAS_PCL`           | 检测到 PCL   | 启用点云库支持                   |

这些选项由 `find_package` 自动设置，无需手动指定。

## 运行测试

```bash
# 构建所有测试
cmake --build build --config Release

# 运行指定测试
./build/bin/Release/tools_optimization_test_optimization_solvers_comparison
./build/bin/Release/odometry_pose_estimation_image_based_optical_flow_test_optical_flow_comparison
```

测试目标命名规则：`<test目录相对路径>_<不含.cpp的文件名>`，其中 `/` 替换为 `_`。详见 `install.md` 获取完整列表。

测试侧重于跨实现对比（Eigen vs OpenCV vs g2o vs Ceres vs Simple），以验证一致性并基准化性能。

## 许可证

Apache License 2.0 -- 详见 [LICENSE](LICENSE)。