최근 GIST에서 인턴을 하면서, 주제를 하나 연구중에 엎었다.. (원래는 3월 IROS 투고 예정) 그러다가 지식 부채가 크다는 것을 깨닫고 다음에 연구해 보고싶은 센서인 radar를 공부하고자 RADAR SLAM: Radar based Large-Scale SLAM in All Weathers 을 구현해보고자 하며 최근에 큰 지식부채를 하나 갚고, 정리하고자 한다.

Caution: 부족한 부분이 많고, 잘못된 정보가 있을것입니다.

Pose Estimation

SLAM이나 localization을 연구하고 공부하는 사람들은 위 주제를 가지고 연구하는 분들도 많다. 내가 느끼기에 pose estimation을 위한 알고리즘들을 solver로 표현한다. 사실 open formulation(?) 이 아닌 closed formulation을 이용한 방법들도 많아서 optimize를 하지 않고 compact하게 구하는 방법들도 있다고 하는데, GNC 논문을 읽어보면서 어떤 차이가 있고, 어떤게 있는지를 읽어보며 정리하는것도 좋을 것 같다.

아무튼, 이 subsection에서는 RadarSLAM 논문에서 svd 를 이용해 pose estimation 을 어떤 방식으로 진행했는지에 대해 간단히 설명한다.

우선 논문에서는 SURF와 같은 visual feature를 radar이미지 에서 뽑아온다.

radar 가 왜 이미지에요..? 라고 할 수 있겠지만 일단 넘어가자. 추후에 다른 포스트에서 다루도록 할 예정

뽑아온 feature들에서 maximum clique를 이용하여 outlier rejection을 진행하고, 남아있는 inlier들을 비교하여 pose estimation을 진행한다.

여기까지가 pose estimation을 위한 준비 단계이다. 이후 inlier들을 이용하여 SVD를 진행하고 initial pose를 구한다. 이렇게 구한 initial pose를 solver에 넣어 optimization을 진행하고, reprojection error 를 통해 optimal한 pose를 구하는게 전체 과정이다.

이렇게 보면 굉장히 급발진한 느낌이지만 전체 process를 읽어만 봤다고 생각하고 위 문단의 이후 부분부터 설명을 시작해 보도록 하겠다.

Problem definition

SVD를 가져다가 사용하기에 앞서, problem definition을 진행하고 설명하도록 하겠다.
우리가 solver를 통해 얻고싶은건 $T_{kt}$이다. k는 keyframe이고, t 는 t시간에서의 값을 의미한다.

$T_{gk}$ 는 key frame에서의 global pose인데 알고있다고 가정한다(keyframe의 좌표를 저장해두면 되니까). 모두 $SE(n)$ 친구들이므로 두 transformation matrix를 곱하면 $T_{gt}$를 얻어 t시간의 global 좌표계 기준 위치를 얻을 수 있다.

그래서 $T_{kt}$를 얻기 위해서 풀어야 하는 식은

처럼 나타낼 수 있다. 그런데, 지금 논문에서는 feature방식이고, t - time에서의 point와 key frame에서의 point를 모두 각 프레임기준 중점을 기준으로 평행이동 하면 rotation을 구하고, 원래 point 들간의 matching으로 translation을 구할 수 있다.

그러면, translation을 구하는 문제를 rotation 을 구하는 문제로 바꿀 수 있고 공식으로 나타내면.

로 rotation을 위한 문제로 바꿀 수 있다.

SVD

SVD 는 singular value decomposition으로, 어떤 matrix를 3개의 matrix 로 분해하는 방법이다.

어떤 matrix가 있다면 해당 matrix를 Orthgonal한 matrix 2개와 scale 을 나타내는 diagonal matrix의 곱으로 나타내는 방법이다.

수식으로 간단히 나타내자면:

Orthogonal한 matrix 는 det의 값에 따라 rotation혹은 reflection을 나타내게 된다. 우리는 pose를 나타내기 위한 $U$, $V$를 찾을 예정이므로 det를 1이 되도록 맞춰준다.

Rotation and SVD

SVD와 rotation을 볼때, 유일하게 감이 잡히는건 U와 V가 orthogonal하다는 것이다. 'SVD로 풀었다'라는 말에서 고민해보고 결국 혼자는 못풀어내서, 힌트를 보고 나니 쉽게 풀렸다.(gpt한테 부탁했다)

우선 위의 problem definition의 rotation optimize식을 전개하다 보면 더 직접적인 힌트가 나온다.

힌트를 보기 위해 $\sum$ 내부의 $||\cdot ||_2$을 전개해보자.

L2 norm이므로 식을 풀어보면 (편의를 위해 아래첨자는 생략한다)

위의 정리된 식에서 두 스칼라는 R의 영향을 주지않는 값이므로 제외하고, 다시한번 위의 식을 정리하면

이 된다.

위 식에서 $\sum(p^k_i)^Tp^t_i$는 알고있는 값이므로 $H$라 정의하고, 문제를 다시 적으면,

이 된다.

위의 $H$에 SVD를 적용한다. 왜냐하면, orthogonal한 값들에 singular value matrix를 남겨서 trace연산에서 최적값을 나타내기 쉬워지도록 하기 위함이다.

$H = U\Sigma V^T$라 하면, 위의 argmax식은 다시

로 나타낼 수 있고, $R$, $U$, $V^T$모두 orthogonal하고 곱이 가능한 형태의 행렬이고, 성질들을 이용하여 정리하면

처럼 나타낼 수 있다. 이때, $\Sigma$는 singular value matrix이므로 $RUV^T$가 identity matrix일때 max값을 갖는다 따라서,

이 된다. 결국 SVD를 계산하면 Ideal한 Rotation을 구할 수 있고 해당 rotation을 기준으로 translation도 구해낼 수 있다.