算法介绍(1)-帧间匹配算法-ICP

帧间匹配算法-ICP类

传统 ICP 方法

1 ICP方法的简单介绍

Iterative closest Points 迭代最近点的方法

Note:

1. 激光雷达是固定在机器人上的。
2. 两帧点云对应的是物理空间中的同一个实体。两帧点云之所以表现的不一样是因为，雷达在动啊。
3. 点云A对应的是机器人在上一帧对应的位姿。
4. 点云B对应的是机器人在当前帧对应的位姿。
5. 使用一种方法对齐两个点云，可以求出前后两帧之间的相对位姿关系。
6. 说白了，就是给定两个点云集合 X 和 P ，求解两个点云集之间的 R 和 t 。

两帧之间机器人位姿变换 T

P2 = P1 × T = P1 × R + t

点云集合	时间戳	激光雷达位姿
A = {a1,a2,a3...an}	t1	P1
B = {b1,b2,b3,,,bn}	t2	P2 = P1 × R + t

假设：a1 和 b1 就是物理空间中的对应点
就应该有 ||a1 - (R*y1+t) || = 0
就可以建立最小二乘的问题；

2 ICP方法求解方法

1 已知对应点的求解方法

1. 前提就是：已知正确的匹配
2. 可以求得解析解的

证明过程：

2 未知对应点的求解方法

• 实际中，不知道对应点匹配
• 不能一步到位计算出R和t
• 进行迭代计算
• EM算法的一个特例

算法流程：

1. 寻找对应点
2. 根据对应点，计算R t
3. 对点云进行转换，计算误差
4. 不断迭代，直至误差小于某一个值

PL-ICP 方法

Point to line
优化思想：修改的是误差尺度

基本思想：

1. 激光点是对环境中曲面的离散采样
2. 重要的不是激光点，而是隐藏在激光点中的曲面
3. 传统的ICP方法用距离最近的点，作为对应点
4. 最好的误差尺度是 当前激光点到实际曲面的距离
5. 关键的问题，是如何恢复曲面
6. 用分段线性的方法来对实际曲面进行近似，从而定义当前帧激光点到曲面的距离。