SVO笔记 - Feature alignment

luoshi006

已于 2022-12-27 09:47:14 修改

阅读量1.6k

点赞数 2

分类专栏： CV 文章标签： computer vision

于 2019-01-27 23:06:31 首次发布

本文链接：https://blog.csdn.net/luoshi006/article/details/80699720

版权

CV 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

SVO - Feature alignment

主要计算集中在 Reprojector 中。

  // map reprojection & feature alignment
  SVO_START_TIMER("reproject");
  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
  SVO_STOP_TIMER("reproject");

Reprojector::reprojectMap()
    map_.getCloseKeyframes(frame, close_kfs);
        [kf_loop]
            frame->isVisible();
    [kf_loop]
        [fts_loop]
            reprojectPoint(frame, (*it_ftr)->point);
                frame->cam_->isInFrame(px.cast<int>(), 8)
    ---
    [cell_loop]
        reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame);

Map Reprojection

将 map 中的 3D 点投影到图片中，并找到对应的 feature。
每个 grid 一个 3D 点。既保证均匀分布，又减小计算量。

// 通过 isVisible() 判断 map 点是否在视野内；找到与当前帧视场重叠的关键帧；
  map_.getCloseKeyframes(frame, close_kfs);
  //close_kfs < 关键帧，与 cur 距离>

// sort(); 按照 当前帧 与 关键帧 的**位移**，对找到的 关键帧 排序；
for(; options_.max_n_kfs ;){
    //将 close_kfs 中前 max_n_kfs 个关键帧取出；
    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
        ...
    //重投影到像素坐标系，判断其是否 isInFrame();
    reprojectPoint(frame, (*it_ftr)->point)
}

// reproject candidates 略

Feature Align

reprojectPoint() 中，为 cell 和 pt 建立指针链接；

  SVO_START_TIMER("feature_align");
  for(size_t i=0; i<grid_.cells.size(); ++i)
  {
    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
      ++n_matches_;
    if(n_matches_ > (size_t) Config::maxFts())
      break;
  }
  SVO_STOP_TIMER("feature_align");

`reprojectCell();`

    found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
        //找到视角偏差最小的 kf, 返回对应的 &ftr; [closest observation angle]
        pt.getCloseViewObs(cur_frame.pos(), ref_ftr_);
        //判断 patch 是否在视野内；排除边缘点；
        ref_ftr_->frame->cam_->isInFrame(像素坐标, halfpatch_size_+2, ref_ftr_->level)
        // 计算放射矩阵
        warp::getWarpMatrixAffine(...);
        //找到 cur 与 ref 缩放比例一致的 level;
        warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
        //得到 ref 中平行四边形状的 patch 图像；
        warp::warpAffine(...);
            //双线性插值函数
            vk::interpolateMat_8u(img_ref, px[0], px[1]);
        feature_alignment::align2D();
        
    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
    frame->addFeature(new_feature);

    new_feature->point = it->pt;

`warp::getWarpMatrixAffine();`

将 ref 中特征点对应的 5x5 half_patch 经 T 转换到 cur，通过 half_patch 上三点，计算 Affine 矩阵。

参考：https://blog.csdn.net/xbcreal/article/details/52549629

`getBestSearchLevel()`

找到 cur 与 ref patch 缩放比例一致的 level。

$\begin{bmatrix}cos\theta & -sin\theta\\ sin\theta & cos\theta \end{bmatrix} \begin{bmatrix}s & 0 \\ 0 & s \end{bmatrix}$

$det(A) = s^2$

int getBestSearchLevel(
    const Matrix2d& A_cur_ref,
    const int max_level)
{
  // Compute patch level in other image
  int search_level = 0;
  double D = A_cur_ref.determinant();
  while(D > 3.0 && search_level < max_level)
  {
    search_level += 1;
    D *= 0.25;
  }
  return search_level;
}

`warp::warpAffine();`

线性插值，仿射变换；
patch 在 cur 中为正方形，在 ref 中为平行四边形；
patch_ptr 中输出 ref patch；
uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));

void warpAffine(
    const Matrix2d& A_cur_ref,
    const cv::Mat& img_ref,
    const Vector2d& px_ref,
    const int level_ref,
    const int search_level,
    const int halfpatch_size,
    uint8_t* patch)
{
  const int patch_size = halfpatch_size*2 ;
  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
  if(isnan(A_ref_cur(0,0)))
  {
    printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
    return;
  }

  // Perform the warp on a larger patch.
  uint8_t* patch_ptr = patch;
  const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
  for (int y=0; y<patch_size; ++y)
  {
    for (int x=0; x<patch_size; ++x, ++patch_ptr)
    {
      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
      px_patch *= (1<<search_level); // cur img; 
      //此处假设 search_level 与 level_ref 的缩放比例一致，金字塔 2 倍缩放是否影响？
      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr); // ref img;
      if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
        *patch_ptr = 0;
      else
        *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
    }
  }
}

