svo理解

最新推荐文章于 2022-10-17 16:51:54 发布

a1059682127

最新推荐文章于 2022-10-17 16:51:54 发布

阅读量835

点赞数 3

分类专栏： slam

本文链接：https://blog.csdn.net/a1059682127/article/details/90401711

版权

slam 专栏收录该内容

17 篇文章 0 订阅

订阅专栏

Semi-direct Monocular Visual Odometry

Q：

这篇论文解决了什么问题？用了什么方法？

一个是位姿估计部分，以及深度估计问题！位姿估计用了直接法估计两帧之间的初始位姿 $T_{k,k-1}$ ，再用特征块配准优化像素点位置 $u^{'}$ ,接着用重投影误差进一步优化位姿 $T_{k,w}$ ！建图部分用到了均匀高斯分布的深度滤波器

流程图

跟直接法的联系？因为sparse model-based image alignment用特征块进行直接匹配来获取相机位姿；不同于直接法的是，在derect method估计位姿后，用特征块的配准来对direct method 估计的位姿进行优化；其中特征提取只有当一个关键帧被选来初始化新的3D点才使用（地图线程里），而不用每一帧都进行特征提取；

Ｑ：ＬＫ用在哪里？

LK的思想在初始化的时候用到，用光流跟踪获得匹配点；在feature aligenment 也用到逆向组成ＬＫ思想优化特像素位置，spareImageAlign用到~~重投影的~~灰度误差来优化位姿，pose and struture refinement 重投影像素误差来优化位姿

Ｑ：spareImageAlign的特征点哪里来的？

有两处说到了特征点的提取，一处是初始化时用ＬＫ跟踪第一帧跟第二帧的匹配点；其它的特征点提取，放在地图线程里；因为在跟踪时，不需要特征点的匹配，而且直接根据图块亮度差匹配的；在svo中，后端的特征点只在关键帧上提取的，用FAST加金字塔；

Q:sparseImageAlign跟pose and structure refinement有什么区别？？？都是重投影误差吗？？？

sparseImageAlign是计算同一3Ｄ点对应的像素的光度误差，从参考帧中back-projecting一个2Ｄ点，然后投影到当前帧，优化的是两帧之间的相对位姿Ｔk,k-1

这里需要理解一下重投影误差的定义：是将像素坐标（观测到的投影位姿）与3Ｄ点按照当前估计的位姿进行投影得到的位置进行比较得到的误差（来自视觉slam十四讲）

pose and structure refinement用到的是重投影误差reprojection residual，论文里用的单帧，直接优化Ｔk,w

1.跟踪

1.1.初始化

目的：获得前面两个关键帧的位姿和初始地图；

通过假设局部平面性场景,用光流跟踪获得对应点，并估计单应性矩阵，分解获得初始位姿；

初始地图从前面两个视图三角话得到；

主要实现在initialization.cpp

（1）图像刚进来时候，就建立金字塔图像，5层，比例为2，在帧初始化中进行；

void Frame::initFrame(const cv::Mat& img)
{
    //检查图像是否存在，是否符合规格
  // check image
  if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
    throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");

  //对序列key_pts_中每个元素执行该函数,关键点初始化为null
  // Set keypoints to NULL
  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });   //??????????

  //建立图像金字塔，层数选择特征金字塔层数和LK跟踪器层数较大者，输出到img_pyr
  // Build Image Pyramid
  frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
}

Q：为啥要建立金字塔图像？

为了使算法适用于large motions，使光流跟踪能跟踪上；在sparse image alignment中，intensity residual 在最粗层优化直至收敛，接着进入更细的下一层，为了节省时间，在第三层上收敛后优化停止，该层次的优化已经足够精确来初始化feature alignment;

（2）处理第一帧

第一帧位姿初始化为单应性矩阵

