SVO笔记 - Sparse Image Align

SVO - Sparse Image Align

svo 的更新过程第一步：Sparse Image Align。
使用 GaussNewton 优化 cur 与 ref 图像帧间的位姿 T_cur_from_ref

FrameHandlerMono

  new_frame_->T_f_w_ = last_frame_->T_f_w_;
  
  SVO_START_TIMER("sparse_img_align");
  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                           30, SparseImgAlign::GaussNewton, false, false);
  size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);
  SVO_STOP_TIMER("sparse_img_align");

run()

1. T_cur_from_ref 初始化为单位阵，计算方法兼容后期 prior；
2. 在图像金字塔 第4层 到 第2层，使用 GaussNewton 优化 T_cur_from_ref；

默认参数中：
- “svo/klt_min_level”, 2；
- “svo/klt_max_level”, 4

size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
   
  reset();

  if(ref_frame->fts_.empty())
  {
   
    SVO_WARN_STREAM("SparseImgAlign: no features to track!");
    return 0;
  }

  ref_frame_ = ref_frame;
  cur_frame_ = cur_frame;
  ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
  jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
  visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?

  SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());

  for(level_=max_level_; level_>=min_level_; --level_)
  {
   
    mu_ = 0.1;
    jacobian_cache_.setZero();
    have_ref_patch_cache_ = false;
    if(verbose_)
      printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
    optimize(T_cur_from_ref);
  }
  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;

  return n_meas_/patch_area_;
}

optimizeGaussNewton()

1. computeResiduals(model, false, true); 计算 GaussNewton 中的阻尼因子；
2. double new_chi2 = computeResiduals(model, true, false); 计算残差（光度误差）；
3. applyPrior(model); 未实现，用于 imu 融合；
4. solve() 调用 eigen 库中的 ldlt 方法，求解方程；
   参考：https://blog.youkuaiyun.com/billbliss/article/details/78560605?locationNum=3&fps=1
5. update(model, new_model); 根据上一步计算得到的优化增量，更新位姿；

template <int D, typename T>
void vk::NLLSSolver<D, T>::optimizeGaussNewton(ModelType& model)
{
   
  // Compute weight scale
  if(use_weights_)
    computeResiduals(model, false, true); // 根据 false/true 参数，选择计算阻尼项/残差；

  // Save the old model to rollback in case of unsuccessful update
  ModelType old_model(model);

  // perform iterative estimation
  for (iter_ = 0; iter_<n_iter_; ++iter_)
  {
   
    rho_ = 0;
    startIteration(); // do nothing.

    H_.setZero();
    Jres_.setZero();

    // compute initial error
    n_meas_ = 0;
    double new_chi2 = computeResiduals(model, true, false);

    // add prior
    if(have_prior_)
      applyPrior(model);

    // solve the linear system
    // H*x = Jres 求解残差 x_；
    if(!solve())
    {
   
      // matrix was singular and could not be computed
      std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
      std::cout << "H = " << H_ << std::endl;
      std::cout << "Jres = " << Jres_ << std::endl;
      stop_ = true;
    }

    // check if error increased since last optimization
    if((iter_ > 0 && new_chi2 > chi2_) || stop_)
    {
   
      if(verbose_)
      {
   
        std::cout << "It. " << iter_
                  << "\t Failure"
                  << "\t new_chi2 = " << new_chi2
                  << "\t Error increased. Stop optimizing."
                  << std::endl;
      }
      model = old_model; // rollback
      break;
    }

    // update the model
    ModelType new_model;
    // 使用残差 x_ 更新pose；
    update(model, new_model);
    old_model = model;
    model = new_model;

    chi2_ = new_chi2;

    if(verbose_)
    {
   
      std::cout << "It. " << iter_
                << "\t Success"
                << "\t new_chi2 = " << new_chi2
                << "\t n_meas = " << n_meas_
                << "\t x_norm = " << vk::norm_max(x_)
                << std::endl;
    }

    // do nothing but printf;
    finishIteration();

    // stop when converged, i.e. update step too small
    if(vk::norm_max(x_)<=eps_)
      break;
  }
}

precomputeReferencePatches()

该函数主要对 特征 对应 patch 计算图像对李代数的 jacobian 矩阵；

• jacobian_xyz2uv() 计算像素坐标uv 对 李代数 的 jacobian 矩阵；
• dx、dy 是图像对像素坐标uv的导数，采用中心差分求导；
  • 对于金字塔中图像，坐标值可能出现小数，故采用 双线性插值 计算其灰度值；
  • 先用双线性插值求出特征附近四个坐标点，再使用中心差分求导（代码中放在一起计算）；

公式：

• $uv$ - ref 像素坐标；

$$u=\frac{f_{x}X+c_x}{Z},v=\frac{f_{y}Y+c_y}{Z}$$

• $q$ - 空间点在 ref 帧图像坐标系；

$$\mathbf{q}=\mathbf{R}\mathbf{P}$$

• 像素坐标对相机坐标系的三维空间点求导：

$$\frac{\partial \mathbf{u}}{\partial \mathbf{q}}=[\begin{array}{c}\underset{\partial X}{}\end{array}$$