SVO 学习笔记（深度滤波）

这篇博客

 这篇博客将介绍SVO论文中的Mapping部分，主要介绍深度滤波器在获取新的图像帧后，更新相应地图点深度的过程。（2020-6-18：修改“更新Seed对象”中的内容）

论文中的深度滤波

 单目SLAM构建地图点是通过三角测量操作实现的。在SVO中，认为通过三角测量产生的地图点的深度值服从某种概率分布。在地图点刚构建时，这个概率分布的不确定性很大，即地图点深度值的不确定性较大（因为单目相机较难获得图中像素的真实深度）。这一点体现为下图中的浅绿色部分，即深度d在[dmin,dmax]中波动：

图中左边的方框是地图点的第一个观测帧（即产生该地图点的关键帧），右边是地图点的其他观测帧。将这两类帧分别记为 Ir 、Ik。
 从图可以看出，降低地图点深度不确定性的方法是：针对一个由 Ir 产生的新地图点，在它的其他观测帧 Ik 上找到匹配的特征点（使用极线搜索的方法，极线为[dmin,dmax]在 Ik 上的投影线段）。以此构建 Ir 和 Ik 上的一对匹配特征，并使用这对匹配特征进行三角测量，获得该地图点新的深度测量值。将这新的测量值与地图点已有的深度信息融合，更新该深度值的分布情况，使[dmin,dmax]的范围变小。
 当地图点的观测帧越多，它的深度值的波动范围就会越小。当其小到一定程度时，就可将该地图点放进地图中使用了。
 上述就是SVO中深度滤波的大致概念，下面介绍它的代码流程。

深度滤波的代码流程

 在SVO中，深度滤波被定义为一个单独的类对象。它将每个新构建的地图点设定为一个Seed对象（种子对象）。Seed对象包括了待优化的地图点、地图点深度的平均值和方差等信息。
 深度滤波器通过观测帧来优化种子对象。当种子对象中深度值的方差足够小时，将该种子对应的地图点添加到地图中使用，否则继续进行优化。
 在优化过程中，普通帧和关键帧对深度滤波的作用不同：
 1、如果是普通帧，就用它来优化深度值未收敛的地图点；
 2、如果是关键帧，还需要提取帧上新的特征点，并为这些特征点产生新的种子对象。
 代码中使用 addFrame 和 addKeyFrame 两个函数处理不同的帧。在处理普通帧时会在addFrame函数中使用 updateSeeds 函数来更新种子对象：

更新Seed对象