//处理第一帧
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
  //第一帧位姿初始化为单位阵
  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
  //添加第一帧
  if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
    return RESULT_NO_KEYFRAME;
  //将当前帧设置为关键帧，并检测5个对应的关键点
  new_frame_->setKeyframe();
  //添加关键帧到地图中
  map_.addKeyframe(new_frame_);
  stage_ = STAGE_SECOND_FRAME;
  SVO_INFO_STREAM("Init: Selected first frame.");
  return RESULT_IS_KEYFRAME;
}

为了更好的确定初始位置，我们对第一帧的选择有一定的限制条件，采用FAST特征点和边缘特征，我们要确保第一帧的图像中检测到的特征数大于100个

InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
  reset();
  detectFeatures(frame_ref, px_ref_, f_ref_);
  if(px_ref_.size() < 100)
  {
    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
    return FAILURE;
  }
  // 将这一帧图像做为参考帧
  frame_ref_ = frame_ref;
  // 先设置当前帧的特征与参考帧的特征一致
  px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());  //insert在指定位置前插入元素
  return SUCCESS;
}

（3）处理第二帧

processSecondFrame()

//处理第二帧
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
    //添加第二帧
  initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
  if(res == initialization::FAILURE)
    return RESULT_FAILURE;
  else if(res == initialization::NO_KEYFRAME)
    return RESULT_NO_KEYFRAME;

  // two-frame bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
#endif

  //新帧设置为关键帧
  new_frame_->setKeyframe();
  double depth_mean, depth_min;
  //得到图像中特征点的平均深度
  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
  //将该帧作为关键帧送入深度滤波器中
  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);

  // add frame to map
  map_.addKeyframe(new_frame_);
  stage_ = STAGE_DEFAULT_FRAME;
  klt_homography_init_.reset();
  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
  return RESULT_IS_KEYFRAME;
}

其中ba::twoViewBA，以及将该帧作为关键帧送入深度滤波器部分未仔细研究

addSecondFrame具体如下：

光流跟踪后，若特征数>50,视差中位数>阈值，则computeHomography进行单应性分解得到位姿T_cur_from_ref_，并三角化得到xyz_in_cur_(当前帧的相机坐标？？？)，若内点inliers_数>40,则认为这一帧适合用来做三角化；由于尺度不确定问题，需要进行尺度初始化，把当前帧的深度均值调为单位1，对恢复出来的位姿和地图点进行尺度变换；

/*
 主要通过跟踪确定下一帧特征的估计位置以及估计特征的单位向量，把特征像素点转换成在相机坐标系下的深度归一化
 的点，并进行畸变矫正，再让模变成1,映射到单位球面上；
而对于跟踪的特征数目要大于50，对于金字塔Lucas-Kanade光流方法的参数调节，后续根据论文及实验进行测试。
在视觉里程计总述里面介绍到关键帧，给出图的形式表示两帧的距离很近会影响3D点的准确度。
在这里对上述disparities的中值做一个阈值限制，如果其中值小于50，则认为选择的帧不是关键帧，不能进行3D点估计。
 */


InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{
  trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");

  // 符合光流跟踪的特征数
  if(disparities_.size() < Config::initMinTracked())
    return FAILURE;

  // 对两帧光流跟踪之后视差的中位数
  double disparity = vk::getMedian(disparities_);
  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
  //  如果中值小于给定配置参数，则表明这一帧不是关键帧，也就是刚开始的时候两帧不能太近
  if(disparity < Config::initMinDisparity())
    return NO_KEYFRAME;

  //  计算单应矩阵
  computeHomography(
      f_ref_, f_cur_,
      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
      inliers_, xyz_in_cur_, T_cur_from_ref_);
  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");

  /*
 * 到这里单应矩阵的估计、分解就结束了，结束之后要对计算得到的内点数添加阈值控制，要确保内点数大于40.
下一步就是尺度估计，离相机的距离不同，则图像的尺度不一样，采用的方法是计算上面计算的所有3D点所有深度的中值scene_depth_median，则此时尺度为scale = 1.0 / scene_depth_median;

    注意：1、对于单目视觉里程计是不知道尺度信息的，也就是在第一帧的时候，我们假设了特征对应的3D点的深度为1。
    2、也就是目前是通过计算下一帧所有特征对应的3D点深度的中值作为尺度计算值，这也就限制了算法的使用场景，定位相机应尽可能的对着一个平面，所以朝地是一个很好的选择

 */
  
  if(inliers_.size() < Config::initMinInliers())
  {
    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
    return FAILURE;
  }

  // Rescale the map such that the mean scene depth is equal to the specified scale
  vector<double> depth_vec;
  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
    depth_vec.push_back((xyz_in_cur_[i]).z());
  double scene_depth_median = vk::getMedian(depth_vec);   //求出下一帧所有特征点的3D深度的中值
  double scale = Config::mapScale()/scene_depth_median;    //计算尺度
  
  //根据单应性矩阵恢复出来的位姿和地图点，进行尺度变换
  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;   //Tk,w = Tk,k-1*Tk-1,w;
  // 对位移变换添加尺度，原因是我们实际是假设深度为1的情况下进行计算，而实际深度不是1，要转变到实际深度情况下的值。另外pos=−R^(-1)*t
  frame_cur->T_f_w_.translation() =
      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

      //对每个内点创建3D点，设置特征，添加到这两帧中
  // For each inlier create 3D point and add feature in both frames
  SE3 T_world_cur = frame_cur->T_f_w_.inverse();
  for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
  {
    Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
    Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
    //检查特征点是否在图像帧中，深度是否大于0
    if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
    {
	//加入尺度的世界坐标
      Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
      //每个特征点创建Point对象
      Point* new_point = new Point(pos);

      Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
      frame_cur->addFeature(ftr_cur);
      // 将同一个点对应的特征保存起来，这样点删除了，对应的特征都可以删除
      new_point->addFrameRef(ftr_cur);

      Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
      frame_ref_->addFeature(ftr_ref);
      new_point->addFrameRef(ftr_ref);
    }
  }
  return SUCCESS;
}

Q:怎么进行尺度变换

矩阵分解出的t具有尺度等价性，也就是t乘以任意非零常数，分解都成立，因此常对t长度进行归一化，直接导致了单目视觉的尺度不确定性；

三角定位是在已知R，t基础上，求解两个特征点的深度s1,s2;此时的s1,s2也具有不确定性；

根据H恢复出来的位姿和地图点，进行尺度变换，把深度的中值调为1(1.0单位/scene_depth_median），即把前两帧的平移视为单位1;

vector<double> depth_vec;
  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
    depth_vec.push_back((xyz_in_cur_[i]).z());
  double scene_depth_median = vk::getMedian(depth_vec);   //求出下一帧所有特征点的3D深度的中值
  double scale = Config::mapScale()/scene_depth_median;    //计算尺度
  
  //根据单应性矩阵恢复出来的位姿和地图点，进行尺度变换
  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;   //Tk,w = Tk,k-1*Tk-1,w;
  // 对位移变换添加尺度，原因是我们实际是假设深度为1的情况下进行计算，而实际深度不是1，要转变到实际深度情况下的值。另外pos=−R^(-1)*t
  frame_cur->T_f_w_.translation() =
      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));

这一段有待理解

Q：pos应该是相机的光心在世界坐标系的坐标，为什么是如下表达？为什么要取逆？

inline Vector3d pos() const { return T_f_w_.inverse().translation(); }

因为T_f_w_.translation()是世界坐标原点在相机坐标下的坐标，而T_f_w_.inverse()即为 $\begin{bmatrix} R^{-1} & -R^{-1}t\\ 0 & 1 \end{bmatrix}$

（3）FrameHandlerMono::processFrame()正常的跟踪过程

源码待理解

FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{
  // Set initial pose TODO use prior
   //初始化为上一帧的位姿  ！ ！ ！绝对位姿
  new_frame_->T_f_w_ = last_frame_->T_f_w_;

  // sparse image align
  SVO_START_TIMER("sparse_img_align");
  //创建SparseImgAlign对象
  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");
  SVO_LOG(img_align_n_tracked);
  SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);

  //地图点reprojection选出共视帧，然后跟据靠近程度选择关键帧，再选出候选地图点，筛选，进行特征块配准
  // map reprojection & feature alignment
  SVO_START_TIMER("reproject");
  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
  SVO_STOP_TIMER("reproject");
  const size_t repr_n_new_references = reprojector_.n_matches_;
  const size_t repr_n_mps = reprojector_.n_trials_;
  SVO_LOG2(repr_n_mps, repr_n_new_references);
  SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
  if(repr_n_new_references < Config::qualityMinFts())
  {
    SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
    tracking_quality_ = TRACKING_INSUFFICIENT;
    return RESULT_FAILURE;
  }

  //位姿优化
  // pose optimization
  SVO_START_TIMER("pose_optimizer");
  size_t sfba_n_edges_final;
  double sfba_thresh, sfba_error_init, sfba_error_final;
  //对当前帧进行最小重投影误差，优化当前帧的相机位姿Ｔk,w
  pose_optimizer::optimizeGaussNewton(
      Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
      new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
  SVO_STOP_TIMER("pose_optimizer");
  SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
  SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
  SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
  //被跟踪特征点的数量num_obs
  if(sfba_n_edges_final < 20)
    return RESULT_FAILURE;

  //进一步优化地图点
  // structure optimization
  SVO_START_TIMER("point_optimizer");
  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
  SVO_STOP_TIMER("point_optimizer");

  //将当前帧插入core_kfts（用于存储附近关键帧）
  // select keyframe
  core_kfs_.insert(new_frame_);
  //根据被跟踪的特征点数量确定跟踪质量，若小于一个阈值，或者比上一帧用于优化的点少了很多
  setTrackingQuality(sfba_n_edges_final);
  //如果跟踪点少的话当前帧位姿同同于上一帧位姿，避免crazy pose jumps
  if(tracking_quality_ == TRACKING_INSUFFICIENT)
  {
    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
    return RESULT_FAILURE;
  }
  //取得景深最小和平均深度（相机坐标系下）
  double depth_mean, depth_min;
  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
  //关键帧选择标准，符合则将当前帧添加到深度滤波器中？？？
  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
  {
    depth_filter_->addFrame(new_frame_);
    return RESULT_NO_KEYFRAME;
  }
 
  new_frame_->setKeyframe();
  SVO_DEBUG_STREAM("New keyframe selected.");

  
  // new keyframe selected
  for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
    if((*it)->point != NULL)
      (*it)->point->addFrameRef(*it);
    //将map_.point_candidates_中与当前帧相关的特征点添加到当前帧
  map_.point_candidates_.addCandidatePointToFrame(new_frame_);

  // optional bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
  if(Config::lobaNumIter() > 0)
  {
    SVO_START_TIMER("local_ba");
    setCoreKfs(Config::coreNKfs());
    size_t loba_n_erredges_init, loba_n_erredges_fin;
    double loba_err_init, loba_err_fin;
    ba::localBA(new_frame_.get(), &core_kfs_, &map_,
                loba_n_erredges_init, loba_n_erredges_fin,
                loba_err_init, loba_err_fin);
    SVO_STOP_TIMER("local_ba");
    SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin);
    SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
                     "Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
  }
#endif

  // init new depth-filters
  //将当前关键帧添加到深度滤波器
  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);

  //如果关键帧数量达到限制，移除最远一帧
  // if limited number of keyframes, remove the one furthest apart
  if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
  {
    FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
    depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better
    map_.safeDeleteFrame(furthest_frame);
  }

  // add keyframe to map
  //添加当前关键帧到map_
  map_.addKeyframe(new_frame_);

  return RESULT_IS_KEYFRAME;
}

1.2.位姿估计部分：

sparse model-based image alignment:

直接法最小化重投影灰度残差，来优化两帧之间的位姿；

优化变量是位姿，参考帧的像素点（通过之前的特征检测与深度估计）重投影到当前帧，最小化重投影前后的image pathches（4*4）的光度差;

使用Gauss-Newton法进行优化，给一个位姿的估计值（初始化为上一帧的位姿或者假设为单位矩阵），使用inverse compositional formulation，将位姿扰动加到参考帧中，求解雅可比矩阵，代入增量方程求解更新量，更新位姿

Q：为啥雅可比矩阵可以预先计算且在迭代过程保持不变

雅可比计算=参考帧中像素梯度*投影关于相机坐标的导数，而参考帧中图块 $I_{K-1}(u_{i})$ 和对应的像素点的相机坐标（世界坐标） $p^{_{i}}$ 保持不变

Q：为啥说是近似的？

因为雅可比矩阵中计算投影关于相机坐标的导数采用了近似： $\frac{\partial u}{\partial b}=\frac{\partial u}{\partial (T(\xi )P)}\approx \frac{\partial u}{\partial P}$

过程：

初始化当前帧的位姿初始化为上一帧的位姿（！！！是T_f_w_），然后计算从最高层开始往最底层优化，每一次优化计算特征块在当前层的雅可比矩阵，然后重投影求残差，求出扰动，更新位姿；几层下来，就得到当前帧的位姿；

  //初始化为上一帧的位姿
  new_frame_->T_f_w_ = last_frame_->T_f_w_;

  // sparse image align
  SVO_START_TIMER("sparse_img_align");
  //创建SparseImgAlign对象
  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_);

SparseImgAlign::run

size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
  reset();  //继承自nlls_solver类

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

  ref_frame_ = ref_frame;
  cur_frame_ = cur_frame;
  //行为特征个数，列为块面积4*4
  ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
  //大小：行为6,列为ref_patch_cache的大小
  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?

  //从参考帧到当前帧的相对位姿Tk,w=Tk,k-1*Tk-1,w    Tk,k-1 = Tk,w * Tk-1,w^(-1)
  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;   //阻尼参数，继承自nlls_solver类
    //每次迭代，雅克比都置0；
    jacobian_cache_.setZero();
    have_ref_patch_cache_ = false;
    if(verbose_)
      printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
    optimize(T_cur_from_ref);   //优化Tk,k-1 要优化的是从参考帧到当前帧的位姿
  }
  //得到当前位姿
  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;  //得到优化后的位姿



  //返回的是平均特征数量；
  return n_meas_/patch_area_;
}

Q:optimize(T_cur_from_ref) ???找不到实现

因为SparseImgAlign类是继承自vk::NLLSSolver类，只要定义自己的computeResiduals、solve、update

在computeResiduals前需要预先计算图块在对应金字塔层上的雅可比矩阵precomputeReferencePatches

