svo的Supplementary matterial 推导过程

在看本博客之前请先看svo的Supplementary matterial .以下的过程借鉴了肖师兄的depth filter.pdf和高翔的推导，将给出公式(17)-(26)的推导推导过程，如果发现有错误之处，敬请指正.(qq:1347893477)

===(πN(x|Z,τ2))+(1−π)U(x))N(Z|μ,σ2)Beta(π|a,b)πN(x|Z,τ2)N(Z|μ,σ2)Beta(π|a,b)+(1−π)U(x)N(Z|μ,σ2)Beta(π|a,b)aa+bN(Z|μ,σ2)N(Z|μ,σ2)Beta(π|a+1,b)+ba+bU(x)N(Z|μ,σ2)Beta(π|a,b+1)aa+bN(x|μ,σ2+τ2)N(Z|m,s2)Beta(π|a+1,b)+ba+bU(x)N(Z|μ,σ2)Beta(π|a,b+1)(17)(18)

对比可知： N(x|Z,τ2)N(x|Z,σ2)=N(x|μ,σ2+τ2)N(Z|m,s2) \
取出 N(x|Z,τ2)N(x|Z,σ2) 展开为：

===========N(x|Z,τ2)N(Z|μ,σ2)12π−−√τe−(x−Z)22τ2⋅12π−−√σe−(Z−μ)22σ212πτσe−(x−Z)22τ2−(Z−μ)22σ212πτσe−σ2(x−Z)2+τ2(Z−μ)22τ2σ212πτσe−(σ2+τ2)Z2−2(xσ2+μτ2)Z+x2σ2+μ2τ22τ2σ212π(σ2+τ2)−−−−−−−−−√12πσ2τ2(σ2+τ2)−−−−−−−−−−√e−(x−μ)22(σ2+τ2)⋅e−(σ2+τ2)Z2−2(xσ2+μτ2)Z+x2σ2+μ2τ22τ2σ2+(x−μ)22(σ2+τ2)12π(σ2+τ2)−−−−−−−−−√e−(x−μ)22(σ2+τ2)⋅12πσ2τ2(σ2+τ2)−−−−−−−−−−√e−((σ2+τ2)Z2−2(xσ2+μτ2)Z+x2σ2+μ2τ2)(σ2+τ2)+(x−μ)2τ2σ22τ2σ2(σ2+τ2)N(x|μ,σ2+τ2)⋅12πσ2τ2(σ2+τ2)−−−−−−−−−−√e−(σ2+τ2)2Z2−2(xσ2+μτ2)(σ2+τ2)Z+(xσ2+μτ2)22τ2σ2(σ2+τ2)N(x|μ,σ2+τ2)⋅12πσ2τ2(σ2+τ2)−−−−−−−−−−√e−[(σ2+τ2)Z−(xσ2+μτ2)]22τ2σ2(σ2+τ2)N(x|μ,σ2+τ2)⋅12πσ2τ2(σ2+τ2)−−−−−−−−−−√e−[Z−(xσ2+μτ2)τ2+σ2]22(τ2σ2τ2+σ2)N(x|μ,σ2+τ2)N(Z|(xσ2+μτ2)τ2+σ2,τ2σ2τ2+σ2))N(x|μ,σ2+τ2)N(Z|m,s2)

所以 s2=τ2σ2τ2+σ2,m=(xσ2+μτ2)τ2+σ2 ,进一步简化：

1s2m=1τ2+1σ2=(xσ2+μτ2)τ2+σ2=s2(xτ2+μσ2)(19)(20)

现在假设:

C1C2=aa+bN(x|μ,σ2+τ2)=ba+bU(x)(21)(22)

公式(16)和(18)的一阶矩和二阶矩相等，先求公式(16)的一、二阶矩：\
Z的一、二阶矩：

∫Zp(Z,π|a′,b′,μ′,σ′)dZdπ=∫ZN(Z|μ′,σ′2)Beta(π|a′,b′)dZdπ=∫ZN(Z|μ′,σ′2)dZ=μ′

