Large-Scale Direct SLAM for Omnidirectional Cameras
[cite]10.1109/IROS.2015.7353366[/cite]
1. Introduction
Visual Odometry (VO)나 Simultaneous Localization and Mapping (SLAM) 문제에서 Fov가 넓으면 더 많은 정보를 얻을 수 있다는 것은 당연한 일입니다. 하지만 많은 연구들은 여전히 핀홀 카메라 모델을 대상으로 하고 있습니다. PTAM의 ATAN 모델과 같은 연구들은 radial 왜곡 모델을 적용하기도 하였지만, 180도가 넘는 omnidirectional 카메라에 대해서는 적용할 수가 없습니다.
여기에서는 기존의 LSD-SLAM을 확장하여 generic omnidirection 카메라 모델을 적용하고자 합니다. 이는 fish-eye와 catadioptric 카메라를 포함합니다. 실험에서는 185도의 FoV를 갖는 카메라에서 평가하엿으며, 기존의 LSD-SLAM과 마찬가지로 실시간 처리가 가능하였습니다.
논문의 기여는 다음과 같습니다.
- Omnidirectional 카메라 모델에서 direct image alignment를 수행한 것
- Omnidirectional 이미지에서 직접 stereo 계산을 한 것
2. Camera Models
Notation
행렬은 $ \mathbf{R} $ , 벡터는 $\mathbf{x}$로 표현하고자 합니다. 이미지 위의 점은 $\mathbf{u} = [u, v]^T = \mathbb{R}^2 $ 으로 픽셀 위치를 나타내며, $\mathbf{x} = [x, y, z]^T \in \mathbb{R}^3 $은 3D 점의 위치를, $\mathbf{\tilde{x}} = [\mathbf{x} 1] $는 호모지니어스 좌표를, $[\cdot]_i$_는 행렬이나 벡터의 i번째 행을 나타냅니다.
일반적인 카메라 모델에서 3차원 점을 이미지 픽셀 좌표계로 옮기는 프로젝션을 $\pi: \mathbb{R}^R \rightarrow \Omega $로 표시합니다. 이 대 카메라 좌표계의 중심은 $\mathcal{C}$입니다. 그리고 이것의 백프로젝션을 $\pi^{-1}$로 표시하고자 합니다. 백프로젝션을 하기 위해서는 깊이에 대한 정보가 필요한데, 여기서는 역 깊이 d를 사용하도록 합니다.
Omnidirectional 모델에서는 위의 내용에 z 축 방향으로 추가적인 계산을 하게 됩니다.
A. Pinhole Model
핀홀 모델에서는 각 점들이 z=1인 평면에 투영되어 이미지가 얻어집니다.
$$\pi_p (\mathbf{x}) = \begin{bmatrix} f_x &\ 0 \\ 0 &\ f_y \end{bmatrix} \begin{bmatrix} x/z \\ y/z \end{bmatrix} + \begin{bmatrix} c_x \\ c_y \end{bmatrix}$$
이 모델에서는 3차원에서의 직선은 이미지에서도 직선이라는 성질이 있고, 이를 이용하면 계산들이 비교적 간단해집니다. 하지만 넓은 FoV에 대해서 다루지 못한다는 단점이 잇으며, 특히 이미지 가장자리로 갈 수록 왜곡되고 해상력이 적어지며, 180도가 넘어가면 투영조차 못한다는 단점이 있습니다.
B. Array of Pinhole Camera
여러개의 가상의 핀홀 카메라를 이용하여 FoV를 확장하는 방식입니다. 카메라 중심을 공유하는 여러 핀홀 카메라를 다른 방향으로 배치시킨 다음, 각각의 카메라에 대해 프로젝션 시킵니다.
$$\pi_{mp}(\mathbf{x}) = \pi_{p_{i(\mathbf{x})}}(R_{i(\mathbf{x})}\mathbf{x})$$
$i(\mathbf{x})$는 [1, k]개의 카메라를 지정하는 함수입니다. 카메라의 방향과 공간을 어떻게 나눌지는 어떠한 방법으로도 가능하나 여기서는 6개의 동일한 크기로 나누어, 큐브형태로 나타내었습니다.
이 모델은 핀홀 모델과 마찬가지로 부분적으로나마 호모지니어스 좌표계에 대하여 선형이며, 적당한 해상력을 제공할 수 있습니다. 하지만 이미지 공간에서 서로 끊기는 현상이 발생합니다.
C. Central Omnidirectional Camera: Unified Model
Catadioptric 혹은 dioptric omnidirectional 카메라를 모델링하기 위한 여러 방법이 있으나 공통적으로 아래의 성질을 만족시켜야 합니다.
- 실제 이미지 장치의 광각을 정확히 기술할 수 있어야 한다.
- 캘리브레이션이 쉬워야 한다.
- 백프로젝션 $pi^{-1}$이 closed-form으로 설명할 수 있어야 한다.
추가적으로 여기에서는 LSD-SLAM에 적용하기 위하여 계산에 필요한 비용 또한 고려야하여야 했습니다.
먼저 radial에 대해 대칭인 함수로 비선형적인 대응 관계를 만들어진 방법은 정확한 결과를 얻을 수 있으나 백프로젝션이 closed-form으로 나오지 않습니다.
대신 여기서는 [18], [19], [20]에서 사용한 모델을 이용하였습니다. 이 모델은 총 2번의 프로젝션을 하게 됩니다. 첫번째는 3차원 점을 카메라 중심의 unit sphere로의 프로젝션입니다. 다음은 카메라 중심을 $-\xi$ 만큼 z축으로 이동한 뒤, 핀홀 프로젝션을 통하여 이미지 플레인으로 투영합니다. 따라서 기존의 focal length, principal center에 $\xi$를 더하여 총 5개의 파라미터를 사용하게 됩니다.
프로젝션은 다음과 같이 이루어집니다.
$$pi_u (\mathbf{x}) = \begin{bmatrix} f_x \frac{x}{z + ||\mathbf{x}|| \xi} \\ f_y \frac{y}{z + ||\mathbf{x}|| \xi} \end{bmatrix} + \begin{bmatrix} c_x \\ c_y \end{bmatrix}$$
백프로젝션 함수는 다음과 같이 closed-form우로 계산됩니다.
$$\pi^{-1}(\mathbf{u}, d) = \frac{1}{d} ( \begin{bmatrix} \frac{\xi + \sqrt{1 + (1- \xi^2)(\tilde{u}^2 + \tilde{v}^2)}}{\tilde{u}^2 + \tilde{v}^2 + 1} \end{bmatrix} \begin{bmatrix} \tilde{u} \\ \tilde{v} \\ 1 \end{bmatrix} - \begin{bmatrix} 0 \\ 0 \\ \xi \end{bmatrix})$$
$$\tilde{u} = (u-c_x) / f_x, \tilde{v} = (v - c_y) / f_y$$
만약 $xi=0$이면 일반적인 핀홀 카메라 모델이됩니다. 여기에 일반성을 더하기 위해 radial-tangential 왜곡을 더하여 렌즈의 공차를 보정하도록 하였습니다.
3. Direct Omnidirectional SLAM
Notation
$ D: \Omega_d \rightarrow \mathbb{R}^+$은 키 프레임의 역 거리(distance) 맵을 나타냅니다. 표현을 쉽게 하기 위해 $ \mathfrak{se}(3)$는 크기 6의 벡터 $\mu$로 나타내었습니다. Lie algebra에 따라 Lie group으로의 변환은 exp, log를 사용하였습니다.
$$\mu_1 \circ \mu_2 = \log(\exp(\mu_1) \cdot \exp(\mu_2))$$
또한 이에 대응하는 회전, 이동은 $R_\mu, t_\mu$로 표시합니다.
A. Method Overview
LSD-SLAM과 마찬가지로 연속적으로 키 프레임으로 이루어진 포즈 그래프를 구성해가며, 각 키 프레임에서의 semi-dense한 역 거리 맵을 계산합니다. 이 맵은 그래디언트가 충분히 큰 픽셀에 대해서만 정보를 갖고 있습니다. 기존의 LSD-SLAM과는 달리 역 깊이가 아닌 역 거리 $d = ||\mathbf{x}||^{-1}$를 사용하였습니다. 이는 unified model에 따른 것입니다.
B. Omnidirectional Direct Image Aligment on SE(3)
새로운 프레임 $I_{new}$과 가장 가까운 키 프레임 $I_{Kf}$, 그리고 그것의 역 거리 맵 $D_{Kf}$는 포토메트릭 에러의 최소화를 통하여 계산됩니다.
$$E^{frame}(\mu) = \sum_{u \in \Omega_d} \rho([\frac{r^I_u(\mu)}{\sigma_{r^I_u(\mu)}}]^2)$$
$\rho$는 Huber norm이고,
$$r^I_u(\mu) = I_{Kf}(u) - I_{new}(\pi(\omega(\mu, u)))$$
$$\omega(\mu, u) = R_\mu \pi^{-1}(u, D_{Kf}(u)) + \mathbf{t}\mu$$
입니다.
여기서 $\omega$는 백프로젝션한 뒤, $\mu$에 의해 이동시킨 점을 나타냅니다.
Weighted least-squares 문제는 coarse0to-fine 방법에 따라 re-weighted Levenberg-Marquad 알고리즘을 통하여 최소화문제를 풀어냅니다. 각 반복에 대하여 좌측 곱 반복을 따랐습니다.
$$\delta \mu^{(k)} = (\mathbf{J}^T \mathbf{ W J} + \lambda \text{diag}(\mathbf{J}^T \mathbf{W J}))^{-1} \mathbf{J}^T \mathbf{Wr}$$
여기서 $\mathbf{r}$은 이미지에서의 residual을 벡터로 만든 것이고, $\mathbf{W}$는 가중치를 나타내는 대각행렬입니다. $\mu$에 대한 residual의 Jacobian은 다음과 같습니다.
$$\mathbf{J} = \frac{\partial \mathbf{r} ( \epsilon \circ \mu^{k})}{\partial \epsilon}$$
$$\mu^{k+1} = \delta \mu^{k} \circ \mu^{k}$$
체인 룰에 따라 $J_u^{fwd} = -J_{I_new}|_\pi J_\pi | _\omega J_\omega|_\mu $ 로 풀어낼 수 있습니다.
각 Jacobian들은 $\mu^{k}$에 종속적이기 때문에 매 반복마다 새로 계산하여야 합니다. 이는 계산 시간의 대부분을 차지하며 더욱이 unified model에서는 이 계산이 더 복잡하게 됩니다.
이를 해결하기 위하여 inverse compositional formulation을 이용하였습니다. 레퍼런스 이미지에다가 계속 변화를 주는 대신, 그에 해당하는 변화를 역으로 키 프레임에 주었습니다. 따라서 아래의 식은,
$$I_{Kf}(\mathbf{u}) - I_{new}(\pi(\omega(\epsilon \circ \mu^{k} , \mathbf{u})))$$
다음과 같이 변화됩니다.
$$I_{Kf}(\pi(\omega(\epsilon, \mathbf{u}))) - I_{new}(\pi(\omega(\mu^{(k)}, \mathbf{u})))$$
따라서 전체 Jacobian은 다음과 같습니다.
$$J_u^{bkwd} = J_{I_{Kf}}|_\pi J_\pi|_\omega J_\omega|_0$$
$\omega = \omega(0, \mathbf{u}), \pi = \pi(\omega(0, \mathbf{u}))$이기 때문에 이는 $\mu^{(k)}$에 비 종속적이고 각 피라미드 레벨에 대하여 미리 계산할 수 있어 계산을 줄일 수 있습니다.
업데이트 방법이 바뀌었기 때문에 우측 곱 형태를 사용하여야 합니다.
$$\mu^{k+1} = \mu^{(k)} \circ (-\delta \mu^{(k)})$$
C. Omnidirectional Direct Image Alignment on Sim(3)
LSD-SLAM과 동일합니다.
D. Semi-Dense Depth Map Estimation
LSD-SLAM과 마찬가지로 sum of squared differences (SSD)를 사용하였으며, 에피폴라 라인 위의 인접한 5개 픽셀에 대하여 계산을 수행하였습니다. 에피폴라 탐색 범위는 역 거리 값에 따른 $[d - 2 \sigma_d, d+ 2 \sigma_d ]$는 내에서 수행됩니다. 이러한 접근법은 잘못된 매칭을 현저하게 줄여줍니다.
1) Non-Rectified Stereo
Unified model에서 스테레오 계산을 할 때에는 에피폴라 라인이 아니라 커브로 나타납니다. 정확히 말하면 unit sphere위에서의 geodesic의 핀폴 프로젝션으로 코닉입니다. 커브를 따라 1픽셀씩 증가하여 비교하기 위해 다음의 방법을 사용하였습니다.
키 프레임의 한 점에 대하여 두개의 점 $\mathbf{p}_0, \mathbf{p}_\infty \in \mathbb{R}^3$을 정의합니다. 이는 각각 레퍼런스 이미지의 unit sphere 위의 점이며, 역 거리 탐색 범위에 경계에 해당하는 지점입니다.
$$\mathbf{p}_0 = \pi_s ( \mathbf{R} \pi_u^{-1} ( \mathbf{u, d_{max})+\mathbf{t}})$$
$$\mathbf{p}_\infty = \pi_s ( \mathbf{R} \pi_u^{-1} ( \mathbf{u, d_{min})+\mathbf{t}})$$
다음 둘을 잇는 직선 위의 점을 표현합니다.
$$\mathbf{p}_L(\alpha) = \alpha \mathbf{p}_0 + (1-\alpha) \mathbf{p}_\infty$$
다음 이 직선 위의 점들을 투영하면 에피폴라 커브로 투영됩니다.
$$\mathbf{u}_L (\alpha) = \pi_u(\mathbf{p}_L(\alpha))$$
$\mathbf{u}_L(0)$으로부터 시작하여 $\alpha$를 증가시켜가며 에피폴라 커브를 탐색해나갑니다. 탐색 단위를 1픽셀로 제한하기 위하여 $alpha$의 증가량으로 $\delta \alpha = ||\mathbf{J}_{\mathbf{U}_L}|_\alpha||^{-1}$을 사용하였습니다. 하지만 이 값은 매 탐색마다 재 계산하여야 합니다.
직선보다는 계산량이 많긴 하지만, 프레임이 계산될 수록 탐색의 범위가 줄어들게 되므로 실질적인 계산은 크게 증가하지 않습니다.
2) Pre-Rectified Stereo
그럼에도 큰 탐색 범위를 가지는 경우 계산량이 많은 것은 사실입니다. 따라서 그 대안으로 앞의 Array of pinhole cameras 모델을 따라 미리 rectified하는 방법을 사용할 수 있습니다. 각 카메라에 대하여 핀홀 모델을 사용하므로 선 단위 탐색을 그대로 사용할 수 있습니다.