深度滤波器推导

深度滤波器推导

测量不确定性

图像$I_r$与$I_k$对应的相机中心分别为$C_r,C_k$。二者的转换关系为$T_{k,r}$,并且平移距离为$t$, 估计场景中的3d点为$rP$.假设匹配误差为一个像素，得到的误差距离为${\tau }_k^2=(\Vert rP\Vert -\Vert r{P}^{+}\Vert {)}^2$。

因此有：
$$a=rP-t$$

角度
$$\alpha =\arccos (\frac{f\cdot t}{\Vert t\Vert })$$

$$\beta =\arccos (-\frac{a\cdot t}{\Vert a\Vert \cdot \Vert t\Vert })$$

$${\beta }^{+}=\beta +2{\tan }^{-1}(\frac{1}{2f})$$

$$\gamma =\pi -\alpha -{\beta }^{+}$$

因此得到，
$$\Vert r{P}^{+}\Vert =\Vert t\Vert \frac{\sin {\beta }^{+}}{\sin \gamma }$$

最终得到测量不确定度为

$${\tau }_k^2=(\Vert rP\Vert -\Vert r{P}^{+}\Vert {)}^2$$

代码分析

基本就是一条条公式对应过来

double DepthFilter::computeTau(
      const SE3& T_ref_cur,
      const Vector3d& f,
      const double z,
      const double px_error_angle)
{
  Vector3d t(T_ref_cur.translation());
  Vector3d a = f*z-t;
  double t_norm = t.norm();
  double a_norm = a.norm();
  double alpha = acos(f.dot(t)/t_norm); // dot product
  double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product
  double beta_plus = beta + px_error_angle;
  double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI
  double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines
  return (z_plus - z); // tau
}

深度滤波器

采用高斯分布与均匀分布叠加的概率密度函数表达深度信息的分布。

• 好处：提高算法对于错误匹配的鲁棒性
• 坏处：公式推导过程复杂，实现计算量稍大

包含四个参数分别为高斯分布的$\mu ,{\sigma }^2$与均匀分布的$a,b$。

$$p(x|Z,\pi )=\pi N(x|\mu ,{\sigma }^2)+(1-\pi )U(x|a,b)$$

迭代更新过程

已知：
上一次的概率分布 $a_k,b_k,{\mu }_k,{\sigma }_k$
当前的测量值 $x,\tau$

求解更新后的概率分布 $a_{k+1},b_{k+1},{\mu }_{k+1},{\sigma }_{k+1}$

计算过程

$$\frac{1}{s^2}=\frac{1}{{\sigma }_k^2}+\frac{1}{{\tau }^2}$$

$$m=s^2(\frac{{\mu }_k}{{\sigma }_2^k}+\frac{x}{{\tau }^2})$$

$$C_1=\frac{a_k}{a_k+b_k}\cdot N(x|{\mu }_t,{\tau }^2+{\sigma }_t^2)$$

$$C_2=\frac{b_k}{a_k+b_k}\cdot U(x)$$
这里的$N(x|{\mu }_t,{\tau }^2+{\sigma }_t^2)$代表高斯分布$N({\mu }_t,{\tau }^2+{\sigma }_t^2)$在$x$位置的概率分布，同理$U(x)$为均匀分布在x位置处的概率。

$$C_1^{{}^{\prime }}=\frac{C_1}{C_1+C_2},C_2^{{}^{\prime }}=\frac{C_2}{C_1+C_2}$$

更新概率分布
$${\mu }_{k+1}=C_1^{{}^{\prime }}m+C_2^{{}^{\prime }}{\mu }_k$$

$${\sigma }_{k+1}^2=C_1^{{}^{\prime }}(s^2+m^2)+C_2^{{}^{\prime }}({\tau }^2+{\sigma }_t^2)$$

更新均匀分布
$$f=C_1^{{}^{\prime }}\frac{a_k+1}{a_k+b_k+1}+C_2^{{}^{\prime }}\frac{a_k}{a_k+b_k+1}$$

$$e=C_1^{{}^{\prime }}\frac{(a_k+1)(a_k+2)}{(a_k+b_k+1)(a_k+b_k+2)}+C_2^{{}^{\prime }}\frac{a_k(a_k+1)}{(a_k+b_k+1)(a_k+b_k+2)}$$

$$a_{k+1}=\frac{e-f}{f-\frac{e}{f}}$$

$$b_{k+1}=\frac{1-f}{f}{a}_{k+1}$$

代码分析

基本能和上面的公式对应起来

void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{
  float norm_scale = sqrt(seed->sigma2 + tau2);
  if(std::isnan(norm_scale))
    return;
  boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
  float s2 = 1./(1./seed->sigma2 + 1./tau2);
  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
  float normalization_constant = C1 + C2;
  C1 /= normalization_constant;
  C2 /= normalization_constant;
  float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
  float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
          + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));

  // update parameters
  float mu_new = C1*m+C2*seed->mu;
  seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
  seed->mu = mu_new;
  seed->a = (e-f)/(f-e/f);
  seed->b = seed->a*(1.0f-f)/f;
}

理论推导

