svo理解二

最新推荐文章于 2022-02-18 13:45:26 发布

原创最新推荐文章于 2022-02-18 13:45:26 发布 · 632 阅读

2 ·

CC 4.0 BY-SA版权

slam 专栏收录该内容

17 篇文章

订阅专栏

Relaxation Through Feature Alignment

上面直接法优化位姿用的是已知的参考帧3D点位置，由于3D点位置可能存在误差，导致优化的位姿需要进一步改善；

将map上对于new image可见的3D点投影到new image上形成 $u_{i}^{'}$ ；（reproject，找出与当前帧共视的关键帧，然后按远近选出10帧，将关键帧上可见的point投影到当前帧形成 $u_{i}^{'}$ ；将point和 $u_{i}^{'}$ 放入cells中形成候选地图点，对候选地图点进行筛选）

对cells上每个网格点上的每个特征点进行配准（对于每个重投影点 $u_{i}^{'}$ ，对观测到该点的关键帧按观察角度大小进行排序，选出观测角度最小的关键帧 $r$ ;在关键帧 $r$ 上通过逆仿射矩阵得到参考patch,与 $u_{i}^{'}$ 一起放到aligen中优化）
特征块对齐步骤优化当前帧特征位置 $u_{i}^{'}$ ，通过最小化光度误差（当前帧对应特征块与关键帧 $r$ 特征块）

公式截图；

对齐优化通过逆组成ＬＫ算法（inverse compositional Lucas-Kanade）.？？？逆向：计算的雅可比矩阵是在参考帧patch上；ＬＫ:用到了灰度不变，像素梯度？？？前面sparseImageAligen用的是重投影

创新点：对参考帧特征图块引入仿射矩阵 $A_{i}$ ,因为patch 更大（8*8）,closest keyframe 可能离previous image较远

这一步没有考虑到极线约束，提高特征块的位置精度

reproject

void Reprojector::reprojectMap(
    FramePtr frame,
    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)

在关键帧列表中先找处共视的关键帧（取5个特征点投影到当前帧，若投影成功为共视）；

map_.getCloseKeyframes(frame, close_kfs);

对关键帧按照与当前帧的远近程度排序，找出距离最近的10帧（Q：距离怎么计算的？？？）

按照关键帧距离从近到远的顺序，依次把关键帧上的特征点对应的地图点往当前帧上面投影，同一个地图点只能被投影一次，不同帧的特征点可能对应着同一地图点；

将地图点的3D世界坐标点转为当前帧的像素点（px_cur这里生成），并判断能否取得的8*8的图块（isInFrame），若能取得，产生候选地图点？？？，并将对应的特征点对应的point, px放进当前帧投影位置的cell中（Ｑ：候选地图点怎么存储的？？？貌似产生如下）

grid_.cells.at(k)->push_back(Candidate(point, px));

对候选地图点进行一个筛选，遍历map_.point_candidates_.candidates_，将地图点投影到当前帧，若能取得8*8的图块，就把这个候选点存放到当前帧投影位置的网格中；如果一个候选点有10帧n_failed_reproj_投影不成功，则把该候选点删除

对于每一个网格点，把其中的地图点list，按照地图点的质量进行排序，TYPE_GOOD>TYPE_UNKNOWN>TYPE_CANDIDATE>TYPE_DELETED；检查地图点质量，如果是TYPE_DELETED，就删除它；

然后放到findMatchDirect中进行配准，findMatchDirect(*it->pt, *frame, it->px)；

若配准失败，则进行如下处理：

 if(!found_match)
    {
	//如果配准失败，n_failed_reproj_ ++
      it->pt->n_failed_reproj_++;
      //如果该地图点是一个TYPE_UNKNOWN类型的地图点，当匹配失败的次数>15次，将它变化DELETED类型的点；
      if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
        map_.safeDeletePoint(it->pt);      
      //如果该地图点是一个TYPE_CANDIDATED类型的地图点，当匹配失败的次数>30次，将它变化DELETED类型的点；
      if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
        map_.point_candidates_.deleteCandidatePoint(it->pt);
      //并在地图点列表中删除它
      it = cell.erase(it);
      continue;
    }

若配准成功，在当前帧上，生成新的特征点（优化后的坐标，层数），特征点指向那个地图点；

如果对应的参考帧上的特征点是边缘点的话，则新的特征点的类型也设置为边缘点，把梯度也仿射过来，归一化后，成为这个新特征点的梯度；

在每个网格中，只要有一个地图点匹配成，就跳出该网格的循环遍历；

如果匹配成功的网格数>180,则跳出所有网格的循环

Ｑ：当匹配成功，在当前帧上，生成新的特征点（优化后的坐标，层数），但是指向原来的point? ? ?不更新point吗？因为只优化像素点位置？？？

Ｑ：关于网格的存储，详见reproject.h

  typedef std::list<Candidate > Cell;
  typedef std::vector<Cell*> CandidateGrid;
  CandidateGrid cells;

  grid_.cells.at(k)->push_back(Candidate(point, px));

说明一个网格点是可以存放多个候选地图点，是个list

Ｑ：如何删除网格中的候选地图点？

检查地图点质量，如果是TYPE_DELETED，就删除它；前面筛选候选地图点时对不合格地图点的质量有修改

if(it->first->n_failed_reproj_ > 30)
        {
          map_.point_candidates_.deleteCandidate(*it);   //修改候选地图点的质量为TYPE_DELETED,便于后续遍历网格中地图点时删除        
          it = map_.point_candidates_.candidates_.erase(it);
          continue;
        }

Q:有几次对n_failed_reproj_进行使用，一处是用判断连续10帧重投影不成功，如上；一处是配准失败后的处理；

void Reprojector::reprojectMap(
    FramePtr frame,
    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
{
  resetGrid();

  // Identify those Keyframes which share a common field of view.
  SVO_START_TIMER("reproject_kfs");
  // 选出与目前帧有重叠视野的关键帧，用列表的方式存储
  list< pair<FramePtr,double> > close_kfs;
  map_.getCloseKeyframes(frame, close_kfs);

  // 对靠近的关键帧根据靠近程度进行排序   ？？？靠近程度怎么衡量
  // Sort KFs with overlap according to their closeness
  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                 boost::bind(&std::pair<FramePtr, double>::second, _2));

  // Reproject all mappoints of the closest N kfs with overlap. We only store
  // in which grid cell the points fall.
  
  size_t n = 0;
  overlap_kfs.reserve(options_.max_n_kfs);   //用于重投影的关键帧的最大数量，进行重投影的关键帧
  // 找出候选地图点
  for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
      it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
  {
    FramePtr ref_frame = it_frame->first;
    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));  

    // Try to reproject each mappoint that the other KF observes
    // 对这个参考帧观察到的点投影到当前帧中
    for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
        it_ftr!=ite_ftr; ++it_ftr)
    {
      // check if the feature has a mappoint assigned
	// 检测这个特征是否有分配的mappoint
      if((*it_ftr)->point == NULL)
        continue;

      // make sure we project a point only once
      // 确保我们只投影一次，不同帧上的特征会对应同一个3D点
      if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
        continue;
      (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
      if(reprojectPoint(frame, (*it_ftr)->point))      //将地图点的3D世界坐标点转为当前帧的像素点，并判断是否在当前帧中，产生候选地图点？？？，并将对应的point, px放进对应位置的cell中
        overlap_kfs.back().second++;   //？？？统计参考帧中成功投影到当前帧的特征点个数？？？
    }
  }
  SVO_STOP_TIMER("reproject_kfs");

  // Now project all point candidates
  //对候选地图点进行筛选
  SVO_START_TIMER("reproject_candidates");
  {
    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
    //遍历候选地图点集合
    auto it=map_.point_candidates_.candidates_.begin();
    while(it!=map_.point_candidates_.candidates_.end())
    {
     //如果该候选点有10帧投影不成功，则把该候选点删除
      if(!reprojectPoint(frame, it->first))
      {
        it->first->n_failed_reproj_ += 3;          
        if(it->first->n_failed_reproj_ > 30)
        {
          map_.point_candidates_.deleteCandidate(*it);   //修改候选地图点的质量为TYPE_DELETED,便于后续遍历网格中地图点时删除        
          it = map_.point_candidates_.candidates_.erase(it);
          continue;
        }
      }
      ++it; //判断下一候选点
    }
  } // unlock the mutex when out of scope
  SVO_STOP_TIMER("reproject_candidates");

  // Now we go through each grid cell and select one point to match.
  // At the end, we should have at maximum one reprojected point per cell.
  SVO_START_TIMER("feature_align");
  //对于每一个网格点，把其中的地图点list，按照地图点的质量进行排序，TYPE_GOOD>TYPE_UNKNOWN>TYPE_CANDIDATE>TYPE_DELETED
  for(size_t i=0; i<grid_.cells.size(); ++i)
  {
 
    // we prefer good quality points over unkown quality (more likely to match)
    // and unknown quality over candidates (position not optimized)
    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
      ++n_matches_;   //累计匹配成功的网格数
    //如果匹配成功的网格数>180,则跳出所有网格的循环
    if(n_matches_ > (size_t) Config::maxFts())    
      break;
  }
  SVO_STOP_TIMER("feature_align");
}

