SLAM Scan-Matching/Registration

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

2. 迭代最近点 Iterative closest Point (ICP)

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

2.1 实现步骤

• 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)
  • 结果振荡
  • 达到最大迭代次数

2.2 Metric-based ICP (MbICP)

• 目标：通过improve matching算法来提高性能
• 基本原理：发现一个变换使用距离$d$最小
• 算法：与ICP相比，只有计算距离的公式有变化（考虑了旋转因素）
  
  
  • $L$是一个与长度等同的设计参数，通过取$L=3$较佳

  2.3 协议差矩阵（Covariance Matrix）

  • 用途：用于描述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