[cite]10.1007/978-3-319-10605-2[/cite]

1 Introduction

Monocular SLAM의 한가지 장점이자 과제는 scale-ambiguity가 있다는 점입니다. 이 때문에 시간이 지날 수록 scale drift현상이 나타나 에러의 주요한 원인이 됩니다. 하지만 이러한 성질을 이용하여 스케일이 다른 환경에 대하여 자연스러운 스케일 변환이 가능하도록 만들기도 합니다.

1.1 Related work

Feature-Based Methods

피쳐 기반의 방법은 전체 문제를 두개로 나눕니다. 하나는 이미지로부터 피쳐를 추출하는 단계, 다른 하나는 피쳐만으로부터 카메라 위치와 장면의 구조를 계산하는 단계입니다.

이러한 접근법에 기인하여 피쳐 타입에 의한 정보만 활용할 수 있게 됩니다. 즉 피쳐가 아닌 나머지 정보들은 사용할 수 없게 됩니다.

Direct Methods

이미지 픽셀 값을 사용하여 직접 구조를 계산하는 방법입니다. 높은 정확도와 강인함을 가집니다. 이 방법은 주로 RGB-D를 이용하거나 스테레오 센서를 이용하는 것이 일반적이고 최근 단안 카메라에서의 논문들이 제안되었습니다.

Pose Graph Optimization

이것은 SLAM에서 잘 알려진 테크닉으로 전체 맵을 생성하는 방법입니다. 월드는 여러 키프레임으로 표현되며 키 프레임이 가지는 포즈와 포즈를 이어 나타냅니다.

1.2 Contributions and Outline

이 논문에서는 Large-Scale Direct monocular SLAM (LSD-SLAM)을 소개하고자 합니다. 이는 일시적인 카메라 움직임 뿐만 아니라 전체 맵 또한 생성합니다.

2 Preliminaries

Notation

행렬은 굵고 대문자로, 벡터는 굵고 소문자로 나타냅니다. 행렬의 n goddms $[\cdot]_n$으로 나타냅니다. 이미지는 $I$, 역깊이(1/D)는 $D$로 나타내고, 이것의 분산은 $V$로 사용합니다. $\Omega$는 normalized pixel 좌표계입니다. 또한 d는 깊이 z의 역수를 나타냅니다.

2.1 3D Rigid Body and Similarity Transformations

3D Rigi Body Transformations

3D rigid body transformation $G \in SE(3)$는 3D 회전과 이동을 의미합니다.

$$G = \begin{bmatrix} R &\ t \\ 0 &\ 1 \end{bmatrix}$$

$$R \in SO(3), t \in R^3$$

이후, 카메라 자세의 간단한 표기를 위해서 Lie-algebra인 $\xi \in se(3)$를 사용합니다. 이를 SE(3)로 변환하는 방법은 exponential 을 사용하는 것입니다.

$$G = \exp_{se(3)}(\xi)$$

또한 이것의 반대는 log를 사용하면 됩니다.

$$\xi = \log_{SE(3)} (G)$$

$\xi \in R^6$는 6차원의 벡터로 나타낼 수 있습니다. j 프레임에서 i 프레임의 움직임 변화는 $\xi_{ji}$로 나타냅니다. 편의를 위해 두 변환의 연속은 $\circ$로 나타냅니다.

$$\xi_{ki} = \xi_{kj} \circ \xi_{ji} = \log_{SE(3)}(\exp_{se(3)}(\xi_{kj})\exp_{se(3)}(\xi_{ji}))$$

$w$는 3D projective 워프 변환을 나타냅니다. 이는 이미지 점 $p$와 그것의 역깊이 $d$를 $\xi$로 이동한 카메라에 투영합니다.

