포인트의 3d 변환에서의 자코비안을 활용한 3d 포즈 최적화 (3d points ICP optimization)

이전 포스팅에서 자코비안 행렬을 구하는 방법을 정리하였고, 이번 포스팅에서는 구해진 자코비안을 활용해서 포즈(state)를 최적화 하는 내용을 정리하고자 한다.

 

일단, https://lss0815.tistory.com/10 에서 우리는 자코비안 행렬을 구하였기 때문에 state $x$와 3d 포인트들로 자코비안 행렬 $H$를 계산할 수 있다고 가정한다.

자코비안 행렬은 $x$에서의 도함수 행렬이고, state $x$에서 선형적으로 근사하여서 에러를 0으로 만드는 dx를 찾으면 실제 에러를 0에 가깝게 만들 더 나은 state를 계산할 수 있다.

$$e(x + dx) \approx e(x) + H(x)dx$$

$$e(x) + H(x)dx = 0 $$

$$H(x)dx = -e(x)$$

 

H(x)는 3$n \times 6$의 행렬이므로, 유사 역행렬(pseudo inverse matrix)로 dx를 구할 수 있다.

$$H(x)dx = -e(x)$$

$$H(x)^TH(x)dx = -H(x)^Te(x)$$

$$dx = -(H(x)^TH(x))^{-1}H(x)^Te(x)$$

 

이를 이용하여 state를 업데이트 하면 되는데, 먼저 자코비안 행렬을 계산할 때에 $x + dx$에 대한 에러를 어떻게 계산했는지를 알아야한다.

$$ e(x + dx)  = R(dr)R(r)p' + t + dt - p $$

 

지난 포스팅에서 언급하지 않았지만, 위와 같이 계산하였을 때는 $x + dx$를 다음과 같이 계산해야 한다.

$$ x + dx = \begin{pmatrix} log(R(dr)R(r)) \\ t + dt \\  \end{pmatrix} $$

여기서, $R(r)$은 3x1 회전 벡터(rotation vector) $r$을 3x3 회전 행렬(rotation matrix)로 바꾸는 함수이고, $log(R)$은 3x3 회전 행렬을 3x1 회전 벡터로 바꾸어주는 함수를 의미한다.

 

일반적으로 위의 과정을 거친 $x +dx$는 $x$에서보다 작은 에러값을 가지게 되는데, 이를 반복하면 최적해에 근사할 수 있다.

Tip. $x$에 $x+dx$를 대입하고, 다시 새로운 $dx$를 계산하여 새로운 $x+dx$를 만들면 된다.

Tip. 위의 방법을 뉴턴 방법(Newtons' method)이라고 한다.

 

랜덤 포인트를 이용해 3d pose를 최적화하는 코드를 작성하여 테스트해보았는데, 이 때 error 벡터들의 크기는 다음과 같았다.
Initial error vector norm: 2.45649

최적화 1번, error vector norm: 0.00815123

최적화 2번, error vector norm: 4.4849e-07

이와 같이, 최적화를 반복할수록 에러값이 줄어드는 것을 볼 수 있다.

 

 

추후에, 다양한 최적화 방법에 대해 정리하도록 하겠습니다.

코드의 경우, https://github.com/lss0815/filter_study/blob/master/pose_diff/src/main.cpp에서 확인하실 수 있습니다.

미흡한 내용의 글 읽어주셔서 감사합니다. 혹시 오류가 있거나 궁금하신게 있으시다면 댓글로 남겨주시고 글에 반영 및 답변드리도록 하겠습니다. 감사합니다.