∫Z2p(Z,π|a′,b′,μ′,σ′)dZdπ=∫Z2N(Z|μ′,σ′2)Beta(π|a′,b′)dZdπ=∫Z2N(Z|μ′,σ′2)dZ=μ′2+σ′2

π 的一、二阶矩：

∫πp(Z,π|a′,b′,μ′,σ′)dZdπ=∫πN(Z|μ′,σ′2)Beta(π|a′,b′)dZdπ=∫πBeta(π|a′,b′)dπ=a′a′+b′

∫π2p(Z,π|a′,b′,μ′,σ′)dZdπ=∫π2N(Z|μ′,σ′2)Beta(π|a′,b′)dZdπ=∫π2Beta(π|a′,b′)dπ=a′(a′+1)(a′+b′)(a′+b′+1)

对公式(18)的概率密度进行积分：

C=∫aa+bN(x|μ,σ2+τ2)N(Z|m,s2)Beta(π|a+1,b)+ba+bU(x)N(Z|μ,σ2)Beta(π|a,b+1)dZdπ=∫C1N(Z|m,s2)Beta(π|a+1,b)+C2N(Z|μ,σ2)Beta(π|a,b+1)dZdπ=C1+C2

公式(18)的一、二阶矩：
Z的一、二阶矩：

==1C∫Z{C1N(Z|m,s2)Beta(π|a+1,b)+C2N(Z|μ,σ2)Beta(π|a,b+1)}dZdπ1C∫Z{C1N(Z|m,s2)+C2N(Z|μ,σ2)}dZdπ1C{C1m+C2μ}

==1C∫Z2{C1N(Z|m,s2)Beta(π|a+1,b)+C2N(Z|μ,σ2)Beta(π|a,b+1)}dZdπ1C∫Z2{C1N(Z|m,s2)+C2N(Z|μ,σ2)}dZdπ1C{C1(m2+s2)+C2(μ2+σ2)}

π 的一二阶矩：

==1C∫π{C1N(Z|m,s2)Beta(π|a+1,b)+C2N(Z|μ,σ2)Beta(π|a,b+1)}dZdπ1C∫π{C1Beta(π|a+1,b)+C2Beta(π|a,b+1)}dZdπ1C{C1a+1a+b+1+C2aa+b+1}

==1C∫π2{C1N(Z|m,s2)Beta(π|a+1,b)+C2N(Z|μ,σ2)Beta(π|a,b+1)}dZdπ1C∫π2{C1Beta(π|a+1,b)+C2Beta(π|a,b+1)}dZdπ1C{C1(a+1)(a+2)(a+b+1)(a+b+2)+C2a(a+1)(a+b+1)(a+b+2)}

对 C1和C2 进行归一化：

C′1C′2=C1C=C2C

(16)(18)的阶矩相等，所以：

μ′μ′2+σ′2a′na′n+b′na′n(a′n+1)(a′n+b′n)(a′n+b′n+1)=C′1m+C′2μ=C′1(m2+s2)+C′2(μ2+σ2)=C′1a+1a+b+1+C′2aa+b+1=C′1(a+1)(a+2)(a+b+1)(a+b+2)+C′2a(a+1)(a+b+1)(a+b+2)(23)(24)(25)(26)

附上源代码的实现，不过其中的 μ 是逆深度， τ 是逆深度的方差：

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);
  // 公式(19)
  float s2 = 1./(1./seed->sigma2 + 1./tau2);
  // 公式(20)计算m.
  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
  // 公式(21)计算C1.
  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
  // 公式(22)计算C2。
  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
  // 概率密度函数归一化
  float normalization_constant = C1 + C2;
  C1 /= normalization_constant;
  C2 /= normalization_constant;
  // 公式(25)
  float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
  // 公式(26)
  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.
  // 公式(23)
  float mu_new = C1*m+C2*seed->mu;
  // 公式(24)变形
  seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
  seed->mu = mu_new;
  // 公式(25)(26)联立求a'
  seed->a = (e-f)/(f-e/f);
  // 公式(25)求b'
  seed->b = seed->a*(1.0f-f)/f;
}