$$w(p, d, \xi) = (x'/z', y'/z', 1/z')$$

$$(x', y', z', 1) = \exp_{se(3)}(\xi)(p_x/d, p_y/d, 1/d, 1)$$

3D Similarity Transformations

3D Similarity Transform $ S \in Sim(3)$는 회전, 스케일, 이동을 나타냅니다.

$$S = \begin{bmatrix} sR &\ t \\ 0 &\ 1 \end{bmatrix}, R \in SO(3), s \in R^+$$

마찬가지로 간단한 표현을 위하여 $\xi \in sim(3) $으로 표현하며, 이는 앞의 것에서 1이 추가된 7의 degree of freedom을 가집니다. exp, log, 연속된 표현은 앞의 SE(3)에서와 같습니다.

2.2 Weight Gauss-Newton Optimization on Lie-Manifold

두개의 이미지는 Gauss-Newton minimization에 의해서 최소화됩니다. 이때 에러는 photometric error를 사용합니다.

$$E(\xi) = \sum_i ( I_{ref}(p_i) - I(w(p_i, D_{ref}(p_i), \xi)))^2 = \sum_i r_i^2$$

이 수식은 카메라 이동 $x_i$가 주어졌을 때, 3D warp function $w$에 의해 이동한 지점과 실제 이동한 지점간의 차이를 나타내며, 각 픽셀에 대하여 i.i.d 인 maximum-likelihood estimator라고 볼 수 있습니다.

초기 카메라 자세 $\xi^{(0)}$에서 시작하여 각 반복마다 좌측에 증가량 $\delta \xi^{(n)}$을 곱해가면서 Gauss-Newton second-order approximation를 이용한 E의 최소점을 찾습니다.

$$\xi^{(n+1)} = \delta \xi^{(n)} \circ \xi^{(n)}$$

$$\delta^{(n)} = -(J^TJ)^{-1}J^T r(\xi^{(n)}), J = \frac{\partial r(\epsilon \circ \xi^{(n)})}{\partial \epsilon}|_{\epsilon = 0}$$

여기서 $J$는 좌측 곱으로 만들어진 현재 자세에서의 residual을 쌓아놓은 벡터 $r$의 편미분 행렬입니다. $J^T J$는 E의 Hessian의 Gauss-Newton approximation입니다.

가려짐이나 반사 등의 아웃라이어를 잘 처리하기 위하여 다른 가중치를 부여하는 방버을 사용합니다. 따라서 이는 반복적인 weighted least-squares 문제로 생각할 수 있습니다. 가중치 행렬 $W = W(\xi^{(n)}) $는 residual이 클 수록 낮게 설정합니다. 따라서 에러 함수는 다음과 같이 변경됩니다.

$$E(\xi) = \sum_i w_i(\xi) r^2_i(\xi)$$

$$\delta \xi^{(n)} = -(J^T W J)^{-1} J^T W r(\xi^{(n)})$$

Residual이 서로 독립적이기 때문에 $(J^T W J)^{-1}$에서 사용되는 Hessian의 역행렬은 최종 결과물의 covariance $\sum_\xi$의 추정치로 사용됩니다.

$$\xi^{(n)} = \epsilon \circ \xi_{true}, \epsilon \sim N(0, \sum_\xi)$$

하지만 실제로는 residual은 높은 수준으로 서로 관련이 있기 때문에 여기서 제시한 $\sum_\xi$는 단시 lower bound를 나타내기만 합니다.

여기서는 좌곱 형태를 사용하였지만, 계산 결과는 우측곱을 사용하여도 상관이 없습니다. 하지만 위에서 계산되는 covariance의 추정치는 곱셈 순서에 영향을 받습니다. 이 covariance는 자세 그래프 최적화에서 사용되는데 이러한 부분을 고려야하여야 합니다.

2.3 Propagation of Uncertainty

입력값$X$에 대한 불확실성 때문에 함수$f(X)$의 결과물 또한 불확실성을 가집니다. 따라서 함수 $f(X)$의 covariance는 $X$의 covariance를 이용하여 추정됩니다.

$$\sum_f \approx J_f \sum_X J_f^T$$

3 Large-Sacle Direct Monocular SLAM

3.1 The Complete Method

알고리즘은 3가지 부분으로 구성되었습니다. 트래킹, 깊이 맵 추정, 맵 최적화입니다.

• 트래킹은 카메라 이미지들을 지속적으로 추적합니다. 즉 각 이미지에서의 카메라의 포즈 $\xi \in se(3)$를 현재 키 프레임에 대해 상대적인 표현으로 계산합니다. 이는 이전 프레임의 포즈를 기반으로 계산됩니다.
• 깊이 맵 추정은 추적된 프레임을 이용하여 현재 키 프레임을 refine하거나 더 나은 것으로 교체합니다. 카메라가 너무 멀리 움직여 버렸다면 현재 보이는 점들이 가장 많이 프로젝션 되는 가까운 키 프레임을 선택하여 교체할 것입니다.
• 키 프레임이 교체되면 기존의 깊이 맵은 더 이상 refine되지 않게 될텐데, 이를 글로벌 맵에 포함시키는 과정이 맵 최적화 과정입니다. loop closure와 scale-drift를 탐지 하기 위해 가장 가까운 키 프레임으로의 similarity transform $\xi \in sim(3)$을 계산하여 정렬 과정을 거칩니다.

Initialization

LSD-SLAM을 초기화하기 위해 첫 키 프레임은 랜덤 깊이 맵과 큰 분산을 가지도록 합니다. 초반 1초 동안 충분히 카메라가 움직이게 되면 알고리즘이 주어진 환경을 고정하고, 그 뒤 몇개의 키 프레임이 들어오면 올바른 깊이를 구성할 수 있게 됩니다.

3.2 Map Representation

맵은 키 프레임의 그래프로 표현됩니다. 각 키 프레임 $K_i$는 카메라 이미지 $I_i$와 역 깊이 맵 $D_i$, 역 깊이 맵의 분산 $V_i$으로 이루어집니다. 깊이 맵과 그 분산은 픽셀 저네가 아닌 일부로 구성되어있으며 전체 픽셀 영역 중 픽셀 값 그래디언트가 충분히 높은 부분 인근에만 해당됩니다. 다시 말하면 semi-desnse입니다. 키 프레임들간 에지 $\varepsilon_i$는 그들 사이의 상대적인 similarity transform과 그 covariance matrix $\Sigma_i$을 나타냅니다.

3.3 Tracking new Frames: Direct $se(3) $ Image Aligment

기존의 키 프레임 $K_i = (I_i, D_i, V_i)$로부터 시작합니다. 새로운 이미지 $I_j$와의 상대적인 자세 $\xi_{ji} \in se(3)$는 variance-normalized photometric error를 최소화하도록 계산됩니다.

$$E_p (\xi_{ji}) = \sum_{p \in \Omega_{D_i}} || \frac{r^2_p (p, \xi_{ji})}{\sigma^2_{r_p(p, \xi_{ji})}} ||_\delta$$

$$r_p(p, \xi_{ji}) = I_i(p) - I_j (w(p, D_i(p), \xi_{ji}) )$$

$$\sigma^2_{r_p(p, \xi_{ji})} = 2 \sigma^2_I + (\frac{\partial r_p (p, \xi_{ji})}{\partial D_i(p)})^2 V_i(p)$$

$$|| r^2 ||_\delta = \begin{cases} \frac{r^2}{2 \delta} & \text{if } |r| \leq \delta \\
|r| - \frac{\delta}{2} & \text{otherwise} \end{cases}$$

$\sigma^2_{r_p(p, \xi_{ji})} $는 앞에서 이야기한 covariance propagation을 이용하여 계산하고, 픽셀 값 노이즈 $\sigma^2_I$는 가우시안으로 가정합니다. 전체 최소화작업은 앞에서 언급했던 re-weighted Gauss-Newton optimization을 이용하게 됩니다.

이전에 제안되었던 Direct method 방법들에 반해 여기서는 깊이 추정에 노이즈를 반영하도록 구성되어있습니다.

3.4 Depth Map Estimation

Keyframe Selection

카메라가 기존의 맵에서 너무 많이 움직였을 경우에는 새로운 키 프레임을 생성합니다. 이 때에 기존의 추적된 이미지 들로부터 하나를 선택하는데, 현재 프레임으로부터 상대적인 거리와 각도를 조합하여 정의한 거리를 이용하여 thresholding 합니다.

$$dist(\xi_{ji}) = \xi_{ji}^T W \xi_{ji}$$

여기서 $W$는 weight들로 구성된 대각행렬입니다. 각 키 프레임은 역 깊이의 평균으로 구성된 스케일을 가집니다. 따라서 threshold 값 또한 현재 스케일에 대해 상대적인 값을 가지며 이는 작은 베이스라인을 가지도록 합니다.

Depth Map Creation

새로운 키 프레임이 선택되면, 이전의 키 프레임으로부터 점들을 프로젝션 하여 깊이 맵을 생성합니다. 다음 [9]에 나온 것처럼 spatial regularization을 한번 수행하고, outlier 제거 작업을 수행합니다. 그런 후, 깊이 맵은 역 깊이의 평균으로 스케일을 조정합니다. 이 스케일은 카메라 자세인 $sim(3)$에 직접 저장됩니다. 마지막으로 이전 키 프레임을 교체하여 앞으로 들어오는 프레임은 새로운 키 프레임을 이용하여 계산을 수행합니다.

Depth Map Refinement

추적된 프레임들은 키 프레임으로 사용되지 않고 현재 키 프레임을 refine하는데 사용됩니다. 작은 베이스라인을 가지는 이미지들로 많은 계산이 이루어지기 때문에 높은 정확도로 깊이가 계산될 것입니다.

3.5 Constraint Acquisition: Direct sim(3) Image Alignment

Direct Image Alignment on sim(3)

RGB-D나 Stereo-SLAM과는 다르게 monocular SLAM에서는 태생적으로 실제 scale을 알 수가 없습니다. 이러한 이유로 추적이 계속되는 동안 scale-drift를 야기하게 됩니다. 또한 모든 거리가 scale에 대하여 비례적으로 계산되기 때문에 threshold 기반의 아웃라이어 제거 방법이나 파라미터를 이용한 커널 방법은 작동하지 않게 됩니다. 여기서는 장면의 깊이야 추적의 정확성 간의 관계를 이용하였습니다.

각 키 프레임의 깊이 맵은 평균 역 깊이가 1이 되도록 스케일을 맞추었습니다. 그 결과 키 프레임간의 스케일이 그럴듯하게 조절될 수 있었습니다. 또한 loop-closures에서도 scale-drift를 탐지할 수 있었습니다.

이를 위해 sim(3)에서의 direct, scale-drift ware image alignment를 제시합니다. 이는 다른 스케일을 갖는 다른 키 프레임을 맞추는데 이용됩니다.

또한 photometric error $r_p$에 더하여 깊이 residual $r_d$를 사용하여 키 프레임간 깊이 차이를 적게 만들고 scale 변환을 추정하고자 하였습니다. 최종 에러 함수는 다음과 같습니다.

$$E(\xi_{ji}) = \sum_{p in \Omega_{D_i}} || \frac{r_p^2 (p, \xi_{ji})}{\sigma_{r_p (p, \xi_{ji})}^2} + \frac{r_d^2 (p, \xi_{ji})}{\sigma_{r_d (p, \xi_{ji})}^2} ||_\delta$$

$$r_d (p, \xi_{ji}) = [p']_3 - D_j ([p']_{1,2}$$

$$\sigma^2_{r_d (p, \xi_{ji})} = V_j ([p']_{1,2})(\frac{\partial r_d (p, \xi_{ji})}{\partial D_j ([p']_{1,2})})^2 + V_i (p) ( \frac{\partial r_d (p, \xi_{ji})}{\partial D_i (p)} )^2$$

