SVO学习日记-7--2021.1.31

SVO-7-sparse_img_align–2021.1.31

sparse_img_align.h

#ifndef SVO_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_

#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <svo/global.h>

namespace vk{
  class AbstractCamera;
}

namespace svo{
  class Feature;
  
  //优化帧间位姿，通过最小化特征patches的光度误差
  class SparseImgAlign : public vk::NLLSSolver<6,SE3>{
   static const int patch_halfsize_ = 2;	//patch一半大小
   static const int patch_size_ = 2 * patch_halfsize_;	//patch边长
   static const int patch_area_ = patch_size_ * patch_size_;//Patch面积
   
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    cv::Mat resimg_;
    
    SparseImgAlign(int n_levels,	//max_level，最大层数，粗金字塔
		   int min_levels,	//细金字塔
		   int n_iter,		//迭代次数
		   Method method,	//方法的选择
		   bool display,	
		   bool verbose);	//输出统计信息
    size_t run(FramePtr ref_frame,FramePtr cur_frame);//run函数，输入参考帧和当前帧
    
    Matrix<double,6,6> getFisherInformation();
    
  protected:
    FramePtr ref_frame_;	//参考帧，有梯度像素的深度
    FramePtr cur_frame_;	//仅仅知道当前图像
    int level_;			//当前优化所运行在的金字塔层数
    bool display_;		//显示残差图像
    int max_level_;		//图像对其的粗金字塔层
    int min_level_;		//细金字塔层
    
    Matrix<double,6,Dynamic,ColMajor> jacobian_cache_;
    bool have_ref_patch_cache_;
    cv::Mat ref_patch_caches_;
    std::vector<bool> visible_fts_;
    
    void precomputeReferencePatches();
    virtual double computeResiduals(const SE3& model,
				    bool linearize_system,
				    bool compute_weight_scale = false);
    virtual int solve();
    virtual void update (const ModelType& old_model,ModelType& new_model);
    virtual void startIteration();
    virtual void finishIteration();
  };
}

#endif
  

sparse_img_align.cpp

#include <algorithm>
#include <svo_practice/sparse_img_align.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <svo/config.h>
#include <svo_practice/point.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>

