幀間匹配算法-ICP類
傳統 ICP 方法
1 ICP方法的簡單介紹
Iterative closest Points 迭代最近點的方法
Note:
- 激光雷達是固定在機器人上的。
- 兩幀點雲對應的是物理空間中的同一個實體。兩幀點雲之所以表現的不一樣是因為，雷達在動啊。
- 點雲A對應的是機器人在上一幀對應的位姿。
- 點雲B對應的是機器人在當前幀對應的位姿。
- 使用一種方法對齊兩個點雲，可以求出前后兩幀之間的相對位姿關系。
- 說白了，就是給定兩個點雲集合 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 已知對應點的求解方法
- 前提就是：已知正確的匹配
- 可以求得解析解的
證明過程：
2 未知對應點的求解方法
- 實際中，不知道對應點匹配
- 不能一步到位計算出R和t
- 進行迭代計算
- EM算法的一個特例
算法流程：
- 尋找對應點
- 根據對應點，計算R t
- 對點雲進行轉換，計算誤差
- 不斷迭代，直至誤差小於某一個值
PL-ICP 方法
Point to line
優化思想：修改的是誤差尺度
基本思想：
- 激光點是對環境中曲面的離散采樣
- 重要的不是激光點，而是隱藏在激光點中的曲面
- 傳統的ICP方法用距離最近的點，作為對應點
- 最好的誤差尺度是 當前激光點到實際曲面的距離
- 關鍵的問題，是如何恢復曲面
- 用分段線性的方法來對實際曲面進行近似，從而定義當前幀激光點到曲面的距離。