SVO学习日记-9--2021.2.2

SVO-9-reprojector–2021.2.2

reprojector.h

#include <svo/global.h>
#include <svo/matcher.h>

namespace vk{
  class AbstractCamera;
}

namespace svo{
  class Map;
  class Point;

  /* 投影3d点从地图到图像,并且寻找对应的特征
   * 我们不对每一个点进行匹配搜索,而仅仅是每个cell里有一个点
   * 因此对于匹配的特征点实现了平均的分布,同时节约了处理时间但又不用投影所有的3D点
   */
  class Reprojector{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    //重投影的参数配置
    struct Options{
      size_t max_n_kfs;	//重投影中关键帧的最大数目
      bool find_match_direct;
      
      Options():max_n_kfs(10),find_match_direct(true){}
    } options_;
    
    size_t n_matches_;	//匹配上的cell数目
    size_t n_trials_;	//尝试匹配数目
    
    Reprojector(vk::AbstractCamera* cam,Map& map);
    ~Reprojector();
    
    //从地图上向图像中投影3D点,首先选择具有重叠视角的关键帧
    //然后仅仅投影这些地图点
    void reprojectMap(FramePtr frame,
		      std::vector<std::pair<FramePtr,std::size_t>& overlap_kfs>;
		      
  private:
    
    //一个候选点的struct,从地图向图像投影point,用于在图像中搜索匹配的特征点
    struct Candidate{
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      Point* pt;//3D点
      Vector2d px;//投影的2d像素位置
      Candidate(Point* pt,Vector2d& px): pt(pt),px(px){}
    };
    
    typedef std::list<Candidate> Cell;
    typedef std::vector<Cell*> CandidateGrid;
    
    //grid存储一系列的候选匹配,每一个网格试图寻找一个匹配
    struct Grid{
      CandidateGrid cells;
      vector<int> cell_order;
      int cell_size;
      int grid_n_cols;
      int grid_n_rows;
    };
    
    Grid grid_;
    Matcher matcher_;
    Map& map_;//使用初始化列表来初始化地图
    
    //比较点的质量
    static bool pointQualityComparator(Candidate& lhs,Candidate& rhs);
    void initializeGrid(vk::AbstractCamera* cam);//初始化grid
    void resetGrid();//重置grid
    bool reprojectCell(Cell& cell,FramePtr frame);
    bool reprojectPoint(FramePtr frame,Point* point);
  };
}

#endif

reprojector.cpp

#include <algorithm>
#include <stdexcept>
#include <svo_practice/reprojector.h>
#include <svo_practice/frame.h>
#include <svo_practice/point.h>
#include <svo_practice/feature.h>
#include <svo_practice/map.h>
#include <svo_practice/config.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <vikit/abstract_camera.h>
#include <vikit/math_utils.h>
#include <vikit/timer.h>

namespace svo{
svo::Reprojector(vk::AbstractCamera* cam, svo::Map& map): map_(map)
{
  initializeGrid(cam);
}

svo::~Reprojector()
{
  //删除这些网格中点的链表的指针
  std::for_each(grid_.cells.begin(),grid_.cells.end(),[&](Cell* c){delete c;});
}

//初始化网格并分配空间
void initializeGrid(vk::AbstractCamera* cam)
{
  //读取网格大小
  grid_.cell_size = Config::gridSize();
  
  //计算网格列数\行数
  grid_.grid_n_cols = ceil(static_cast<double>(cam -> width())/grid_.cell_size);
  gird_.grid_n_rows = ceil(static_cast<double>(cam -> height())/grid_.cell_size);
  
  //留出网格数量的空间
  grid_.cells.resize(grid_.grid_n_cols * grid_.grid_n_rows);
  
  //为每个小网格分配内存
  std::for_each(grid_.cells.begin(),grid_.cells.end(),[&](Cell*& c){c = new Cell;});
  
  //网格按照顺序来预留大小
  grid_.cell_order.resize(grid_.cells.size());
  
  //顺序赋值
  for(size_t i = 0;i < grid_.cells.size();++i){
    grid_.cell_order[i] = i;
  }
  
  //打乱顺序函数
  random_shuffle(grid_.cell_order.begin(),grid_.cell_order.end());
}

//重置
void resetGrid()
{
  n_matches_ = 0;
  n_trials_ = 0;
  //清空cell链表
  std::for_each(grid_.cell.begin(),grid_.cell.end(),[&](Cell* c){c -> clear();})
}

/* 将地图中的关键帧上的3D点投影到图像网格里,并记录对应帧和重叠度
 * 将候选点投影到图像网格中,对每个网格进行图像对齐
 * 每个网格只对应于一个特征,candidate提供更多机会
 */
void Reprojector::reprojectMap(FramePtr frame,
			       std::vector<std::pair<FramePtr,std::size_t>>& overlap_kfs){
  resetGrid();//重置网格
  
  //开始寻找具有共视关系的关键帧
  SVO_START_TIMER("reproject_kfs");
  
  //找到有重叠关系的关键帧,返回共享指针和距离
  list<pair<FramePtr,double>> close_kfs;
  map_.getCloseKeyframes(frame,close_kfs);
  
  //根据重叠程度排序
  close_kfs.sort(boost::bind(&std::pair<FramePtr,double>::second,_1) < 
		 boost::bind(&std::pair<FramePtr,double>::second,_2));
  
  //对所有具有重叠关系的最近的N个关键帧进行地图点的重投影,仅仅存储那些落在网格里的点
  size_t n = 0;
  //预留空间
  overlap_kfs.reserve(options_.max_n_kfs);
  
  //遍历这些具有共视关系的最近的N帧
  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;//获取close_kfs里的每一帧,并创建对象
    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));//reserve需要push_back,压入的是最近的
    
