ICP (Iterative Closest Point) 개념 정리

ICP (Iterative Closest Point)로, Point Cloud alignment를 위한 알고리즘

 

Point cloud는 2D 혹은 3D point의 집합을 말하고, 두 포인트 클라우드 사이의 변환을 계산하여 실제 세계에서 동일한 포인트로 겹쳐 일관된 환경 모델을 구축하는 것을 목표로 한다.

 

두 포인트 클라우드 사이(p, q)의 변환이란,

간략히 두 점군을 거의 정합시키도록 에러를 최소로 갖게 하는 최적의 Rotation, Translation을 구하는 문제이다.

즉, ICP는 LiDAR나 RGBD data와 같은 sequential한 data를 통한 SLAM systems에 사용되곤 한다.

 

Two steps of ICP:

1. data association (데이터 연결)

간단한 가장 가까운 이웃 접근 방식을 사용해, 이 작업을 수행한다고 가정

이의 경우는 initial configuration에 기반된다.

2. transformation (데이터 연관)

두 포인트 클라우드 간의 거리를 최소화하도록 움직인다.

1) Center of Mass를 계산한다.

2) SVD를 이용해 rotation을 계산한다.

Decoupling the translation을 통해 rotation에 대한 식으로 바꾼다.

 

Rotation에 대해서 정리했던 식을 전개해서 더 정리해보면, 두번째 term을 최대화하는 것이 전체 cost를 줄이는 방법이 된다.

3x3의 행렬 M을 p, q로부터 얻게 되는데, M을 SVD를 통해 나오는 eigenvector로 부터 나오는 U, V를 통해 R = U V^T가 얻어진다.

R을 통해 t는 다시 앞서 정한 식대로 t = q - Rp로 계산할 수 있다.

 

왜 더 정렬이 되는 정도일 뿐, 결과가 정확하지 않은 이유는 뭘까?

이는 initial data association이 정확하지 않아서일 수 있다.

따라서, 우리는 이 과정을 반복해야 한다. (Recompute the data association & then the alignment)

-> Iterate until convergence so that clouds are aligned

Estimating the data ssociation is the key challenge.

 

위의 내용은 Vanlia ICP 알고리즘이고,

실제로는, ICP 알고리즘을 최적화하기 위한 여러 변형이 있다.

point-to-plane ICP

환경에서 점들이 줄기가 없거나, 특징점이 없다고 가정하지만, 실제로는 표면에서 줄기가 있다.

(the points don't stem or are not distinct points in the environment)

 

기본적으로 표면에서 스캐너를 통해 무작위로 중심에 있고, 포인트 클라우드와 표면 간의 정렬을 만든다.

 

현실적으로 존재할 수 없는 인공적인 point-to-point를 하지 않고,

point-to-plane alignment를 하거나 혹은 plane metric을 주는 것이 ICP 알고리즘의 결과를 더 좋게 할 수 있다.

destination의 normal vector를 통해 surface의 평면을 얻어내고, 이 point-to-plane 거리 L을 얻어 최소화하는 방법을 말한다.

 

 

다만, 이를 풀기 어렵기 때문에

lindaer approximation을 통해 general least squares approach로 접근해야 한다.

또 point-to-plane metric error를 최소화하기 위해 Gauss Newton method를 사용하게 될 것이다.

미소 회전 값 alpha, beta, gamma로 이동한다고 가정하여, Transformation Matrix M을 근사화해서 식을 아래와 같이 간단히 한다.

이를 N correspondences로 확장해서 본다.

 

 

 

Projective ICP

(스캐너에서 생성한 이미지 range에 투영하여 오류를 최소화하는 기법)와 다른 variants도 있다.

Robust kernels를 고려할 수도 있는 정렬을 찾으려고 시도한다는 의미이다.

= error minimization of promotion / the better deal with outliers

위와 같은 outlier 상황에서 매우 잘못된 aligned이 가능할 수 있는데, 이때 outlier ejection 기술이 도울 것이다.

 

[참고하기 좋은 주피터노트]

SVD 접근부터 least squares 접근까지 알려줄 것이다.

https://github.com/niosus/notebooks/blob/master/icp.ipynb

notebooks/icp.ipynb at master · niosus/notebooks

 

 

ICP is a key building block for laser-based SLAM

예를 들어, 모바일 플랫폼에 3D laser를 장착할 때, 환경의 일관된 모델을 구축하기 위해, 자율주행차 로봇 공학에 사용된다.

 

---

 

[ICP 알고리즘 비교]

- PCL library의 ICP, GICP, ICP Non Linear

- ICP, AA-ICP, Fast ICP, Robust ICP, ICP point-to-plane, Robust ICP point-to-plane, Saprse ICP, Sparse ICP point-to-plnae

---

[ICP를 사용할 때 고려할 점]

icp는 동영상 데이터를 하나로 뭉치는데 좋은 방법입니다. 다만 항상 icp가 최적으로 찾는다는 보장은 할 수 없습니다.
그러한 이유는 초기 매칭포인트가 어떻게 되냐, 리컨 성능, 포인트 클라우드의 랜덤성에 따라 그 성능이 크게 좌지우지 되기때문입니다.

따라서, icp가 잘 했는지 판단하는 판단 모듈도 필요하며, 실패했다면, 다른 방법(fast global registration 등) 이용하여 초기 매칭을 시도(re-localization) 한 뒤 최종 fine tuning입장에서 icp를 접근하는 것이 중요합니다.

속도측면에서는 icp가 느린 축에 속하므로 이를 빠르게 바꾸는 것도 다양하게 존재하는 것으로 압니다.
해당 기술에 대한 코드는 open3d 라이브러리에 잘 정리 되어있으니 참고하면 공부하시는데 도움 될 것 같습니다.

---

참고자료

https://www.youtube.com/watch?v=QWDM4cFdKrE (Cyrill Stachniss, Iterative Closest Point (ICP) - 5 Minutes with Cyrill)

https://www.youtube.com/watch?v=zNv92rtvvT0 (신동원, SLAM Night LIVE! 2 | 하이라이트 1편 | Iterative Closest Points)

https://owl-d.tistory.com/27 (ABOUT-D, ICP 알고리즘 비교)

https://www.facebook.com/groups/spatialaikr/permalink/2248855312140767/?m_entstream_source=group&_rdr (임규범, 김기상님, Spatial AI KR)