对每个网格点处理如下：

bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{
    //按照地图点质量进行排序
  cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
  Cell::iterator it=cell.begin();
  while(it!=cell.end())
  {
    ++n_trials_;

    //如果是TYPE_DELETED就在网格中删除掉
    if(it->pt->type_ == Point::TYPE_DELETED)
    {
      it = cell.erase(it);
      continue;
    }

    bool found_match = true;
    //进入feature alignment
    if(options_.find_match_direct)
	//特征点配准，优化px_cur
      found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
    if(!found_match)
    {
	//如果配准失败，n_failed_reproj_ ++
      it->pt->n_failed_reproj_++;
      //如果该地图点是一个TYPE_UNKNOWN类型的地图点，当匹配失败的次数>15次，将它变化DELETED类型的点；
      if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
        map_.safeDeletePoint(it->pt);      
      //如果该地图点是一个TYPE_CANDIDATED类型的地图点，当匹配失败的次数>30次，将它变化DELETED类型的点；
      if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
        map_.point_candidates_.deleteCandidatePoint(it->pt);
      //并在地图点列表中删除它
      it = cell.erase(it);
      continue;
    }
    
    //如果配准成功，累计该点成功匹配成功的次数>10,则将TYPE_UNKNOWN的地图点类型置为TYPE_GOOD
    it->pt->n_succeeded_reproj_++;
    if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
      it->pt->type_ = Point::TYPE_GOOD;

    //在当前图像上，新生成一个特征点（包括坐标和层数），
    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
    frame->addFeature(new_feature);

    // Here we add a reference in the feature to the 3D point, the other way
    // round is only done if this frame is selected as keyframe.
    //该特征点指向那个地图点
    new_feature->point = it->pt;

    //如果对应的参考帧上的特征点是边缘点的话，则新的特征点的类型也设置为边缘点，把梯度也仿射过来，归一化后，成为这个新特征点的梯度
    if(matcher_.ref_ftr_->type == Feature::EDGELET)
    {
      new_feature->type = Feature::EDGELET;
      new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
      new_feature->grad.normalize();
    }

    // If the keyframe is selected and we reproject the rest, we don't have to
    // check this point anymore.
    //删除后返回指向下一个元素的迭代器
    it = cell.erase(it);

    //在每个网格中，只要有一个地图点匹配成，就跳出该网格的循环遍历；
    // Maximum one point per cell.
    return true;
  }
  return false;
}