void SparseImgAlign::precomputeReferencePatches()
{
  const int border = patch_halfsize_+1;
  const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);   //得到当前金字塔等级参考图像
  const int stride = ref_img.cols;
  const float scale = 1.0f/(1<<level_);       //1单位/（1<<level_像素）
  const Vector3d ref_pos = ref_frame_->pos();  //参考帧在世界坐标系的位置
  const double focal_length = ref_frame_->cam_->errorMultiplier2();
  size_t feature_counter = 0;
  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
  for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end();
      it!=ite; ++it, ++feature_counter, ++visiblity_it)
  {
      // 确保面片在图像内
    // check if reference with patch size is within image
     //将特征点映射回原来的层数！ ！ ！时，坐标不是取整，就进行插值
    const float u_ref = (*it)->px[0]*scale;      
    const float v_ref = (*it)->px[1]*scale;
    const int u_ref_i = floorf(u_ref);
    const int v_ref_i = floorf(v_ref);
    if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows)
      continue;
    *visiblity_it = true;

    // 这边不能直接使用3d点的坐标，会存在重投影的误差，而是通过单位点乘以深度的方式进行计算
    // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!!
    const double depth(((*it)->point->pos_ - ref_pos).norm());
    const Vector3d xyz_ref((*it)->f*depth);   

    // evaluate projection jacobian
    // 估计投影的雅克比矩阵
    Matrix<double,2,6> frame_jac;
    Frame::jacobian_xyz2uv(xyz_ref, frame_jac);

    // compute bilateral interpolation weights for reference image
    // 对参考图像进行双边差值操作
    const float subpix_u_ref = u_ref-u_ref_i;
    const float subpix_v_ref = v_ref-v_ref_i;
    const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref);
    const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref);
    const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref;
    const float w_ref_br = subpix_u_ref * subpix_v_ref;
    size_t pixel_counter = 0;
    //指向第n个特征第1个像素
    float* cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
    for(int y=0; y<patch_size_; ++y)
    {
      uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
      for(int x=0; x<patch_size_; ++x, ++ref_img_ptr, ++cache_ptr, ++pixel_counter)
      {
	  
        // precompute interpolated reference patch color
	  // 先计算像素插值点，再计算插值点在图像上的梯度
        *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];

        // we use the inverse compositional: thereby we can take the gradient always at the same position
        // get gradient of warped image (~gradient at warped position)
	// 采用逆向组合算法(inverse compositional): 通过采取梯度总是在相同位置这一性质
	// 得到warped的图像 
        float dx = 0.5f * ((w_ref_tl*ref_img_ptr[1] + w_ref_tr*ref_img_ptr[2] + w_ref_bl*ref_img_ptr[stride+1] + w_ref_br*ref_img_ptr[stride+2])
                          -(w_ref_tl*ref_img_ptr[-1] + w_ref_tr*ref_img_ptr[0] + w_ref_bl*ref_img_ptr[stride-1] + w_ref_br*ref_img_ptr[stride]));
        float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
                          -(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));

        // cache the jacobian
	//记录该特征每个像素点的雅可比矩阵，同一特征点不同像素点的frame_jac一样
        jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
            (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
      }
    }
  }
  have_ref_patch_cache_ = true;
}

Ｑ：为什么不直接用地图点，而是参考帧中的特征点（深度单位化的世界坐标）延伸到对应深度的三维点

因为地图点与特征点之间也存在这投影误差，这样延伸保证针孔模型

构造目标函数并计算增量方程的H,b SparseImgAlign::computeResiduals()