深度滤波器的公式实现较为简单，但是具体的理论推导较为繁琐，因此放在最后没有特别高的需求的可以直接用上面的结论和代码，而不需要过分深究推导过程。

推导过程分为两步

1. 证明分布后验分布的近似表达形式，即
   $$p(x|Z,\pi )=\pi N(x|\mu ,{\sigma }^2)+(1-\pi )U(x|a,b)\approx N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b)$$
2. 推导迭代公式
   $$(\pi N(x|Z,{\tau }^2)+(1-\pi )U(x))N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b)\to N(Z|{\mu }^{{}^{\prime }},{\sigma }^{{}^{\prime }2})Beta(\pi |a^{{}^{\prime }},b^{{}^{\prime }})$$

后验分布的参数近似

参数更新公式

已知：

1. 当前分布 $N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b)$
2. 新测量值 $x,{\tau }^2$

$$Beta(\pi |a,b)=\frac{\Gamma (a+b)}{\Gamma (a)\Gamma (b)}{\pi }^{a-1}(1-\pi {)}^{b-1}$$
$$E(Beta(X))=\frac{a}{a+b}E(Beta(X^2))=\frac{a(a+1)}{(a+b+1)(a+b)}$$
其中$\Gamma (n)$为gamma函数[1]，存在如下性质
$$\Gamma (n+1)=n\Gamma (n)$$
因此得到性质如下
$$\begin{gathered} Beta(\pi |a,b+1)=\frac{\Gamma (a+b+1)}{\Gamma (a)\Gamma (b+1)}{\pi }^{a-1}(1-\pi {)}^b \\ =\frac{a+b}{a}\frac{\Gamma (a+b)}{\Gamma (a)\Gamma (b)}{\pi }^{a-1}(1-\pi {)}^{b-1}\pi \\ =\frac{a+b}{a}\pi Beta(\pi |a,b) \end{gathered}$$

$$\begin{gathered} Beta(\pi |a+1,b)=\frac{\Gamma (a+b+1)}{\Gamma (a+1)\Gamma (b)}{\pi }^a(1-\pi {)}^{b-1} \\ =\frac{a+b}{b}\frac{\Gamma (a+b)}{\Gamma (a)\Gamma (b)}{\pi }^{a-1}(1-\pi {)}^{b-1}(1-\pi ) \\ =\frac{a+b}{b}(1-\pi )Beta(\pi |a,b) \end{gathered}$$

此外，推导过程中用到高斯分布乘积的性质如下[2]：
设有高斯分布$N(x|Z,{\tau }^2)$及$N(Z|\mu ,{\sigma }^2)$,则
$$N(x|Z,{\tau }^2)N(Z|\mu ,{\sigma }^2)=N(x|\mu ,{\tau }^2+{\sigma }^2)N(Z|m,s^2)$$
其中，
$$\begin{gathered} \frac{1}{s^2}=\frac{1}{{\sigma }^2}+\frac{1}{{\tau }^2} \\ m=s^2(\frac{\mu }{{\sigma }^2}+\frac{x}{{\tau }^2}) \end{gathered}$$

得到后验分布为：

$$\begin{gathered} (\pi N(x|Z,{\tau }^2)+(1-\pi )U(x))N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b) \\ \pi N(x|Z,{\tau }^2)N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b)+(1-\pi )U(x)N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b) \\ \frac{a}{a+b}N(x|\mu ,{\sigma }^2+{\tau }^2)N(Z|m,s^2)Beta(\pi |a+1,b)+\frac{b}{a+b}U(x)N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b+1) \end{gathered}$$

将常量替换成单一变量
$$\begin{gathered} C_1=\frac{a}{a+b}N(x|\mu ,{\sigma }^2+{\tau }^2) \\ {C}_2=\frac{b}{a+b}U(x) \end{gathered}$$

得到如下分布
$$C_{1}N(Z|m,s^2)Beta(\pi |a+1,b)+C_{2}N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b+1)$$

计算对变量$Z$求一阶矩和二阶矩

$${\mu }^{{}^{\prime }}=E(Z)=C_{1}m+C_2\mu$$

$${\sigma }^{{}^{\prime }2}+{\mu }^{{}^{\prime }2}=C_1(m^2+s^2)+C_2({\mu }^2+{\sigma }^2)$$
计算对变量$\pi$求一阶矩和二阶矩

$$\frac{a^{{}^{\prime }}}{a^{{}^{\prime }}+b^{{}^{\prime }}}=C_1\frac{a+1}{a+b+1}+C_2\frac{a}{a+b+1}$$
$$\frac{a^{{}^{\prime }}(a^{{}^{\prime }}+1)}{(a^{{}^{\prime }}+b^{{}^{\prime }}+1)(a^{{}^{\prime }}+b^{{}^{\prime }})}=C_1\frac{(a+1)(a+2)}{(a+b+2)(a+b+1)}+C_2\frac{a(a+1)}{(a+b+2)(a+b+1)}$$

最后化简即得到更新公式。

参考资料

[1] Gamma 函数 https://zh.m.wikipedia.org/wiki/%CE%93%E5%87%BD%E6%95%B0

[2] 高斯分布的乘积推导 https://www.zhihu.com/question/46458824