    //试图对其他关键帧观测到的地图点进行重投影
    for(auto it_ftr = ref_frame -> fts_.begin(),ite_ftr = ref_frame -> fts_.end();it_ftr != ite_ftr;++it_ftr){
      if((*it_ftr) -> point == NULL) continue;//检查
      
      //确保每个点向帧上指投影一次
      if((*it_ftr) -> point -> last_projected_kf_id_ == frame -> id_) continue;
      
      //把投影帧Id赋值给投影Id,并进行投影图像,放入网格中
      (*it_ftr) -> point -> last_projected_kf_id_ = frame -> id_;
      
      if(reprojectPoint(frame,(*it_ftr) -> point)){
	overlap_kfs.back().second++;//投影成功的个数,重叠程度
      }
    }
  }
  SVO_STOP_TIMER("reproject_kfs");
  
  //候选点是指未分配的收敛点,关键帧上是已经收敛的地图点
  //候选点是深度滤波得到的收敛的点
  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()){
      if(!reprojectPoint(frame,it -> first)){//投影候选点
	it -> first -> n_failed_reproj += 3;
	if(it -> first -> n_failed_reproj_ > 30){	//失败10次,就是10帧,删除该点
	  map_.point_candidates_.deleteCandidate(*it);
	  it = map_.point_candidates_.candidates_.erase(it);
	  continue;
	}
      }
      ++it;
    }
  }
  SVO_STOP_TIMER("reproject_candidates");
  
  //现在开始遍历每一个网格,并选择点进行匹配
  //最后,我们在每个网格单元里应该有最大的一个投影点
  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");
  }
}

bool pointQualityComparator(Candidate& lhs, Candidate& rhs)
{
  if(lhs.pt -> type_ > rhs.pt -> type_)
    return true;
  return false;
}

/* 在每个cell里找到一个好的点进行匹配,有一个成功则返回
 * 将新投影的特征加入到帧中
 */
bool reprojectCell(Cell& cell, FramePtr frame)
{
  //在网格内,按照点的质量排序,优先使用优质点
  cell.sort(boost::bind(&Reprojector::pointQualityComparator,_1,_2));
  Cell::iterator it = cell.begin();	//创建迭代器
  while(it != cell.end()){
    ++n_trials_;	//实验次数增加
    if(it -> pt -> type_ == Point::TYPE_DELETED){	//若是删除点的话
      it = cell.erase(it);	//从cell里删除
      continue;
    }
    
    //定义
    bool found_match = true;
    
    //前面有定义直接找的方法
    if(options_.find_match_direct){
      found_match = matcher_.findMatchDirect(*it -> it,*frame,it -> px);
    }
    
    //如果没找到,则重投影失败
    if(!found_match){
      it -> pt -> n_failed_reproj_++;	//叠加失败次数
      if(it -> pt -> type_ == Point::TYPE_UNKONWN && it -> pt -> n_failed_reproj_ > 15){
	map_.safeDeletePoint(it -> pt);//未知点,观测15次没有就删除
      }
      
      //候选点,30次没观测到,删除
      if(it -> pt -> type_ == Point::TYPE_CANDIDATE && it -> pt -> n_failed_reproj_ > 30){
	map_.point_candidates_.deleteCandidatePoint(it -> pt);
      }
      it = cell.erase(it);
      continue;
    }
  
  //如果成功观测到
  it -> pt -> n_successed_reproj_++;
  
  //未知点则升级为好点
  if(it -> pt -> type_ == Point::TYPE_UNKONWN && it -> pt -> n_successed_reproj_ > 10){
    it -> pt -> type_ = Point::TYPE_GOOD;
  }
  
  //在帧上找到匹配点,加入到帧中
  Feature* new_feature = new Feature(frame.get(),it -> px,matcher_.search_level_);
  frame -> addFrame(new_feature);
  //把point的指针给feature
  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();
  }
  //成功之后返回,只要有一个匹配上就行
  it = cell.erase(it);
  return true;
return false;    
}

bool reprojectPoint(FramePtr frame, Point* point)
{
  //获取3d点到像素坐标的转换
  Vector2d px(frame -> w2c(point -> pos_));
  
  //判断在边界的8个像素之内
  if(frame -> cam_ -> isInFrame(px,cast<int>(),8)){
    
    //计算在第几格
    const int k = static_cast<int>(px[1] / grid_.cell_size) * grid_.grid_n_cols +
		  static_cast<int>(px[0] / grid_.cell_size);
		  
    grid_.cells.at(k) -> push_back(Candidate(point,px));
    return true;
  }
  return false;
}