SVO学习日记-1--2021.1.15

SVO-1-Frame.h–2021.1.15

Frame.h

#ifndef SVO_FRAME_H_
#define SVO_FRAME_H_

#include <sophus/se3.h>
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <boost/noncopyable.hpp>
#include <svo/global.h>

namespace g2o{
  class VertexSE3Expmap;
}
typedef g2o::VertexSE3Expmap g2oFrameSE3;

namespace svo{
  class Point;
  class Feature;
  
typedef list<Feature*> Features;
typedef vector<cv::Mat> ImgPyr;

class Frame : boost::noncopyable{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
  static int		frame_counter_;	//记录帧的数目
  int 			id;		//帧的id
  double		timestamp_;	//图像被记录时的时间戳
  vk::AbstracCamera*	cam_;		//相机模型
  Matrix<double>	Cov_;		//协方差
  ImgPyr		img_pyr_;	//图像金字塔
  Features		fts_;		//图像中集合的特征链表
  vector<Features*>	key_pts_;	//五个特征点以及关联的3d点，主要是用于检测两幅图像中的重叠部分
  bool			is_keyframe_;	//这一帧是否被选为关键帧
  g2oFrameSE3*		v_kf_;		//临时指针--关键帧的g2o节点
  int 			last_published_ts;//上一个被发布的时间戳
  
  Frame(vk::AbstracCamera* cam,const cv::Mat& img,double timestamp;
  ~Frame();
  
  //初始化新的帧并创建图像金字塔
  void initFrame(const cv::Mat& img);
  
  //设置此帧为关键帧
  void setKeyframe();
  
  //给图像添加特征
  void addFeature(Feature* ftr);
  
  //设置的五个特征点是最近的4个图像焦点以及图像中心
  //这些点用于快速的检查是否两帧图像有重叠视角
  void setKeyPoints();
  
  //检查是否我们能选择更好的五个关键点
  void checkKeyPoints(Feature* ftr);
  
  //如果一个点被删除，我们必须要删除对应的关键点
  void removeKeyPoint(Feature* ftr);
  
  //返回观测点的数目
  inline size_t nObs() const{return fts_.size();}
  
  //检查在世界坐标系下的点在图像中能否被看到
  inline isVisble(const Vector3d& xyz_w) const;
  
  //完整分辨率的图像---金字塔最底层
  inline const cv::Mat& img() const{
    return img_pyr_[0];
  }
  
  //是否能被选为关键帧
  inline bool isKeyframe() const{
    return is_keyframe_;
  }
  
  /* w 世界
   * c 像素
   * f 归一化坐标 相机坐标系
   * */
  
  //从世界坐标系-像素坐标系 Vector3d-Vector2d
  inline Vector2d w2c(const Vector3d& xyz_w) const{
    return cam_ -> world2cam(T_f_w_ *xyz_w);
  }
  
  //像素坐标到归一化坐标
  inline Vector3d c2f(const Vector2d& px) const{
    return cam_ -> cam2world(px[0],px[1]);
  }
  //同上
  inline Vector3d c2f(const double x,const double y) const{
    return cam_-> cam2world(x,y);
  }
  
  //世界坐标到相机坐标
  inline Vector3d w2f(const Vector3d& xyz_w) const{
    return T_f_w_ * xyz_w;
  }
  
  //归一化到世界坐标
  inline Vector3d f2w(const Vector3d& f) const{
    return T_f_w_.inverse() * f;
  }
  
  //归一化到像素
  inline Vector2d f2c(const Vector3d& f) const{
    return cam_ -> world2cam(f);
  }
  
  //在世界坐标系的平移或者说是位置
  inline Vector3d pos() const{
    return T_f_w_.inverse().translation();
  }
  
  //帧中投影的3D点单位平面坐标
  inline static void jacobian_xyz2uv(const Vector3d& xyz_in_f,Matrix<double,2,6>& J){
    const double x = xyz_in_f[0];
    const double y = xyz_in_f[1];
    const double z_inv = 1. / xyz_in_f[2];
    const double z_inv_2 = z_inv * z_inv;
    
    //误差相对于李代数的导数矩阵2x6
    J(0,0) = -z_inv;
    J(0,1) = 0.0;
    J(0,2) = -x * z_inv_2;
    J(0,3) = x * y * z_inv_2;
    J(0,4) = -(1.0 + x * J(0,2));
    J(0,5) = y * z_inv;
    
    J(1,0) = 0;
    J(1,1) = -z_inv;
    J(1,2) = y * z_inv_2;
    J(1,3) = 1.0 + y * J(1,2);
    J(1,4) = - x * y * z_inv_2;
    J(1,5) = -x * z_inv;
  }
};

namespace frame_utils{
  //创建图像金字塔，采样一半
  void createImgPyramid(const cv::Mat& img_level_0,int n_levels,ImgPyr& pyr);
  
  //获得图像中的场景深度的均值
  bool getScenceDepth(const Frame& frame,double& depth_mean,double& depth_min);
}
}

#endif