Reference: Least-Squares Fitting of Two 3-D Point Sets
ICP 알고리즘은 fine registration에서 거의 가장 기본이 되는 알고리즘이라고 볼 수 있다.
ICP 알고리즘의 과정 자체는 복잡하지 않은데 늘 이해가 안됐던 부분이 center point로 translation을 한 이후에 두 frame상의 point들에 대해 outer product를 해서 나온 matrix의 SVD를 통해서 rotation을 구하는 부분이 ICP외에도 많이 등장하는데 이전부터 이해가 잘 안됐어서 이 부분과 함께 이 참에 정리를 해보려고 한다.
ICP의 기본적인 flow는 다음과 같다
- Initialize error with inf ($\epsilon = \infty$)
- Calculate correspondence (get closest pair $<q_i, q_i’>)$
- Calculate alignment (from $q_i’, q_i$ get $R, T$)
- Apply alignment ($p = Rp + T)$
- Update error $(\epsilon’ = \Vert p’ - p \Vert, \epsilon = \epsilon’)$
If($\epsilon$ > Threshold) {back to 2.}
else {end}
자세히 설명을 하자면,
- error minimize 문제이기 때문에 당연히 error를 infinity로 initialize한다.
- 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’>$를 계산한다.
Paper: Least-Squares Fitting of Two 3-D Point Sets
결국 이 ICP문제는 두 frame사이의 rotation $R$과 translation $T$를 구해서 $\Vert p’ - Rp - T\Vert$를 minimize 하는 문제인데 여기에서 $T$의 경우 $R$을 구하면 $T = p’ -Rp$ 를 통해서 쉽게 구할 수 있다. 그래서 결국은 center point를 기준으로 frame을 이동하고 $\Vert q’ - Rq\Vert$를 minimize하는 문제로 볼 수 있다.
이 문제를 자세히 보면 2에서 구한 pair를 사용하여 $\Sigma{\Vert q_i’ - Rq_i\Vert}^2$$=$ $\Sigma({q_i’}^tq’_i + q_i^tq_i-2{q_i’}^tRq_i)$가 되고 여기서 첫번째와 두번째 term은 고정값이므로 3번째 term인 $F = \Sigma {q_i’}^tRq_i$ 를 maximize하는 문제로 바꿀 수 있다.
maximization problem의 최종 형태는 $F = Tr(\Sigma Rq_i{q_i’}^t) = Tr(RH)$ $(H=\Sigma q_i{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한 의미를 찾으면 좋겠지만 일단은 왜 나오지는지를 이해한걸로 만족하고 넘어가보려 한다.)
- 3에서 구한 $R, T$를 사용하여 source frame을 transformation($p\leftarrow Rp+T$)하고
- $\epsilon’ =\Vert p’-p\Vert$의 값을 구해서 $\epsilon’$이 기존 $\epsilon$보다 작으면 값을 update한다.
- 5에서 update한 $\epsilon$이 Threshold(end condition)보다 작으면 ICP를 종료하고 아니면 2로 돌아가서 다시 같은 과정을 계속 반복한다.