깊이 필터 유도하기
깊이 필터(depth filter)는 SVO (Semi-Direct Visual Odometry) 1 2와 같은 단안 카메라 visual odometry 시스템에서 유용한 구성 요소다. 이는 단일 카메라로 촬영한 영상 시퀀스로부터 깊이 값을 추정하도록 설계되었다. 이 필터는 영상 측정에 내재된 불확실성을 나타내는 가우시안 측정 잡음과, 잘못된 측정이나 데이터의 이상치를 설명하는 균일(uniform) 이상치 잡음을 가정하고 동작한다. 이 글에서는 깊이 필터의 식을 유도한다. 먼저 그 기저에 깔린 가정들을 설명하는 것으로 시작한다. 여기에는 잡음 특성에 대한 확률 모델을 정식화하고 그것이 깊이 추정에 어떻게 영향을 미치는지가 포함된다. 또한 깊이 필터가 시간에 따라 새로운 측정을 통합하여 깊이 값을 정제하고 갱신함으로써 강건하고 정확한 visual odometry를 보장하는 방법을 논의한다.
Likelihood와 Posterior 확률 모델
깊이 필터는 깊이가 가우시안 분포와 균일 분포의 조합으로부터 측정된다고 가정한다. 깊이 참값을 추정하기 위해, 측정 \(x_n\)의 likelihood는 아래와 같이 정의된다.
\[p(x_n|Z,\pi) := \pi N(x_n|Z,\tau^2) + (1-\pi) U(x_n|Z_{\textrm{min}},Z_{\textrm{max}}), \tag{1}\label{eq:model}\]여기서 \(Z\)와 \(\tau\)는 깊이 참값의 가우시안 분포 파라미터, \(\pi\)는 inlier 비율, \(Z_{\textrm{min}}, Z_{\textrm{max}}\)는 균일 분포의 범위다. 그리고 posterior는 다음과 같다.
\[\begin{align} &p(Z,\pi|x_1,...x_n)\nonumber\\ &=\frac{p(x_1,...,x_n|Z,\pi)p(Z,\pi)}{p(x_1,...,x_n)} && \because \textrm{Bayes's rule} \nonumber\\ &=\frac{p(x_1,...,x_{n-1}|Z,\pi)p(x_n|Z,\pi)p(Z,\pi)}{p(x_1,...,x_n)} && \because \textrm{$x_i$ are independent}\nonumber\\ &=\frac{p(Z,\pi|x_1,...,x_{n-1})p(x_1,...,x_{n-1})p(x_n|Z,\pi)}{p(x_1,...,x_n)}.\nonumber \end{align}\]위 식들로부터, posterior는 이전 시간의 posterior에 비례한다.
\[p(Z,\pi|x_1,...x_n) \propto p(Z,\pi|x_1,...,x_{n-1})p(x_n|Z,\pi).\]\(x_1,...,x_n\)을, 아래를 만족하는 \(a_n,b_n,\mu_n,\sigma_n\)으로 재매개변수화하자.
\[\begin{align} p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) &\propto p(Z,\pi|a_{n-1},b_{n-1},\mu_{n-1},\sigma_{n-1})p(x_n|Z,\pi) \\ p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) &:= N(Z|\mu_n,\sigma_n^2)Beta(\pi|a_n,b_n),\\\tag{2}\label{eq:propagation} \end{align}\]여기서 \(\Pi \sim Beta(a,b)\)는 두 개의 양의 파라미터 \(a\), \(b\)를 갖는 베타 분포이며, 그 확률 밀도 함수는 다음과 같다.
\[Beta(\pi|a,b) = \frac{\pi^{a-1}(1-\pi)^{b-1}}{\int_0^1{u^{a-1}(1-u)^{b-1}du}} = \frac{B(\pi;a,b)}{B(a,b)}\]그리고 베타 함수 \(B(a,b)\)는 다음과 같이 정의된다.
\[B(a,b) := \int_0^1{u^{a-1}(1-u)^{b-1}du} = \frac{\Gamma(a)\Gamma(b)}{\Gamma(a+b)},\nonumber\]그리고 베타 함수의 incomplete 형태는
\[B(x;a,b) := x^{a-1}(1-x)^{b-1}\nonumber\]이다. 또한 감마 함수 \(\Gamma(z)\)는 다음과 같이 정의되며,
\[\Gamma(z) := \int_0^\infty{x^{z-1}e^{-x}dx},\nonumber\]양의 정수 \(n\)에 대해,
\[\Gamma(n) := (n-1)!\nonumber\]이다. 그러면 확률 변수 \(Z\)의 기댓값과 그 제곱의 기댓값은
\[\begin{align} E(Z)&=\int{Z p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= \int{Z N(Z|\mu_n,\sigma_n^2)Beta(\pi|a_n,b_n) dZd\pi} \nonumber\\ &= \int{Z N(Z|\mu_n,\sigma_n^2) dZ} \nonumber\\ &= \mu_n,\\\tag{3}\label{eq:meanZ} \end{align}\] \[\begin{align} E(Z^2)&=\int{Z^2 p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\&= \int{Z^2 N(Z|\mu_n,\sigma_n^2) dZ} \nonumber\\ &= \mu_n^2+\sigma_n^2,\\\tag{4}\label{eq:varZ} \end{align}\]이고, 확률 변수 \(\pi\)의 기댓값과 그 제곱의 기댓값은
\[\begin{align} E(\pi)&=\int{\pi p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= \int{\pi Beta(\pi|a_n,b_n) d\pi} \nonumber\\ &= \int{\pi \frac{B(\pi;a_n,b_n)}{B(a_n,b_n)}d\pi} \nonumber\\&= \frac{1}{B(a_n,b_n)}\int{B(\pi;a_n+1,b_n)d\pi} \nonumber\\ &= \frac{\Gamma(a_n+b_n)}{\Gamma(a_n)\Gamma(b_n)}\cdot\frac{\Gamma(a_n+1)\Gamma(b_n)}{\Gamma(a_n+b_n+1)} \nonumber\\ &= \frac{a_n}{a_n+b_n},\\\tag{5}\label{eq:meanpi} \end{align}\] \[\begin{align} E(\pi^2)&=\int{\pi^2 p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\&= \int{\pi^2 Beta(\pi|a_n,b_n) d\pi} \nonumber\\ &= \frac{\Gamma(a_n+b_n)}{\Gamma(a_n)\Gamma(b_n)}\cdot\frac{\Gamma(a_n+2)\Gamma(b_n)}{\Gamma(a_n+b_n+2)} \nonumber\\ &= \frac{a_n (a_n+1)}{(a_n+b_n)(a_n+b_n+1)}.\\\tag{6}\label{eq:varpi} \end{align}\]이다.
파라미터 업데이트
이제 식 \eqref{eq:model}과 \eqref{eq:propagation}을 이용해 모델 파라미터 \(a_{n-1}, b_{n-1}, \mu_{n-1}, \sigma_{n-1}\)을 업데이트한다.
\[\begin{align} p&(Z,\pi|a_n,b_n,\mu_n,\sigma_n) \propto \hspace{1mm} p(Z,\pi|a_{n-1},b_{n-1},\mu_{n-1},\sigma_{n-1})p(x_n|Z,\pi) \nonumber\\ =& N(Z|\mu_{n-1},\sigma_{n-1}^2)Beta(\pi|a_{n-1},b_{n-1}) (\pi N(x_n|Z,\tau^2) + (1-\pi) U(x_n|Z_{\textrm{min}},Z_{\textrm{max}})) \nonumber\\ =& N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{B(\pi;a_{n-1}+1,b_{n-1})}{B(a_{n-1},b_{n-1})} \cdot N(x_n|Z,\tau^2) + \nonumber\\ & N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{B(\pi;a_{n-1},b_{n-1}+1)}{B(a_{n-1},b_{n-1})} \cdot U(x_n|Z_{\textrm{min}},Z_{\textrm{max}}) \nonumber\\ =& N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{a_{n-1}}{a_{n-1}+b_{n-1}}\frac{B(\pi;a_{n-1}+1,b_{n-1})}{B(a_{n-1}+1,b_{n-1})} \cdot N(x_n|Z,\tau^2) + \nonumber\\ & N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{b_{n-1}}{a_{n-1}+b_{n-1}}\frac{B(\pi;a_{n-1},b_{n-1}+1)}{B(a_{n-1},b_{n-1}+1)} \cdot U(x_n|Z_{\textrm{min}},Z_{\textrm{max}})\nonumber\\ =& N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{a_{n-1}}{a_{n-1}+b_{n-1}}Beta(\pi|a_{n-1}+1,b_{n-1}) \cdot N(x_n|Z,\tau^2) + \nonumber\\ & N(Z|\mu_{n-1},\sigma_{n-1}^2)\frac{b_{n-1}}{a_{n-1}+b_{n-1}}Beta(\pi|a_{n-1},b_{n-1}+1) \cdot U(x_n|Z_{\textrm{min}},Z_{\textrm{max}}). \\\tag{7}\label{eq:update} \end{align}\]위 식으로부터, 베타 함수의 파라미터 \(a, b\)가 각각 가우시안 항과 균일 분포 항에 대해 1씩 증가함에 유의한다. 그러면 사후 확률 \(p(Z,\pi \rvert a_n,b_n,\mu_n,\sigma_n)\)의 평균과 분산을 구한다. 간단히 하기 위해, 아래를 만족하는 상수 \(\hat{C_1}\)과 \(\hat{C_2}\)를 정의한다.
\[\begin{align} \hat{C_1} N(Z|m,s^2) &:= N(Z|\mu_{n-1},\sigma_{n-1}^2) N(x_n|Z,\tau^2), \hspace{1em} \\ \hat{C_2} &:= U(x_n|Z_{\textrm{min}},Z_{\textrm{max}}) = \frac{1}{Z_{\textrm{max}}-Z_{\textrm{min}}},\nonumber \end{align}\]그러면 식 \eqref{eq:update}는 다음과 같이 된다.
\[\begin{align} p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) \propto & \hspace{1mm} \hat{C_1} \frac{a_{n-1}}{a_{n-1}+b_{n-1}} N(Z|m,s^2) Beta(\pi|a_{n-1}+1,b_{n-1}) + \nonumber\\ &\hat{C_2} \frac{b_{n-1}}{a_{n-1}+b_{n-1}} N(Z|\mu_{n-1},\sigma_{n-1}^2)Beta(\pi|a_{n-1},b_{n-1}+1).\\\tag{8}\label{eq:simpleupdate} \end{align}\]\(m, s\)는 다음으로부터 계산된다.
\[\begin{align} &N(Z|\mu_{n-1},\sigma_{n-1}^2) N(x_n|Z,\tau^2) \nonumber\\ &= \frac{1}{2\pi \tau\sigma_{n-1}}\exp\left(-\frac{(Z-\mu_{n-1})^2}{2\sigma_{n-1}^2}-\frac{(x_n-Z)^2}{2\tau^2}\right) \nonumber\\ &= \frac{1}{2\pi \tau\sigma_{n-1}}\exp\left(-\frac{\tau^2(Z-\mu_{n-1})^2+\sigma_{n-1}^2(x_n-Z)^2}{2\tau^2\sigma_{n-1}^2}\right) \nonumber\\ &= \frac{1}{2\pi \tau\sigma_{n-1}}\exp\left(-\frac{(\tau^2+\sigma_{n-1}^2)Z^2 - 2(\tau^2 \mu_{n-1}+\sigma_{n-1}^2x_n)Z+\tau^2 \mu_{n-1}^2 + \sigma_{n-1}^2 x_n^2}{2\tau^2\sigma_{n-1}^2}\right) \nonumber\\ &= \frac{1}{2\pi \tau\sigma_{n-1}}\exp\left(-\frac{\tau^2 \mu_{n-1}^2 + \sigma_{n-1}^2 x_n^2}{2\tau^2\sigma_{n-1}^2}\right)\exp\left(-\frac{(\tau^2+\sigma_{n-1}^2)Z^2 - 2(\tau^2 \mu_{n-1}+\sigma_{n-1}^2x_n)Z}{2\tau^2\sigma_{n-1}^2}\right) \nonumber \end{align}\]여기서 \(m = \frac{(\tau^2 \mu_{n-1}+\sigma_{n-1}^2x_n)}{(\tau^2+\sigma_{n-1}^2)}\), \(s^2 = \frac{\tau^2\sigma_{n-1}^2}{(\tau^2+\sigma_{n-1}^2)}\)로 두면,
\[\begin{align} &N(Z|\mu_{n-1},\sigma_{n-1}^2) N(x_n|Z,\tau^2) \nonumber\\ &= \frac{1}{2\pi \tau\sigma_{n-1}}\exp\left(-\frac{\tau^2 \mu_{n-1}^2 + \sigma_{n-1}^2 x_n^2}{2\tau^2\sigma_{n-1}^2}\right)\exp\left(-\frac{Z^2 - 2\frac{(\tau^2 \mu_{n-1}+\sigma_{n-1}^2x_n)}{(\tau^2+\sigma_{n-1}^2)}Z}{2\frac{\tau^2\sigma_{n-1}^2}{(\tau^2+\sigma_{n-1}^2)}}\right) \nonumber\\ &= \frac{1}{\sqrt{2\pi (\tau^2+\sigma_{n-1}^2)}}\exp\left(-\frac{\tau^2 \mu_{n-1}^2 + \sigma_{n-1}^2 x_n^2}{2\tau^2\sigma_{n-1}^2}+\frac{m^2}{2s^2}\right) \cdot \frac{1}{\sqrt{2 \pi s^2}}\exp\left(-\frac{(Z - m)^2}{2s^2}\right) \nonumber\\ &= \hat{C_1} \cdot N(Z|m,s^2). \nonumber \end{align}\]이다. 따라서,
\[\begin{align} \hat{C_1} &:= \frac{1}{\sqrt{2\pi (\tau^2+\sigma_{n-1}^2)}}\exp\left(-\frac{\mu_{n-1}^2}{2\sigma_{n-1}^2}-\frac{x_n^2}{2\tau^2}+\frac{m^2}{2s^2}\right) \nonumber\\&= \frac{1}{\sqrt{2\pi (\tau^2+\sigma_{n-1}^2)}}\exp\left(-\frac{(x_n-\mu_{n-1})^2}{2(\tau^2+\sigma_{n-1}^2)}\right) \nonumber \end{align}\]이다. 마지막으로, 미지의 스케일링 상수 \(C\)를 갖는 식 \eqref{eq:simpleupdate}는 아래를 만족하는 확률이다.
\[\begin{align} \int{p(Z,\pi|a_n,b_n,\mu_n,\sigma_n)dZd\pi} =& \frac{1}{C} \int{\hat{C_1} \frac{a_{n-1}}{a_{n-1}+b_{n-1}} N(Z|m,s^2) Beta(\pi|a_{n-1}+1,b_{n-1})} + \nonumber\\ &\hat{C_2} \frac{b_{n-1}}{a_{n-1}+b_{n-1}} N(Z|\mu_{n-1},\sigma_{n-1}^2)Beta(\pi|a_{n-1},b_{n-1}+1) dZd\pi \nonumber\\ =& \frac{1}{C} (\frac{a_{n-1}}{a_{n-1}+b_{n-1}}\hat{C_1}+\frac{b_{n-1}}{a_{n-1}+b_{n-1}}\hat{C_2}) = 1. \end{align}\]간단히 하기 위해,
\[\begin{align} C_1 := \frac{a_{n-1}}{a_{n-1}+b_{n-1}} \frac{\hat{C_1}}{C}, \hspace{1em} C_2 := \frac{b_{n-1}}{a_{n-1}+b_{n-1}} \frac{\hat{C_2}}{C} \nonumber \end{align}\]로 둔다. 이제 확률 변수 \(Z\)의 기댓값과 그 제곱의 기댓값은
\[\begin{align} E(Z)&=\int{Z p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= \int{Z \Big\{C_1 N(Z|m,s^2) Beta(\pi|a_{n-1}+1,b_{n-1}) + C_2 N(Z|\mu_{n-1},\sigma_{n-1}^2)Beta(\pi|a_{n-1},b_{n-1}+1)\Big\} dZd\pi} \nonumber\\ &= \int{Z \Big\{C_1 N(Z|m,s^2) + C_2 N(Z|\mu_{n-1},\sigma_{n-1}^2)\Big\} dZ} \nonumber\\ &= C_1 m + C_2 \mu_{n-1} \end{align}\\\tag{9}\label{eq:meanZnew}\] \[\begin{align} E(Z^2)&=\int{Z^2 p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= C_1 (m^2+s^2) + C_2 (\mu_{n-1}^2+\sigma_{n-1}^2) \end{align}\\\tag{10}\label{eq:varZnew}\]이고, 확률 변수 \(\pi\)의 기댓값과 그 제곱의 기댓값은
\[\begin{align} E(\pi)&=\int{\pi p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= \int{\pi \Big\{C_1 N(Z|m,s^2) Beta(\pi|a_{n-1}+1,b_{n-1}) + C_2 N(Z|\mu_{n-1},\sigma_{n-1}^2)Beta(\pi|a_{n-1},b_{n-1}+1)\Big\} dZd\pi} \nonumber\\ &= \int{\pi \Big\{C_1 Beta(\pi|a_{n-1}+1,b_{n-1}) + C_2 Beta(\pi|a_{n-1},b_{n-1}+1)\Big\} d\pi} \nonumber\\ &= \int{C_1 \frac{B(\pi;a_{n-1}+2,b_{n-1})}{B(a_{n-1}+1,b_{n-1})} + C_2 \frac{B(\pi;a_{n-1}+1,b_{n-1}+1)}{B(a_{n-1},b_{n-1}+1)} d\pi} \nonumber\\ &= C_1 \left(\frac{a_{n-1}+1}{a_{n-1}+b_{n-1}+1}\right) + C_2 \left(\frac{a_{n-1}}{a_{n-1}+b_{n-1}+1}\right) \end{align}\\\tag{11}\label{eq:meanpinew}\] \[\begin{align} E(\pi^2)&=\int{\pi^2 p(Z,\pi|a_n,b_n,\mu_n,\sigma_n) dZd\pi} \nonumber\\ &= \int{C_1 \frac{B(\pi;a_{n-1}+3,b_{n-1})}{B(a_{n-1}+1,b_{n-1})} + C_2 \frac{B(\pi;a_{n-1}+2,b_{n-1}+1)}{B(a_{n-1},b_{n-1}+1)} d\pi} \nonumber\\ &= C_1 \frac{(a_{n-1}+1)(a_{n-1}+2)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)} + C_2 \frac{(a_{n-1})(a_{n-1}+1)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)} \end{align}\\\tag{12}\label{eq:varpinew}\]이다. 식 \eqref{eq:meanZ}, \eqref{eq:varZ}, \eqref{eq:meanpi}, \eqref{eq:varpi}를 식 \eqref{eq:meanZnew}, \eqref{eq:varZnew}, \eqref{eq:meanpinew}, \eqref{eq:varpinew}와 비교하면, 다음을 얻는다.
\[\begin{align} \mu_n &= C_1 m + C_2 \mu_{n-1} \nonumber\\ \mu_n^2+\sigma_n^2 &= C_1 (m^2+s^2) + C_2 (\mu_{n-1}^2+\sigma_{n-1}^2) \nonumber\\ \frac{a_n}{a_n+b_n} &= C_1 \left(\frac{a_{n-1}+1}{a_{n-1}+b_{n-1}+1}\right) + C_2 \left(\frac{a_{n-1}}{a_{n-1}+b_{n-1}+1}\right) \nonumber\\ \frac{a_n (a_n+1)}{(a_n+b_n)(a_n+b_n+1)} &= C_1 \frac{(a_{n-1}+1)(a_{n-1}+2)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)} + C_2 \frac{(a_{n-1})(a_{n-1}+1)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)} \nonumber \end{align}\]따라서,
\[\begin{align} \mu_n &= C_1 m + C_2 \mu_{n-1} \nonumber\\ \sigma_n^2 &= C_1 (m^2+s^2) + C_2 (\mu_{n-1}^2+\sigma_{n-1}^2)-\mu_n^2 \nonumber\\ a_n &= \frac{[2]-[1]}{[1]-[2]/[1]} \nonumber\\ b_n &= a_n\frac{1-[1]}{[1]}, \nonumber \end{align}\]이며, 여기서
\[\begin{align} &[1]: C_1 \left(\frac{a_{n-1}+1}{a_{n-1}+b_{n-1}+1}\right) + C_2 \left(\frac{a_{n-1}}{a_{n-1}+b_{n-1}+1}\right) \nonumber\\ &[2]: C_1 \frac{(a_{n-1}+1)(a_{n-1}+2)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)} + C_2 \frac{(a_{n-1})(a_{n-1}+1)}{(a_{n-1}+b_{n-1}+1)(a_{n-1}+b_{n-1}+2)}. \nonumber \end{align}\]이다.