# Slam_Toolbox

## SLAM Toolbox是一個開源的ROS SLAM套件,其算法重點如下:

1. 基礎:改进自Open Karto,使用局部掃描匹配和全局圖優化。
2. 圖優化:使用Google Ceres替換了原有的優化接口,速度更快。
3. 多模式:同步建圖、非同步建圖和純定位三種模式。
4. 多回合建圖:可以序列化和反序列化整個pose graph,進行多次建圖。
5. 圖合併:可以合併多個序列化的地圖。
6. 位姿圖修改:可以人工修改pose graph以助於回環閉合。
7. 效果:可以建立超過10萬平方英尺的大尺度地圖,定位精度高。
8. 易用性:配置簡單,可由非專業人員操作。
9. 應用:許多機器人平台已採用該套件,成為ROS 2的默認SLAM。
10. 貢獻:填補了開源社區大尺度激光SLAM的空白。

SLAM Toolbox使大尺度激光SLAM變得簡單可靠,為開源機器人技術作出了重要貢獻。

## 可以歸納為以下幾個部分:

1. 前端
- 掃描預處理:去雜訊,過濾離群點等。
- 掃描匹配:採用局部特徵匹配算法,匹配当前掃描與近期的先前掃描,生成初始姿態估計。

2. 后端

- 圖建立:將匹配的掃描投影到二維栅格地圖中,更新格點機率值。
- Pose Graph構建:每個掃描匹配產生一個姿態節點,相鄰節點間添加邊構成圖。
- 迴圈檢測:尋找重複地區的相似掃描,添加迴圈閉合約束。
- 圖優化:使用Ceres優化框架,最小化所有約束誤差,得到全局一致的地圖。

3. 前端與后端交互

- 前端提供初始姿態,后端返回最优化後的姿態。
- 前端按模式使用后端的最新結果或緩衝結果。

4. MAPPING模式

- 同步:緩衝掃描,全部進行圖優化。
- 非同步:只優化最近的掃描,保證實時性。

5. 定位模式

- 使用原地圖,匹配當前掃描,構建臨時動態圖進行定位。

SLAM Toolbox通過閉環檢測與全局優化,實現大尺度環境的激光SLAM,並提供多種實用模式。它優化了Open Karto算法,在開源社區具有重要影響。

## Overview
The Slam Toolbox package incorporates information from laser scanners in the form of a LaserScan message and TF transforms from odom->base link, and creates a map 2D map of a space. This package will allow you to fully serialize the data and pose-graph of the SLAM map to be reloaded to continue mapping, localize, merge, or otherwise manipulate. We allow for SLAM Toolbox to be run in synchronous (process all valid sensor measurements, regardless of lag) and asynchronous (process valid sensors measurements on an as-possible basis) modes.

ROS drop in replacement to gmapping, cartographer, karto, hector, etc featuring a feature complete SLAM build on the robust scan matcher at the heart of Karto that's been souped up and sped up for use in this package. A new optimization plugin based on Google Ceres is also introduced. We also introduce a new localization method called "elastic pose-graph localization" that takes a sliding window of measurements and adds them to the graph to optimize and refine. This allows for the tracking of local features which have changed in the environment rather than viewing them as deviations and removes these extra nodes when you leave an area to not effect the long term map.

This was created in response to inadequate mapping and localization quality from GMapping, Karto, Cartographer and AMCL in massive and dynamic indoor environments, though it has been tested and deployed on sidewalk robots as well.

There is included an RVIZ plugin for interacting with SLAM Toolbox over ROS topics and services. While it is included as a debug tool, for production use its recommended to wrap your own operation interface. I allow for manual pose-graph manipulation through the RVIZ plugin which works well for small maps. In larger maps, the interactive markers will overload RVIZ so its mostly useful as a debug instrument in smaller maps or for introspection. In the interactive pose-graph manipulation mode, you can move and rotate nodes in the graph, while being displayed the laser scan of that node to align it better with a loop closure or match, then rerun the optimizer over that section of the pose-graph. It is also very useful to rotate maps to be axially aligned.

## Karto vs Cartographer
cartographer与karto的都是2D激光SLAM算法，而且不是基于概率估计的SLAM框架

### 两者采取的都是图优化框架

采取的优化库不一致， karto采取的是spa(karto_slam)或g2o(nav2d), cartographer采取的是google的ceres构建problem优化。 karto的前端与后端采取的是单线程进行，cartographer按paper说明，采取的是4线程后端优化，还在进一步确定。

### 运动预测部分：tracker

karto利用的是odom进行初始位置的预测， cartographer部分利用imu构建预测模型，scanmatcher与odom(可选)构建观测模型，采取UKF进行运动预测， cartographer带有tracker的说法。

### scanMatcher 部分

1. karto 采取的的是real-time correlative scan matcher（三维窗口遍历寻优）的方式进行的。 采取的是双分辨率的低分辨率和高分辨率的两次搜索。

2. cartoGrapher也是采取的双搜索的方式进行的， 先用一次real-time correlative scan matcher（三维窗口遍历寻优），再构建优化等式，利用ceres优化求解。（栅格概率， T的偏差，R的偏差）

### submap的说明

1. karto没有submap的概念，全部以keyScan的形式存储在sensorManager。 无地图缓存，但每次计算地图有计算消耗。采取的是scan-map的匹配方式，每次keyScan进入主动的依据pose的距离窗口生成localMap进行匹配。 local 与 gloal的loop closure依据graph的结构和sensorManage顺序存储分配的ID信息，选择候选scans，生成localMap，进行匹配，依据score进一步确定闭环。

2. cartographer采用了submap的概念， 依据一定数量的scan初始一个submap, 依据窗口大小， 插入newScan，更新submap. 有子图缓存，会占用内存。

### Loop-Closure

1. karto 主要依据pose 和 distance信息创建localMap，scanMatcher（real-time correlative scan matcher）确定。

1) 依据当前的Vertex, 从Graph中找到与之相邻的所有vertex(一定距离范围内).
2) 采取广度优先搜索的方式，将相邻（next）与相连（adjacentVertices）添加进nearLinkedScans.
3) 从sensorManager中取从前到后，依据id序号挑选与当前在一定距离范围内，且不在nearLinkedScans中的candidateScans, 当数量达到一定size，返回。
4) loopScanMatcher进行scanTomap的匹配，当匹配response 和covariance达到一定要求认为闭环检测到。得到调整的correct pose.
5. Add link to loop : 调整边（全局闭环）
6) 触发correctPose: spa优化
2. cartogapher 类似（（real-time correlative scan matcher）），引入了branch and bound的方式， 加快了闭环的查找。

依据多分辨率多层的树型结构，单枝生长的方式(branch)，及时剪枝操作（bound），深度优先搜索(DFS)确定闭环。 （Intra-submap Inter-submap ）

添加相应的闭环约束。构建优化问题，利用ceres优化。