深度滤波器推导

深度滤波器推导

测量不确定性

图像深度滤波器推导深度滤波器推导对应的相机中心分别为深度滤波器推导。二者的转换关系为深度滤波器推导,并且平移距离为深度滤波器推导, 估计场景中的3d点为深度滤波器推导.假设匹配误差为一个像素,得到的误差距离为深度滤波器推导

因此有:
深度滤波器推导

角度
深度滤波器推导

深度滤波器推导

深度滤波器推导

深度滤波器推导

因此得到,
深度滤波器推导

最终得到测量不确定度为

深度滤波器推导

代码分析

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

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
}

深度滤波器

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

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

包含四个参数分别为高斯分布的深度滤波器推导与均匀分布的深度滤波器推导

深度滤波器推导

迭代更新过程

已知:
上一次的概率分布 深度滤波器推导
当前的测量值 深度滤波器推导

求解更新后的概率分布 深度滤波器推导

计算过程

深度滤波器推导

深度滤波器推导

深度滤波器推导

深度滤波器推导
这里的深度滤波器推导代表高斯分布深度滤波器推导深度滤波器推导位置的概率分布,同理深度滤波器推导为均匀分布在x位置处的概率。

深度滤波器推导

更新概率分布
深度滤波器推导

深度滤波器推导

更新均匀分布
深度滤波器推导

深度滤波器推导

深度滤波器推导

深度滤波器推导

代码分析

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

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. 证明分布后验分布的近似表达形式,即
    深度滤波器推导
  2. 推导迭代公式
    深度滤波器推导

后验分布的参数近似

参数更新公式

已知:

  1. 当前分布 深度滤波器推导
  2. 新测量值 深度滤波器推导

深度滤波器推导
深度滤波器推导
其中深度滤波器推导为gamma函数[1],存在如下性质
深度滤波器推导
因此得到性质如下
深度滤波器推导

深度滤波器推导

此外,推导过程中用到高斯分布乘积的性质如下[2]:
设有高斯分布深度滤波器推导深度滤波器推导,则
深度滤波器推导
其中,
深度滤波器推导

得到后验分布为:

深度滤波器推导

将常量替换成单一变量
深度滤波器推导

得到如下分布
深度滤波器推导

计算对变量深度滤波器推导求一阶矩和二阶矩

深度滤波器推导

深度滤波器推导
计算对变量深度滤波器推导求一阶矩和二阶矩

深度滤波器推导
深度滤波器推导

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

参考资料

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

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

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
上一篇 2022年5月22日 下午12:57
下一篇 2022年5月22日 下午1:01

相关推荐

本站注重文章个人版权,不会主动收集付费或者带有商业版权的文章,如果出现侵权情况只可能是作者后期更改了版权声明,如果出现这种情况请主动联系我们,我们看到会在第一时间删除!本站专注于人工智能高质量优质文章收集,方便各位学者快速找到学习资源,本站收集的文章都会附上文章出处,如果不愿意分享到本平台,我们会第一时间删除!