SVO学习日记-3--2021.1.17

SVO-3-Feature.h–2021.1.16

Feature.h

#ifndef SVO_FEATURE_H_
#define SVO_FEATURE_H_

#include <frame.h>

namespace svo{
  struct Feature{
    EIGEN_MAKE_ALGINED_OPERATOR_NEW
    
    enum FeatureType{
      CORNER,
      ENGELET
    };
    
    FeatureType type;		//角点还是边
    Frame* frame;		//当特征被检测到时帧的指针
    Vector2d px;		//金字塔最底层的像素坐标
    Vector3d f;			//归一化平面的坐标
    int level;			//特征点被提取的那一层金字塔
    Point* point;		//对应特征的3D点
    Vector2d grad;		//边的主梯度方向（没有）
    
    //对应的像素特征
    Feature(Frame* _frame,const Vector2d& _px, int _level):
      type(CORNER),
      frame(_frame),
      px(_px),
      f(frame -> cam_ -> cam2world(px)),
      level(_level),
      point(NULL),
      grad(1.0,0.0)
      {}
      
    //对应的归一化平面上的特征  
    Feature(Frame* _frame,const Vector2d& _px, const Vector3d& _f,int _level):
      type(CORNER),
      frame(_frame),
      px(_px),
      f(_f),
      level(_level),
      point(NULL),
      grad(1.0,0.0)
      {}
      
    //对应的3D特征  
    Feature(Frame* _frame,Point* _point, const Vector2d& _px, const Vector3d& _f,int _level):
      type(CORNER),
      frame(_frame),
      px(_px),
      f(frame -> cam_ -> cam2world(px)),
      level(_level),
      point(_point),
      grad(1.0,0.0)
      {}
  };
}

#endif

Feature_detection.h

#ifndef SVO_FEATURE_DETECTION_H_
#define SVO_FEATURE_DETECTION_H_

#include <svo/global.h>
#include <svo_practice/frame.h>

namespace svo{
  namespace feature_detection{
    //对不同类型的特征进行细化，SVO中只有角点
    //对于角点检测来说这只是个临时变量，特征点是从这里被初始化的
    struct Corner{
      int x;		//图像中角点的x坐标
      int y;		//图像中角点的y坐标
      int level;	//角点的金字塔层数
      float score;	//角点的shi-tomasi得分
      float angle;	//梯度和特征之间的关系
      Corner(int x,int y,float score,int level,float angle):x(x),y(y),level(level),score(score),angle(angle){}
    };
    
    //定义角点的容器
    typedef vector<Corner> Corner;
    
    //从这个抽象类中进行角点的检测，服务于下面的一个类
    class AbstractDetector{
    public:
      AbstractDetector(const int img_width,
		       const int img_height,
		       const int cell_size,
		       const int n_pyr_levels);
      
      virtual ~AbstractDetector(){};
      
      //检测
      virtual void detect(Frame* frame,
			  const ImgPyr& img_pyr,
			  const double detection_threshold,
			  Features& fts) = 0;
      
      //被占据的网格
      void setGridOccpuancy(const Vector2d& px);
      
      //特征所占据的网格
      void setExistFeatures(const Feature& fts);
      
    protected:
      static const int border_ = 8;	//特征不应该在离图像边界小于8的地方出现
      const int cell_size_;
      const int n_pyr_levels_;
      const int grid_n_cols_;		//网格列数
      const int grid_n_rows_;		//网格行数
      vector<bool> grid_occupancy_;
      
      void resetGrid();
      
      //获取网格索引
      inline int getCellIndex(int x,int y,int level){
	const int scale = (1 << level);
	return (scale * y) / cell_size_ * grid_n_cols_ + (scale * x) / cell_size_;
      }
    };
    
    typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
    
    class FastDetector : public AbstractDetector{
    public:
      FastDetector(const int img_width,
		   const int img_height,
		   const int cell_size,
		   const int n_pyr_levels);
      
      virtual ~FastDetector(){}
      