//使用普通帧更新所有能够更新的Seed对象
//只更新一定数量的Seeds，不能在此消耗过多时间
void DepthFilter::updateSeeds(FramePtr frame)
{
  size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
  lock_t lock(seeds_mut_);
  std::list<Seed>::iterator it=seeds_.begin();
  const double focal_length = frame->cam_->errorMultiplier2();
  //像素点的测量偏差
  double px_noise = 1.0;
  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0;
  while( it!=seeds_.end())
  {
    //判断种子更新是否需要终止
    if(seeds_updating_halt_)
      return;
    //不去更新那些很老的Seed（产生这些Seed的关键帧ID比较老）
    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
      it = seeds_.erase(it);
      continue;
    }
    //检查待优化的地图点能否在当前帧上观测到
    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
    const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
    if(xyz_f.z() < 0.0)  {
      ++it; // 投影点在相机成像面的后面
      continue;
    }
    if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
      ++it; // 投影点不再相机观测范围内
      continue;
    }
    // we are using inverse depth coordinates使用逆深度进行参数化
    float z_inv_min = it->mu + sqrt(it->sigma2);
    float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
    double z;
    if(!matcher_.findEpipolarMatchDirect(//因为是逆深度，所以用分式
        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
    //z是找到匹配后，三角化计算出来该地图点在参考帧中的深度
    {
      it->b++; // increase outlier probability when no match was found
      ++it;
      ++n_failed_matches;
      continue;
    }
    // compute tau计算由一个像素测量偏差所带来的不确定度
    double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
    //计算测量值的标准差
    double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
    // update the estimate更新种子的概率分布。更新的是逆深度的概率分布
    updateSeed(1./z, tau_inverse*tau_inverse, &*it);
    ++n_updates;
    if(frame->isKeyframe())
    {
       //如果当前帧是关键帧，特征点检测时，不能在这个位置上（网格内）提取特征点
      //因为这个位置已经找到了与地图点的匹配点，即已存在了一个特征
      feature_detector_->setGridOccpuancy(matcher_.px_cur_);
    }
    //如果种子收敛了，就初始化一个候选地图点，并删除掉这个种子
    if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
    {
      assert(it->ftr->point == NULL); // TODO this should not happen anymore
      //注意是逆深度！！
      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
      //创建新地图点
      Point* point = new Point(xyz_world, it->ftr);
      //将参考帧中对应特征点指向这个新的地图点
      it->ftr->point = point;
      {
        seed_converged_cb_(point, it->sigma2); // put in candidate list
      }
      it = seeds_.erase(it);
    }
     //种子的深度值的下限不确定，是个坏种子
    else if(isnan(z_inv_min))
    {
      SVO_WARN_STREAM("z_min is NaN");
      it = seeds_.erase(it);
    }
    else
      ++it;
  }
}

 在使用普通帧进行种子更新的过程中，有两个重要函数：1、updateSeed函数完成多个测量值的融合；2、computeTau函数计算观测帧中特征点的测量误差（代码中认为有一个像素坐标的测量误差）对地图点深度不确定性的影响。这两个函数是相关计算公式的实现，前者对应的公式和推导过程可以参考这篇博客；后者的公式推导可以参考《视觉SLAM十四讲》P326中的内容。（2020-6-18更新：）这里主要介绍updateSeed函数，因为computeTau和参考资料中的代码相似。

// 完成逆深度分布的融合。参数：新估计的逆深度 x 、x对应的方差 tau2 、x对应的 seed
void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{
  float norm_scale = sqrt(seed->sigma2 + tau2);
  if(std::isnan(norm_scale))
    return;
  // 构建以norm_scale为方差的正态分布
  boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
  // 1/(s^2) = 1/(sigma2)+ 1/(tau2)
  float s2 = 1./(1./seed->sigma2 + 1./tau2);
  // m = (s^2) * (mu/sigma2 + x/tau2)
  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
  // C1 = a/(a+b) * N(x|mu,sigma2 + tau2) ，pdf()概率密度：正态分布nd在x处的概率密度
  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
  // C2 = b/(a+b) * U(x)
  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
  // C1、C2的归一化
  float normalization_constant = C1 + C2;
  C1 /= normalization_constant;
  C2 /= normalization_constant;
  // 计算 f、e 来进行a、b的更新
  // f = C1*（a+1)/(a+b+1) + C2*(a)/(a+b+1)
  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 参数更新
  // mu = C1*m+C2*mu;
  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;
  // a 和 b 的更新
  seed->a = (e-f)/(f-e/f);
  seed->b = seed->a*(1.0f-f)/f;
}

初始化Seed对象

 在处理关键帧时，会在addKeyFrame函数中调用initializeSeed函数来初始化新的Seed对象：

//当有关键帧产生时，初始化新的Seed
void DepthFilter::initializeSeeds(FramePtr frame)
{
  //从帧中提取特征
  Features new_features;
  feature_detector_->setExistingFeatures(frame->fts_);
  feature_detector_->detect(frame.get(), frame->img_pyr_,
                            Config::triangMinCornerScore(), new_features);
  //为每个新的特征初始化一个Seed对象
  seeds_updating_halt_ = true;
  lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
  ++Seed::batch_counter;
  //初始化新的Seed对象时，使用的初始深度是当前帧中观测到的地图点的平均深度
  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
  });
  if(options_.verbose)
  //显示产生了多少个新的Seeds
    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
  seeds_updating_halt_ = false;
}

结尾

 感谢耐心的博友读到最后，希望这篇博客能帮助到各位。如果这篇博客的内容存在不足，希望大家指出，感谢大家的纠正。