下面对特征块配准findMatchDirect进行研究：

//特征位置配准，前提是px_cur的初始值在final result的2~3像素内
bool Matcher::findMatchDirect(
    const Point& pt,
    const Frame& cur_frame,
    Vector2d& px_cur)
{
    //找到地图点被观察到的所有的关键帧，获取关键帧光心与该地图点连线，与，地图点与当前帧光心连线的夹角，
    //选出夹角最小的那个关键帧作为参考帧
  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
    return false;

  //对选中的特征点，在对应的层次上，判断图块是否投影在当前帧中
  if(!ref_ftr_->frame->cam_->isInFrame(
      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
    return false;

  //计算仿射矩阵，首先，获取参考帧的光心与地图点连线的模 (ref_ftr_->frame->pos() - pt.pos_).norm()
  //cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse()   -> Tk,k-1
  // warp affine
  warp::getWarpMatrixAffine(
      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
      (ref_ftr_->frame->pos() - pt.pos_).norm(),
      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
  
  //计算在当前帧的目标搜索层数？？？
  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
  
  //在参考帧特征对应层上取patch，该patch经过仿射变换之后是当前帧的正patch
  warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
                   ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
  //从映射过来的10*10图块取出8*8的图块，作为参考图块
  createPatchFromPatchWithBorder();

  // px_cur should be set
  //变换到目标搜索层数下的坐标！！！因为之前参考帧ftr对应的point对应的px_cur是通过px(frame->w2c(point->pos_))计算的
  Vector2d px_scaled(px_cur/(1<<search_level_));

  bool success = false;
  if(ref_ftr_->type == Feature::EDGELET)
  {
    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
    dir_cur.normalize();
    success = feature_alignment::align1D(
          cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
  }
  else
  {
    success = feature_alignment::align2D(
      cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
      options_.align_max_iter, px_scaled);
  }
  px_cur = px_scaled * (1<<search_level_);    //恢复第0层下的像素坐标
  return success;
}

1、对每个网格中的每个特征点，找到地图点被观察到的所有的关键帧obs_，获取关键帧光心与该地图点连线，与，地图点与当前帧光心连线的夹角，选出夹角最小的那个关键帧作为参考帧r，并获得对应的特征点！！！不同帧上的特征点不同的，但是保存在同一point的list<Feature*> obs_中; getCloseViewObs

Ｑ：getCloseViewObs中的obs_是指？？？怎么来的？遍历obs_？？？list<Feature*> obs_ (*it)->frame 指针指向特征点被检测到的图像帧

Ｑ：怎么计算地图点的xyz_ref？？？首先计算该帧的光心与地图点的连线的模作为深度，再用f_ref*defth_ref,因为f_ref是！！！因为用的是球面坐标，所以模长即代表该特征点的深度信息

defth_ref = (ref_ftr_->frame->pos() - pt.pos_).norm()
const Vector3d xyz_ref(f_ref*depth_ref);  //p点世界坐标

2、对选中的特征点，在对应的层次上，判断图块是否投影在当前帧中；每个特征有对应的ftr_->level

if(!ref_ftr_->frame->cam_->isInFrame(
      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))

3、接着计算仿射矩阵Ａ_cur_ref，由三点确定，对应特征点，在它对应层上，取右边第5个像素位置，和下边的第5个像素位置，恢复出三个点的世界坐标，再将三个点映射到当前帧，根据它们与中心投影点的位置变换，算出了仿射变换，如下：

//计算两帧之间的仿射变换，用窗口的3个点做重投影变换
void getWarpMatrixAffine(
    const vk::AbstractCamera& cam_ref,
    const vk::AbstractCamera& cam_cur,
    const Vector2d& px_ref,
    const Vector3d& f_ref,
    const double depth_ref,
    const SE3& T_cur_ref,
    const int level_ref,
    Matrix2d& A_cur_ref)
{
  // Compute affine warp matrix A_ref_cur
  const int halfpatch_size = 5;
  const Vector3d xyz_ref(f_ref*depth_ref);  //p点世界坐标 ！！！因为用的是球面坐标，所以模长即代表该特征点的深度信息
  //取右边的第5个像素位置，并映射到第0层？？？才能计算其三维坐标
  //映射到0层后，像素坐标转单位球面坐标
  Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));     //p1点世界坐标（单位球面坐标下）
  Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));     //p2点世界坐标（单位球面坐标下）
  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];   //p1 3D坐标
  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];    //p2 3D坐标
  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));         //p点重投影坐标
  const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));    //p1点重投影坐标
  const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));     //p2点重投影坐标
  //根据他们与中心投影点的位置变换，算出了仿射矩阵
  A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;     
  A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;     
}

