ICP(Iterative Closest Algorithm) 정리

ICP 알고리즘은 fine registration에서 거의 가장 기본이 되는 알고리즘이라고 볼 수 있다.

ICP 알고리즘의 과정 자체는 복잡하지 않은데 늘 이해가 안됐던 부분이 center point로 translation을 한 이후에 두 frame상의 point들에 대해 outer product를 해서 나온 matrix의 SVD를 통해서 rotation을 구하는 부분이 ICP외에도 많이 등장하는데 이전부터 이해가 잘 안됐어서 이 부분과 함께 이 참에 정리를 해보려고 한다.

ICP의 기본적인 flow는 다음과 같다

1. Initialize error with inf ($\epsilon = \infin$)

2. Calculate correspondence (get closest pair $<q_i, q_i'>)$

3. Calculate alignment (from $q_i', q_i$ get $R, T$)

4. Apply alignment ($p = Rp + T)$

5. Update error $(\epsilon' = ||p' - p || ,\epsilon = \epsilon')$

6. If($\epsilon$ > Threshold)

   else

자세히 설명을 하자면,

1. error minimize 문제이기 때문에 당연히 error를 infinity로 initialize한다.

2. source frame의 point들을 $p$ 라하고 target frame의 point들을 $p'$ 이라 하면 이 두 frame 각각에 대해서 center point를 구하고 그 center point를 origin으로 하도록 point들을 각각 translation하여 새롭게 $q, q'$ 을 정의하고 이 두 point들 사이의 closest pair $<q_i, q_i'>$를 계산한다.

3. Paper: Least-Squares Fitting of Two 3-D Point Sets

   결국 이 ICP문제는 두 frame사이의 rotation $R$과 translation $T$를 구해서 $||p' - Rp - T||$를 minimize 하는 문제인데 여기에서 $T$의 경우 $R$을 구하면 $T = p ' -Rp$ 를 통해서 쉽게 구할 수 있다. 그래서 결국은 center point를 기준으로 frame을 이동하고 $||q' - Rq||$를 minimize하는 문제로 볼 수 있다.

   이 문제를 자세히 보면 2에서 구한 pair를 사용하여 $\Sigma{||q_i' - Rq_i||}^2$$=$ $\Sigma(^tq'_i + q_i^tq_i-2^tRq_i)$가 되고 여기서 첫번째와 두번째 term은 고정값이므로 3번째 term인 $F = \Sigma ^tRq_i$ 를 maximize하는 문제로 바꿀 수 있다.

   maximization problem의 최종 형태는 $F = Tr(\Sigma Rq_i^t) = Tr(RH)$ $(H=\Sigma q_i^t)$ 가 된다. 여기서 한가지 $Lemma$를 소개하면

   $Lemma:$ 모든 positive definite matrix $AA^t$, orthonormal matrix $B$에 대해서 $Tr(AA^t) > Tr(BAA^t)$ 를 만족한다.

   우선 $H$에 SVD를 적용하면 $H = U\Lambda V^t$ 로 나타낼 수 있고 여기서 어떤 $X=VU^t$라 하면, $XH=VU^tU\Lambda V^t=V\Lambda V^t$가 된다. 이 $XH$는 symmertic이면서 positive definite이므로 위의 $Lemma$에 의해 모든 3x3 orthonormal matrix $B$에 대해서 $Tr(XH)\ge Tr(BXH)$ 를 만족하므로 이 $X$가 결국 위의 $F$를 maximize하는 $R$값이 된다. 이렇게 구한 $R$ 값으로 앞서 말했듯 $T$도 구했다.

   (사족: 결국은 outer product자체에 의미가 있다기 보다는 least square problem을 푸는 과정 중에 나오는 term이고 이를 maximize하는 과정중에서 SVD를 활용을 했던 것이었다. 여기서 보다 physical한 의미를 찾으면 좋겠지만 일단은 왜 나오지는지를 이해한걸로 만족하고 넘어가보려 한다.)

4. 3에서 구한 $R, T$를 사용하여 source frame을 transformation($p\leftarrow Rp+T$)하고

5. $\epsilon' =||p'-p||$의 값을 구해서 $\epsilon'$이 기존 $\epsilon$보다 작으면 값을 update한다.

6. 5에서 update한 $\epsilon$이 Threshold(end condition)보다 작으면 ICP를 종료하고 아니면 2로 돌아가서 다시 같은 과정을 계속 반복한다.

Uploaded by Notion2Tistory v1.1.0