[SLAM] ICP, Registration of PointCloud

이번 포스팅은 Registration, ICP(Iterative Closest Point)에 대해서 다뤄보겠습니다. Registration이란 source points, target points가 주어졌을 때 두 pointcloud사이의 관계(R|T)를 계산하는 방법입니다. ICP란 두 pointcloud 사이의 correspondences가 주어져 있지 않을 때 Point들을 Registration하는 방법입니다. optimal solution이 존재하기 않기 때문에 pointcloud의 관계를 잘 estimation하는 것이 중요합니다.
Lidar SLAM의 대부분은 이 ICP를 응용 발전한 케이스가 많습니다. ICP는 많은 computation cost를 요하기 때문에 real-time을 추구하는 SLAM에서는 여러 테크닉을 사용해서 이를 해소합니다. 예를 들어 NDT를 이용하여 noise를 제거한 후 ICP를 돌린다거나 LOAM같은 Lidar SLAM에서는 모서리나 평면같은 특징점(Feature point)를 추출하고 이것들만 ICP를 진행하기도 합니다.

 

---

Method

Registration은 크게 두 pointcloud 사이의 correspondance가 주어졌을때와 아닐때로 나뉩니다.

Known Data Association 

두 pointcloud사이의 관계를 이미 알고있다면 Direct하게 optimal solution을 구할 수 있습니다.

(Direct = no initial guess needed, Optimal = no better solution exists)

아래는 optimal solution을 SVD를 이용해서 구하는 과정입니다. 아래의 cost function에서 $y_{i}$에서의 i와 매칭되는 $x_{j}$에서의 j는 이미 알고있는 상황입니다.

 

상세 유도과정은 아래 영상참고바랍니다. 

 

UnKnown Data Association 

사전에 대응되는 관계를 모른다면 여기서 ICP를 사용하게 됩니다(실제로는 unknown data association경우가 더 많습니다). ICP를 이용해서 반복하여 해를 업데이트 합니다.(gaussian newton method)

사전에 필요한 최적화 방법은 아래 포스팅참고하세요.

 

[SLAM] 5. Optimization, 비선형 최적화

대략적인 Vanilar ICP Algorithm을 순서대로 나열해보면,
1. source의 각각 point에 대해 가장 가까운 거리에 있는 target의 point를 매칭하여 corresponding relation을 찾습니다.
2. corresponding relation을 활용하여 R|T를 구합니다.
3. R|T를 활용하여 다시 Rx+T와 가장 가까운 target의  point들간의 차이(error)를 구합니다. (gaussian newton method)
4. error가 threshold보다 적어질 때까지 반복합니다.

위의 기본적인 vanilar 알고리즘은 $O(N^{2}$ 시간으로 많은 시간이 소요되고 bad initial guess가 주어질 경우 안 좋은 결과를 나타냅니다. 아래는 이를 해결하기 위한  다양한 advanced method들이 있습니다. 예를 들면 KD-Tree이용하여 가장가까운 점을 찾는다던가 
비교할 데이터를 sampling하여 선택으로 Uniform, Random등의 방법, association하는 방법들 즉, Matching을 어떤 방법으로 할지를 말하는데 Closest, Normal 방법등이 있습니다. weighted 방법으로 correspond pair들에 가중치를 주는 것인데 거리에 따라 줄 수도 있고 법선벡터에 따라 줄 수도 있습니다.또한 outlier 제거하는 방법으로 일정거리 이상이면 버리던가 정렬을 통해 하위 몇%버리는 식으로 진행합니다.

아래는 위에서 설명한 point-to-point ICP에 대한 유도과정 입니다.

point-to-point는 convergence 속도가 느리고 robust하지 못하기에 실제로는 point-to-plane의 방법을 많이 사용합니다. 아래는 유도과정입니다.

 

 

Reference

ICP는 reference가 비교적 많습니다. 아래는 코드로 비교적 쉽게 구현해 볼 수 있는 예제코드들 입니다.

Jupyter Notebook Viewer

 

GitHub - 93won/2D_LiDAR_Odom_ICP: Simple 2D LiDAR Odometry using ICP

 
kiss icp는 최근에 Bonn대학에서 나온 꽤 핫한 icp 알고리즘입니다.

GitHub - PRBonn/kiss-icp: A LiDAR odometry pipeline that just works

 

ETC

ICP : 보통 Point-to-Point의 association을 통해 iterative하게 R|T를 구함

G-ICP : Point-to-Plane을 G-ICP로 말하기도 함 (loop closing에 유리)

NDT : 직접적인 distance가 아닌(error term) voxel로 나눠서 probablistic하게 R|T를 구함 (sequence registration에 유리)

GitHub - koide3/ndt_omp: Multi-threaded and SSE friendly NDT algorithm