4、接着计算在当前帧的目标搜索层数？？？Ｑ：为啥要有？怎么搜素？在原来的ftr_level不行吗？是px_cur所在的层数

search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);

这个search_level后续有什么用？目标搜索层数

Vector2d px_scaled(px_cur/(1<<search_level_));

5、接着计算参考图块？？？干嘛用?这样子，取投影中心（0,0），取10*10的图块，则图块上每个像素点（相对于中心点）位置，就可以通过逆仿射矩阵，得到对应的参考帧上的对应层数图像上的（相对中心点的）像素位置；进行像素插值；这样得到的patch，经Ａ_cur_ref_自然是当前帧中10*10的正图块；

从映射过来的10*10图块取出8*8的图块，作为参考图块；

Ｑ：这里用到了search_level，同时也用到了level_ref，最后两者直接相加? ? ?

const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref); 
px_patch *= (1<<search_level);   
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);

6、px_cur变换到目标搜索层数下的坐标px_scale！！！因为之前参考帧ftr对应的point对应的px_cur是通过px(frame->w2c(point->pos_))计算的

Vector2d px_scaled(px_cur/(1<<search_level_));

7、然后将px_scaled和参考patch放进feature_alignment::align2D中优化；

前面已经获得参考patch和cur_px_estimate

//特征优化，残差就是当前图像可能特征与投影特征的差值，优化变量包括像素值和均值差
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)
{
#ifdef __ARM_NEON__
  if(!no_simd)
    return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
#endif

  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;   //记录x方向的梯度值
  float* it_dy = ref_patch_dy;   //记录y方向的梯度值
  //计算参考patch上的雅可比
  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]);    //x方向梯度值
      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);  //y方向梯度值
      J[2] = 1;   
      *it_dx = J[0];
      *it_dy = J[1];
      H += J*J.transpose();
    }
  }
  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;   //如果某次調整位置的模小于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];   //当前帧patch上像素点的灰度
        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;
      }
    }


/*
    if(iter > 0 && new_chi2 > chi2)
    {
#if SUBPIX_VERBOSE
      cout << "error increased." << endl;
#endif
      u -= update[0];
      v -= update[1];
      break;
    }
    chi2 = new_chi2;
*/
    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;
}

同样利用反向的方法，计算的是参考图块上的雅可比，然后利用高斯牛顿法进行迭代；

计算误差的时候也把两图块的均值差加进去；

最多迭代10次，若某次调整位置的模小于0.03,则认为收敛，退出迭代;