namespace svo{
  
//构造函数
svo::SparseImgAlign::SparseImgAlign(int max_levels, int min_levels, 
				    int n_iter, Method method, bool display, 
				    bool verbose) : 
				    display_(display),max_levels_(max_levels),min_levels_(min_levels){
	n_iter_ = n_iter;
	n_iter_init_ = n_iter_;
	method_ = method;
	verbose_ = verbose;
	eps_ = 0.000001;
}

size_t svo::SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
{
  //复位solver中的变量值
  reset();
  
  //判断参考帧里有没有特征点
  if(ref_frame -> fts_.empty()){
    SVO_WARN_STREAM("SparseImgAlign: no features to track!");
    return 0;
  }
  
  //赋值变量
  ref_frame_ = ref_frame;
  cur_frame_ = cur_frame;
  
  //存储每个特征点的patch,大小为：feature_size * patch_area
  ref_patch_cache_ = cv::Mat(ref_frame_ -> fts_.size(),patch_area_,CV_32F);
  
  //存储所有patch的雅可比，大小：6 * ref_patch_cache_.size
  jacobian_cache_.resize(Eigen::NoChange,ref_patch_cache_.rows * patch_area_);
  
  //存储可见的特征点
  visible_fts_.resize(ref_patch_cache_.rows,false);
  
  //获得从参考帧到当前帧的变换
  SE3 T_cur_from_ref(cur_frame_->T_f_w_*ref_frame_->T_f_w_.inverse());
  
  //在不同的金字塔层对T_c_r进行稀疏图像对齐优化，由粗到精，这里是指从金字塔顶端到金字塔最低端，
  //顶端的分辨率低，反之低端分辨率高，这样会有着更好的初始值
  for(level_ = max_levels_;level_ >= min_levels_;--level_){//从最高层开始遍历
    mu_ = 0.1;
    jacobian_cache_.setZero();	//patch的雅可比设置为0
    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_;
  
  //n_meas表示前一帧所有特征点块(feature patch)像素投影后在cur_frame中的像素个数
  //表示特征点数
  return n_meas / patch_area_;
}

Matrix<double,6,6> SparseImgAlign::getFisherInformation(){
  //图像噪声
  double sigma_i_sq = 5e-4*255*255;
  Matrix<double,6,6> I = H_ / sigma_i_sq;
  return I;
}

//逆向组合法的预计算阶段，计算J：主要目的是提前计算J
void SparseImgAlign::precomputeReferencePatches()
{
  const int border = patch_halfsize_ + 1;	//先定一个边界
  
  const cv::Mat& ref_img = ref_frame_ -> img_pyr_.at(level_);	//当前金字塔层的图像，按照上面的金字塔层定义，这里的level是粗金字塔，最上层
  const int stride = ref_img.cols;	//当前金字塔层图像的列数
  const float scale = 1.0f / (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){

    //原图像的的像素坐标变换到对应金字塔层
  const float u_ref = (*it)-> px[0] * scale;
  const float v_ref = (*it)-> px[1] * scale;
  
  //将特征点变换到对应金字塔图像上,并判断feature的patch是否在图像内
  //变换后向下取整
  const int u_ref_i = floorf(u_ref);
  const int v_ref_i = floorf(v_ref);
  
  //判断特征点patch是否在变换后的图像内,不包括边界,且具有三维坐标可见
  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;
  
  //得到相机坐标系下的特征点,并对变换矩阵求雅可比矩阵
  
  //求得的是深度,点的坐标-ref相机坐标的值的模=深度
  const double depth(((*it) -> point -> pos_ - ref_pos).norm());
  const Vector3d xyz_ref((*it) -> f * depth);//上面求得是归一化坐标,这里是cam下的坐标,因为下面求得雅可比需要相机系下的坐标
  
  Matrix<double,2,6> frame_jac;//定义雅可比矩阵
  Frame::jacobian_xyz2uv(xyz_ref,frame_jac);
  
  //利用该层金字塔图像对每个patch中的像素进行插值,计算图像对像素的导数
  //参考帧图像中计算双线性插值的权重
  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;
  
  //首指针 + 特征点的索引 * patch的大小,就可以遍历ref_patch_cache_内每个特征点Patch的访问指针
  //data表示像素值,cache_ptr表示对应的像素值,等号右边的值赋给左边
  float* cache_ptr = reinterpret_cast<float*>(ref_patch_caches_.data) + 
		     patch_area_ * feature_counter;
  //开始逐像素遍历图像块
  for(int y = 0;y < patch_size_;++y){
    
    //计算出该层金字塔图像上,这个特征patch的第一个像素指针
    //插值后,在这一金字塔层图像,这个特征点patch的第一个像素指针
    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){
	
	//我们已知这个特征点Patch在金字塔层上的点,向下取整得到一个值,插值也会得到一个值
	//向下取整插值的,这样插值会得到第一个点的值,要多次插值得到所有在该层金字塔上特征点patch的值
	*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];
	
	//逆混合求导,在同一个位置求导就行了
	//利用四个方向像素的差除以2
	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]));
	
	//图像导数与se3的导数的乘积求雅可比
	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;
}