      virtual void detect(Frame* frame,
			  const ImgPyr& img_pyr,
			  const double detection_threshold,
			  Features& fts);
    };
  }
}

#endif

Feature_detection.cpp

#include <svo/global.h>
#include <svo_practice/frame.h>
#include <svo_practice/feature.h>
#include <fast/fast.h>
#include <vikit/vision.h>

namespace svo{
  namespace feature_detection{
    
    //抽象类定义
  AbstractDetector::AbstractDetector(const int img_width, 
				     const int img_height, 
				     const int cell_size, 
				     const int n_pyr_levels):
				     cell_size_(cell_size),
				     n_pyr_levels_(n_pyr_levels),
				     grid_n_cols_(ceil(static_cast<double>(img_width) / cell_size_)),	//图像宽度除以网格大小等于网格列数
				     grid_n_rows_(ceil(static_cast<double>(img_height / cell_size_)),	//图像高度除以网格大小等于网格行数
				     grid_occopancy_(grid_n_cols_ * grid_n_rows_,false)		//网格行数x网格列数=网格数
				     {}
				     
  void AbstractDetector::resetGrid(){
    std::fill(grid_occopancy_.begin(),grid_occopancy_.end(),false);
  }
  
  //将特征点所在的网格设置为occupancy
  void AbstractDetector::setExistingFeatures(const Features& fts){
    std::for_each(fts.begin(),fts.end(),[&](Feature* i)){
      grid_occopancy_.at(static_cast<int>(i -> px[1]/cell_size_)*grid_n_cols_
		       + static_cast<int>(i -> px[0]/cell_size_)) = true;
    });
  }
  //同上，这个是像素点
  void AbstractDetector::setGridOccpuancy(const Vector2d& px){
    grid_occopancy_.at(static_cast<int>(px[1] / cell_size_) * grid_n_cols_ + 
		       static_cast<int>(px[0] / cell_size_)) = true;
  }
  
  void FastDetector::detect(Frame* frame,
			    const ImhPyr& img_pyr,
			    const double detection_threhold,
			    Features& fts){
    //每个网格里面一个特征点
    Corner corners(grid_n_cols_ * grid_n_rows_,Corner(0,0,detection_threhold,0,0.0f));
    
    //循环金字塔
    for(int L = 0;L < n_pyr_levels_;++i){
      const int scale = (1 << L);
      vector<fast::fast_xy> fast_corners;	//存储特征点的容器
      
      //加速操作及筛选操作
#if __SSE2__
      fast::fast_corner_detect_10_sse2(
          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#elif HAVE_FAST_NEON
      fast::fast_corner_detect_9_neon(
          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#else
      fast::fast_corner_detect_10(
          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
#endif
      vector<int> scores, nm_corners;
      
      //计算fast角点的得分
      fast::fast_corner_detect_10((fast::fast_byte*) img_pyr[L].data,
	img_pyr[L].cols,fast_corners,20,scores);
      
      //3x3区域内取最大值
      fast::fast_nonmax_3x3(fast_corners,scores,nm_corners);
      
      for(auto it = nm_corners.begin(),ite = nm_corners.end();it != ite;++it){
	//取最大值的点
	fast::fast_xy& xy = fast_corners.at(*it);
	
	const int k = static_cast<int>((xy.y * scale) / cell_size_)* grid_n_cols_ +
		      static_cast<int>((xy.x * scale) / cell_size_);//计算在第几个网格内
		      
	if(grid_occopancy_[k]) continue;	//如果该网格有特征点，则继续
	//计算得分，这个特征点->对应金字塔层
	const float score = vk::shiTomasiScore(img_pyr[L],xy.x,xy.y);
	if(score > corners.at(k).score)
	  corners.at(k) = Corner(xy.x*scale,xy.y*scale,score,L,0.0f);
      }
    }
    
    std::for_each(corners.begin(),corners.end(),[&](Corner& c)){
      if(c.score > detection_threhold)
	//大于阈值则加入feature
	fts.push_back(new Feature(frame,Vector2d(c.x,c.y),c.level));
    });
    
    resetGrid();
  }
  }
}