`feature_alignment::align2D();`

计算代价函数对像素坐标的雅可比矩阵及海森矩阵；
高斯牛顿迭代特征点位置；
采用 LK 光流法（Inverse Compositional Image Alignment）
- 参考：http://www.cs.cmu.edu/~16385/ [Lecture 25]
优化变量中加入光照模型 photometric model
- svo 1 代码中仅优化了 mean_diff，
- svo 2 中添加了 alpha，但是对 Hessian 矩阵的近似误差较大，不建议独立使用，代码中有些奇怪的负号，看代码有点费劲。
- 参考：
  - Direct Image Registration With Gain and Bias, Adrien Bartoli
  - Active Exposure Control for Robust Visual Odometry in HDR Environments, Zichao Zhang

公式：
$\begin{align} & \min_{\Delta p, \alpha, \beta} \sum_x [\alpha T(W(x,0)) + \beta - I(W(x,p+\Delta p))]^2 \\ {}_{Taylor} \approx & \min_{\Delta p, \Delta \alpha, \Delta \beta} \sum_x [(\alpha + \Delta \alpha) (T(W(x,0)) + \nabla T \frac{\partial W}{\partial \mathbf p} \Delta \mathbf p)+ \beta + \Delta \beta - I(W(x,p))]^2 \\ = & \min_{\Delta p, \Delta \alpha, \Delta \beta} \sum_x [ \alpha \nabla T \frac{\partial W}{\partial \mathbf p} \Delta \mathbf p + T(W(x,0)) \Delta \alpha + \Delta \beta + \alpha T(W(x,0))+ \beta - I(W(x,p))]^2 \end{align}$

$\Delta\mathbf{p} = H^{-1} \sum_{\mathbf{x}} \begin{bmatrix} \nabla T \frac{\partial W}{\partial \mathbf p } \\ 1 \\ T(W(x,0)) \end{bmatrix}^T [I(\mathbf{W})(\mathbf{x};\mathbf{p})- \alpha \mathbf{T}(\mathbf{x}) - \beta]$

bool align2D(
    const cv::Mat& cur_img,
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate,
    bool no_simd)
{
  const int halfpatch_size_ = 4;
  const int patch_size_ = 8;
  const int patch_area_ = 64;
  bool converged=false;

  // compute derivative of template and prepare inverse compositional
  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
  Matrix3f H; H.setZero();

  // compute gradient and hessian
  const int ref_step = patch_size_+2;
  float* it_dx = ref_patch_dx;
  float* it_dy = ref_patch_dy;
  for(int y=0; y<patch_size_; ++y)
  {
    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
    for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
    {
      Vector3f J;
      J[0] = 0.5 * (it[1] - it[-1]);
      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
      J[2] = 1;
      *it_dx = J[0];
      *it_dy = J[1];
      H += J*J.transpose();
      // 矩阵相乘，列向量 x 行向量；
    }
  }
  Matrix3f Hinv = H.inverse();
  float mean_diff = 0;

  // Compute pixel location in new image:
  float u = cur_px_estimate.x();
  float v = cur_px_estimate.y();

  // termination condition
  const float min_update_squared = 0.03*0.03;
  const int cur_step = cur_img.step.p[0];
//  float chi2 = 0;
  Vector3f update; update.setZero();
  for(int iter = 0; iter<n_iter; ++iter)
  {
    int u_r = floor(u);
    int v_r = floor(v);
    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
      break;

    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
      return false;

    // compute interpolation weights
    float subpix_x = u-u_r;
    float subpix_y = v-v_r;
    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
    float wTR = subpix_x * (1.0-subpix_y);
    float wBL = (1.0-subpix_x)*subpix_y;
    float wBR = subpix_x * subpix_y;

    // loop through search_patch, interpolate
    uint8_t* it_ref = ref_patch;
    float* it_ref_dx = ref_patch_dx;
    float* it_ref_dy = ref_patch_dy;
//    float new_chi2 = 0.0;
    Vector3f Jres; Jres.setZero();
    for(int y=0; y<patch_size_; ++y)
    {
      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
      for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
      {
        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
        // T - cur
        float res = search_pixel - *it_ref + mean_diff;
        Jres[0] -= res*(*it_ref_dx);
        Jres[1] -= res*(*it_ref_dy);
        Jres[2] -= res;
//        new_chi2 += res*res;
      }
    }

    update = Hinv * Jres;
    u += update[0];
    v += update[1];
    mean_diff += update[2];

#if SUBPIX_VERBOSE
    cout << "Iter " << iter << ":"
         << "\t u=" << u << ", v=" << v
         << "\t update = " << update[0] << ", " << update[1]
//         << "\t new chi2 = " << new_chi2 << endl;
#endif

    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
    {
#if SUBPIX_VERBOSE
      cout << "converged." << endl;
#endif
      converged=true;
      break;
    }
  }

  cur_px_estimate << u, v;
  return converged;
}