//逆向组合法对cur_image和ref_image进行计算res
//param:model	待优化量,linearize_system	是否为线性系统,compute_weight_scale是否计算权重
double SparseImgAlign::computeResiduals(const SE3& T_cur_from_ref,bool linearize_system,bool compute_weight_scale){
  //得到当前金字塔的图像
  const cv::Mat& cur_img = cur_frame_ -> img_pyr_.at(level_);
  if(linearize_system && display_){	//用来显示res图像
    resimg_ = cv::Mat(cur_img.size(),CV_32F,cv::Scalar(0));
  }
  
  //是否计算了需要的导数等信息,没有的话执行上面的函数,预先计算J
  if(have_ref_patch_cache_ == false){
    precomputeReferencePatches();
  }
  
  //计算第一次迭代的权重
  std::vector<float> errors;//存放res
  if(compute_weight_scale)
    errors.reserve(visible_fts_.size());
  
  const int stride = cur_img.cols;//获取当前图像列
  const int border = patch_halfsize_ + 1;//边界定义
  const float scale = 1.0f / (1 << level_);//尺度
  const Vector3d ref_pos(ref_frame_ -> pos());//参考图像帧位置
  float chi2 = 0.0;
  
  //用于计算雅可比的索引
  size_t feature_counter = 0;
  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
  
  //对每个ref图像上的特征点进行循环
  for(auto it = ref_frame_ -> fts_.begin();it != ref_frame_ -> fts_.end();++it,++feature_counter,++visiblity_it){
    if(!*visiblity_it) continue;//不在图像中则忽略
    
    //获取深度,归一化
    //整个以下的步骤将参考帧图像坐标转换到了当前帧对应的图像金字塔上,并获取对应像素值
    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);
    
    //判断转换后是否在当前帧这一层的金字塔上
    for(u_cur_i<0 || v_ref_i<0 || u_cur_i-border<0 || v_cur_i-border<0 || 
        u_cur_i+border>=cur_img.cols || v_cur_i>=cur_img.rows)
      continue;
    
    //获取插值系数与权重
    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;
    
    //首指针 + 特征点的索引 * patch的大小,就可以遍历ref_patch_cache_内每个特征点Patch的访问指针
    //data表示像素值,cache_ptr表示对应的像素值,等号右边的值赋给左边
    float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_caches_.data)
				 + patch_area_ * feature_counter;
    size_t pixel_counter = 0;
    
    //对每个当前帧图像上的特征周围的Patch进行插值
    for(int y = 0;y < patch_size_;++y){
      
      //计算出该层金字塔图像上,这个特征patch的第一个像素指针
      //插值后,在这一金字塔层图像,这个特征点patch的第一个像素指针
      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){
	
	//计算残差
	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);
	
	//计算尺度用于计算鲁棒的cost
      if(compute_weight_scale) errors.push_back(fabsf(res));
      
      float weight = 1.0;
      if(use_weights_){
	weight = weight_function_ -> value(res / scale_);
      }
      
      //残差的平方乘以权重,权重与残差和尺度有关
      chi2 += res * res * weight;
      n_meas_++;//计算的所有Patch中的像素数量
      
      //构建方程
      if(linearize_system){
	const Vector6d J(jacobian_cache_.col(feature_counter * patch_area_ + pixel_counter))'
	H_.noalias() += J*J,transpose() * weight;
	Jres_.noalias() -= J*res*weight;
	if(display_){
	  resimg_.at<float>((int) v_cur + y - patch_halfsize_,(int) u_cur + x - patch_halfsize_) = res / 255;
	}
      }
    }
  }
  //在第一次迭代的时候计算权重
  if(compute_weight_scale && iter == 0){
    scale_ = scale_estimator_ -> compute(errors);
  }
  return chi2 / n_meas_;
}
  
  //采用DLT计算最小二乘的问题
int SparseImgAlign::solve(){
  x_ = H_.ldlt().solve(Jres);
  if(bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

//逆向组合更新T
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_);
}

void SparseImgAlign::startIteration(){}

//显示残差图像
void SparseImgAlign::finishIteration(){
  if(display_){
    cv::nameWindow("residuals",CV_WINDOW_AUTOSIZE);
    cv::imshow("residuals",resimg_ * 10);
    cv::waitKey(0);
  }
}
}