# SLAM_Fusion

**Repository Path**: Frees_cale/slam_fusion

## Basic Information

- **Project Name**: SLAM_Fusion
- **Description**: 双目相机与激光雷达融合的SLAM。

- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 1
- **Created**: 2023-12-03
- **Last Updated**: 2023-12-03

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# SLAM_Fusion

#### 介绍
双目相机与激光雷达融合的SLAM。
前端里程计及后端BA优化都使用Ceres库实现。

图一：点云投影到图像，帮助计算深度
![点云投影到图像，帮助计算深度](images/image_fusion.png)

图二：3d点云。彩色表示计算出的“平面点”（LOAM算法）
![3d点云。彩色表示计算出的“平面点”](images/point_cloud_surf.png)

#### 运行环境
Ubuntu 20.04

ROS Noetic

Ceres-Solver 2.1.0

OpenCV 3.4.14


#### 编译运行
先[下载KITTI数据集](https://www.cvlibs.net/datasets/kitti/eval_odometry.php)，并解压

添加环境变量，粗体部分需要替换为你的电脑上的KITTI数据集所在路径：

export PATH_KITTI_GRAY=**~/Datasets/KITTI**/data_odometry_gray/dataset/sequences

export PATH_KITTI_VELODYNE=**~/Datasets/KITTI**/data_odometry_volodyne/dataset/sequences

export PATH_KITTI_CALIB=**~/Datasets/KITTI**/data_odometry_calib/dataset/sequences

export PATH_KITTI_GT=**~/Datasets/KITTI**/data_odometry_poses/dataset/poses


编译并运行SLAM程序：
```
cd slam_fusion
. build.sh
rosrun slam_fusion_project System ORBvoc.txt
```

再开启一个新的终端，运行publisher：
```
rosrun kitti_publisher publisher 00  // 发布00序列
```




#### 架构说明
kitti_publisher文件夹内，实现了一个ROS节点用于发布数据集中的图片和点云。目前是面向KITTI离线数据集开发的，暂不支持发布在线采集的数据。

slam_fusion_project文件夹内是系统的具体实现。出于性能考虑，将它设计为一个ROS节点（而不是拆分成多个），以节省通信开销。
参考了ORB-SLAM2的架构，分为主线程，FrontEnd线程和BackEnd线程。其中：

System.cpp：系统的入口，也是主线程，负责启动其他线程，以及收发ROS消息。

FrontEnd.cpp：前端线程，用于预处理输入的数据，并迅速生成一个初步的位姿估计（视觉里程计）。
FrontEnd的工作也是由多个线程完成的。下图显示了FrontEnd内部各个线程执行的先后关系，每个蓝色方块代表1个线程。
![FrontEnd内部结构](images/frontend.png)

BackEnd.cpp：后端线程，用于优化位姿和地图点。
对于到来的每一帧，根据与前一帧的共视程度，决定是否建立关键帧。建立关键帧后，立即执行一次BA优化（使用Ceres实现）。



本程序的Pipline与OCB-SLAM2大致相同，即提取ORB特征点，双目深度估计，视觉里程计，BA优化等。
而激光雷达在本程序中的作用主要是：

1.利用激光雷达拥有深度信息的特性，在双目深度估计后，矫正估计的特征点的深度。

2.在最小二乘优化问题中，除了将重投影误差作为约束项，也将点云相关信息（角点到线的距离和平面点到平面的距离）作为约束项。