plicp 點雲迭代最近鄰點配準法
阿新 • • 發佈：2022-04-06
輸入引數
- 點雲A的極座標集合
- 點雲A對應Lidar所在pose
- 點雲B的極座標集合
- 點雲B對應Lidar所在pose
Features
- 根據兩個點雲的弧度關係確定找點的起始位置
- 根據兩個點雲的弧度關係設定找點的停止條件
- 算出被投影點雲的大跳小跳錶，根據大跳小跳錶來遍歷點雲
大跳小跳錶(假設點雲A中間點為a點)
建立大跳小跳錶的前提點雲的弧度是遞增的
理論上來說當點雲A的所有點的弧度都一樣，那麼點雲A中長度和b點長度相同的點，距離最近
a點一共有兩個大跳錶和兩個小跳錶：
| 序號比a點小的點 | 序號比a點小的點 | 序號比a點大的點 | 序號比a點大的點 | |
|---|---|---|---|---|
| 小跳錶1(有序) | 大跳錶1(有序) | 小跳錶2(有序) | 大跳錶2(有序) | |
| 長度比a短 | 長度比a長 | 長度比a短 | 長度比a長 |
前提
- 輸入引數點雲的極座標的弧度是有序的(這裡的有序不是說弧度是遞增或者遞減，而是說點是Lidar順時針或者逆時針掃了一圈出來
- 不同弧度上的任意兩點長度越接近，距離越近
- A和C之間區取餘角，Lsin(餘角)是A點距離CC'這條線最近距離
步驟
- 過濾原始資料的無效點
a. 原始資料中長度為0(或者近似)的點標記為無效
b. 原始資料中長度過長(超出Lidar掃描能裡的點)標記為無效
c. 使用滑窗(2cm)，每次滑窗內的點合併成一個點(x和y取平均值) - 找出點雲A和點雲B各自的最小弧度和最大弧度
- 排序點雲(原始資料的有序請看前提1)，排序結果為弧度遞增
- 轉換兩個點雲各自的迪達爾座標
- 計算兩個點雲對應Lidar pose的差
- 根據Lidar pose的差，將點雲B投影到點雲A為C，算出C的笛卡爾座標
- 建立點雲A的大跳小跳錶
- 算出點雲A每弧度的點數量NumA
- 遍歷點雲C的每個點X
a. 算出X的極座標
b. 算出X點弧度和點雲A最小弧度的差值，然後乘以NumA，得到的結果cur_idx是遍歷點雲A的起始點序號
c. 如果儲存了X的前一點在點雲A最近點的序號last_beat_idx，那麼X點就從last_beat_idx+1開始在點雲A上找最近點，cur_idx=last_beat_idx+1
d. 從X點的分別向前向後查詢最近點，每次遍歷都儲存目前找到的最近距離dist(根據大跳小跳錶遍歷)
e. 算出點雲A任意一點a和X的弧度差raX，然後用X的長度做斜邊，算出和X點弧度相差raX的點的距離best_dist(最近距離)，一旦之前存在dist <= best_dist，那麼就認為找到了最近點D - 找到最近點D之後，算出點D前一個點D1和後一個點D2分別距離點X的距離，距離更短的點就認為是距離點X第二最近的點