$$p' = w_s (p, D_i(p), \xi_{ji})$$

최소화는 기존의 방법과 똑같은 방식으로 실행됩니다.

Constraint Search

새로운 키 프레임 $K_i$가 맵에 추가되면 loop closure가 될만한 키 프레임들 $K_{j1}, \cdots, K_{jn}$을 추려냅니다. 여기서는 가장 가까운 10개의 키 프레임들만을 찾아 appearance based mapping algorithm [11]을 이용하여 loop closures를 찾아냅니다. 잘못 추적된 loop closure의 삽입을 막기 위해 reciprocal tracking check를 합니다. 이는 각 후보 $K_{jk}$에 대하여 $\xi_{jki}$와 $\xi_{ijk}$를 살펴보아 다음 에러가 적은지 살펴보는 것입니다.

$$e(\xi_{jki}, \xi_{ijk}) = (\xi_{jki} \circ \xi_{ijk})^T(\Sigma_{jki} + \text{Adj}_{jki}\Sigma_{ijk}\text{Adj}_{jki}^T)^{-1} (\xi_{jki} \circ \xi_{ijk}$$

에러가 적으면 글로벌 맵에 추가합니다.

Convergence Radius for sim(3) Tracking

Direct image aligment의 한계는 최적화 과정의 non-convexity 문제입니다. 따라서 초기화가 정확히 이루어져야만 합니다.

한가지 방법은 초기화할 때에 적은 수의 키포인트들을 이용하는 것입니다. 기존의 역깊이맵으로부터 계산된 깊이 값을 이용때에 Horn[13]의 방법을 등을 이용할 때에는, correspondence문제를 풀어야 하는데 이때에는 최적화가 아닌 closed form으로 3D 위치를 계산하는 것입니다. 또한 다음의 방법을 이용하여 수렴의 radius를 늘릴 수 있습니다.

• Efficient Second Order Minimization (ESM)
• Coase-to-Fine Approach

3.6 Map optimization

키 프레임과 추적된 sim(3)-constraints로 이루어진 맵은 pose graph optimizatin을 이용하여 계속적으로 최적화됩니다. 다음의 에러 함수를 최소화하게 될 것입니다.

$$E(\xi_{W_1}, \cdots, \xi_{W_n}) = \sum_{(\xi_{ji}, \Sigma_{ji})}(\xi_{ji} \circ \xi_{w_1}^{-1} \circ \xi_{W_j})^T \Sigma_{ji}^{-1} (\xi_{ji} \circ \xi_{w_1}^{-1} \circ \xi_{W_j})^T$$

4 Results

---