SVO中的深度滤波代码流程

本文链接：https://blog.csdn.net/zhiwei121/article/details/103723675

本文详细解析了SVO（semi-direct visual odometry）中深度滤波的代码流程，通过关键代码展示了如何在视觉里程计算法中进行有效的深度信息处理，提升定位和跟踪的精度。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在这里插入图片描述

关键部分代码：

// 新的关键帧到来时，则中断updateSeeds()，因为这时有很大可能正在处理普通帧，而不是关键帧，所以就丢掉了，即，关键帧优先
// 仅针对能在该帧（frame）中能看到的seeds进行updateSeed,并将深度值收敛的seed加入到地图点中，同时清除该seed。每传入一个frame，才进行一次深度滤波更新。
void DepthFilter::updateSeeds(FramePtr frame)
{
   
   
  // update only a limited number of seeds, because we don't have time to do it
  // for all the seeds in every 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();     // 得到fx_
  double px_noise = 1.0;                                           // 像素点的偏差
  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)，一个像素点的偏差引起的角度变化

  while( it!=seeds_.end())
  {
   
   
    // set this value true when seeds updating should be interrupted
    if(seeds_updating_halt_)
      return;

    // check if seed is not already too old  检查种子是否已经太老，最多维护三个普通帧
    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
   
   
      it = seeds_.erase(it);
      continue;
    }

    // check if point is visible in the current image
	// 检查种子点是否在当前帧下
    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; // behind the camera
      continue;
    }
    if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>