1. 概念

SLAM：它根据两帧扫描数据，基于环境的外形和扫描匹配来计算刚体变换。
仅仅基于里程计传感器（Odometry Sensors provided by wheel encoders）或IMU传感器创建的地图是不准确的，因为里程计传感器存在累计误差，所以LDS(Laser Distance Sensor)使用Scan-Matching/Registration算法使新扫描的激光数据与已有地图或激光扫描数据重新对齐，从而获得更加准确的机器人位姿(Pose)和地图(Map)。
Scan-Matching性能评估指标
- 计算量
- 结果的直观性
Scan-Matching问题描述

1.1 SLAM定位方法

SLAM定位方法流派：概率方法（probabilistic methods）、
概率方法
- Extended Kalman Filter (EKF)
- Particle filter
  - Monte-Carlo （stochastic）
  - Olson (brute-force）
  - Hough (analysis-oriented)
- Maximum Likelihood
匹配法（Scan Matching）：通过匹配两帧点云数据（即把两帧点云数据对齐）来计算机器人的刚体变换
- ICP及各种变体
- Scan-to-Scan
- Scan-to-Map
- Feature-based
- RANSAC for outlier rejection
- Correlative matching
- Iterative Dual Correspondence

基本原理：试图迭代地找到两次扫描之间的变换（平移 $T$ +旋转 $R$ ），以使两次扫描（scan frame）的最近邻居之间的距离最小化。
算法组成：对应点搜索和位姿求解。它的目的是寻求点集之间的匹配关系，求解的结果是两点集之间的平移及旋转量。
实现方法：没有特征提取的条件下，把两次scan frame进行对齐
缺点： $\color{red}{速度慢}$ (因为需要使用所有的点云(point cloud)数据来找到最优变换）
输入：
- 参考激光扫描数据
- 新的激光扫描数据
输出：
- 匹配的精确度（the accuracy of the matching）
- 扫描匹配的协方差矩阵（covariance of the scan matching）：常用于做闭环检测（loop closing）
a

1) moving
通过变换矩阵（初始变换矩阵通过Odom或IMU计算得到、其它时候为上一次的迭代结果），使两个Scan frame位于同一个坐标系
2) matching
为每一个new scan frame 中的点在reference scan frame中找到一个距离最近的点（逐点搜索或kd-tree optimized search）
3) weighting
根据一定的标准（匹配的距离大小、指定点的不确定性）为每个激光点赋予不同的权重，因为它不能提高收敛速度，一般很少使用
4) rejection
由于移动和遮挡，两个scan frame不可能完全相同，假设有80%是相同的激光点。其它20%为异常点（如距离过大被认为是误匹配、多个新激光点匹配到同一个旧的激光点，则只保留距离最小的激光点），则把它们拒绝掉
5)minimization
- 它返回使两次扫描误差最小的变换矩阵
- 最小化常使用点到线段的距离（point-to-segment distance）, 因为每次观察同一个位置，激光点不一定是同一个点，但这些点总是在同一条直线上
- 最小化点到点的距离（point-to-point）将导致坏的收敛或收敛速度慢
- 误差最小化可通过 $\color{red} {Cholesky \quad decomposition}$ 实现
Loop以上5步，直到满足以下任一条件为止：
- 收敛(converges)
- 结果振荡
- 达到最大迭代次数

用途：用于描述ICP匹配输出的变换的准确度（accuracy）
2D空间
- 每一个Pose有三个参数 $(x, y , \theta)$
- 协方差矩阵为 $3 \times 3$
3D空间
- 每一个Pose有三个参数 $(x, y , z, \alpha, \beta, \gamma)$
- 协方差矩阵为 $6 \times 6$
计算方法
- Bruteforce Method
- Closed Form Covariance of a Minimization Algorithm
a
a
a