double SparseImgAlign::computeResiduals(
    const SE3& T_cur_from_ref,
    bool linearize_system,
    bool compute_weight_scale)
{
  // Warp the (cur)rent image such that it aligns with the (ref)erence image
    //当前迭代金字塔层的图像
  const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);

  if(linearize_system && display_)
    resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0));

  //预计算参考帧特征patch的雅可比矩阵
  //保证computeResiduals前先precomputeReferencePatches
  if(have_ref_patch_cache_ == false)
    precomputeReferencePatches();  //先计算雅可比矩阵

  // compute the weights on the first iteration
  std::vector<float> errors;
  if(compute_weight_scale)
    errors.reserve(visible_fts_.size());       //预留空间大小
  const int stride = cur_img.cols;            //类似与OpenCV Mat中的步；
  const int border = patch_halfsize_+1; //patch的边界；
  const float scale = 1.0f/(1<<level_);    //1单位/1<<level_像素；
  const Vector3d ref_pos(ref_frame_->pos());   //参考帧相机光心在世界坐标系中的位置；
  float chi2 = 0.0;          //光度误差cost
  size_t feature_counter = 0; // is used to compute the index of the cached jacobian   特征索引；
  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
  //遍历参考帧上的特征点
  for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end();
      ++it, ++feature_counter, ++visiblity_it)
  {
    // check if feature is within image
      //检查特征是否在图像中
    if(!*visiblity_it)
      continue;

    // compute pixel location in cur img计算该特征通过位姿变换和相机投影过程后，在当前帧中的像素坐标；
    // 这边不能直接使用3d点的坐标，会存在重投影的误差，而是通过单位点乘以深度的方式进行计算
    const double depth = ((*it)->point->pos_ - ref_pos).norm();
    const Vector3d xyz_ref((*it)->f*depth);     //特征点逆投影得到的世界坐标
    
    const Vector3d xyz_cur(T_cur_from_ref * xyz_ref); 
    const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast<float>() * scale);     //将特征点映射回原来的层数，不是取整就是进行插值
    const float u_cur = uv_cur_pyr[0];
    const float v_cur = uv_cur_pyr[1];
    const int u_cur_i = floorf(u_cur);
    const int v_cur_i = floorf(v_cur);

    // check if projection is within the image
    //确保投影后的像素点在图像中
    if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows)
      continue;

    // compute bilateral interpolation weights for the current image
    //对当前帧图像进行双边插值操作
    const float subpix_u_cur = u_cur-u_cur_i;
    const float subpix_v_cur = v_cur-v_cur_i;
    const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur);
    const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur);
    const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur;
    const float w_cur_br = subpix_u_cur * subpix_v_cur;
    //指向ref_patch_cache_中第n个特征第1个像素
    float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
    size_t pixel_counter = 0; // is used to compute the index of the cached jacobian
    for(int y=0; y<patch_size_; ++y)
    {
	//指向cur_img中该特征块的第1个像素
      uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride + (u_cur_i-patch_halfsize_);

      for(int x=0; x<patch_size_; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
      {
        // compute residual
	  //计算当前特征点的强度（双边插值）
        const float intensity_cur = w_cur_tl*cur_img_ptr[0] + w_cur_tr*cur_img_ptr[1] + w_cur_bl*cur_img_ptr[stride] + w_cur_br*cur_img_ptr[stride+1];
	//计算残差
        const float res = intensity_cur - (*ref_patch_cache_ptr);

        // used to compute scale for robust cost
        if(compute_weight_scale)
          errors.push_back(fabsf(res));

        // robustification
        float weight = 1.0;
        if(use_weights_) {
          weight = weight_function_->value(res/scale_);   //？ ？ ？权重的计算方式
        }

        //残差平方再加权
        chi2 += res*res*weight;
        n_meas_++; //Number of measurements

        if(linearize_system)
        {
	    // 计算Jacobian, 带权重的Hessian 和残差图像
          // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error)
          const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter));  //第n个特征第m个像素对应的雅可比矩阵
          H_.noalias() += J*J.transpose()*weight;     //计算总的H矩阵
          Jres_.noalias() -= J*res*weight;                  
          if(display_)
            resimg_.at<float>((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0;     //在残差图上标记
        }
      }
    }
  }

  // compute the weights on the first iteration
  // 在第一次迭代时计算权重
  if(compute_weight_scale && iter_ == 0)
    scale_ = scale_estimator_->compute(errors);

  return chi2/n_meas_;
}

int SparseImgAlign::solve()
{
  x_ = H_.ldlt().solve(Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

Q:计算残差的时候为什么要加上权重，权重怎么给的？

增量方程的求解函数solve()和更新函数update()

int SparseImgAlign::solve()
{
  x_ = H_.ldlt().solve(Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

//注意求雅克比矩阵的时候，对Rt无法求导数，改成李代数的形式，这边用SE3::exp
void SparseImgAlign::update(
    const ModelType& T_curold_from_ref,
    ModelType& T_curnew_from_ref)
{
  T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);    //? ? ?Ｔk,k-1'=Tk,k-1*Tk-1,k-1' = Tk,k-1 * T(x)^(-1) 
}

更新： $Ｔ_{k,(k-1)^{'})}$ $T_{k,(k-1)^{'}}=T_{k,k-1}T_{k-1,(k-1)^{'}}=T_{k,k-1}T(\psi )=T_{k,k-1}T(\xi )^{-1}$