SVO代码分析

未分类

frame.h

g2oFrameSE3(VertexSE3Expmap); //定义了一个位姿结点
list<Feature*>Features;
vector<cv::Mat> ImgPyr; //存放图像金字塔
class Frame
{
 static int                    frame_counter_;         //!< Counts the number of created frames. Used to set the unique id.
  int                           id_;                    //!< Unique id of the frame.
  double                        timestamp_;             //!< Timestamp of when the image was recorded.
  vk::AbstractCamera*           cam_;                   //!< Camera model.
  Sophus::SE3                   T_f_w_;                 //!< Transform (f)rame from (w)orld. 它的逆为该帧在世界坐标系下的位姿。
  Matrix<double, 6, 6>          Cov_;                   //!< Covariance.
  ImgPyr                        img_pyr_;               //!< Image Pyramid.
  Features                      fts_;                   //!< List of features in the image.
  vector<Feature*>              key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
  bool                          is_keyframe_;           //!< Was this frames selected as keyframe?
  g2oFrameSE3*                  v_kf_;                  //!< Temporary pointer to the g2o node object of the keyframe.
  int                           last_published_ts_;     //!< Timestamp of last publishing.
 Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
  ~Frame();
  /// Initialize new frame and create image pyramid.
  void initFrame(const cv::Mat& img);//初始化新的帧并创建图像金字塔。初始化金字塔时用frame_utils中的createImgPyrmaid.其中的参数n_level为Max(config::npyrlevels,config::kitMaxLevel);
  /// Select this frame as keyframe.
  void setKeyframe();  //setKeyPoints();is_keyframe=true;
  /// Add a feature to the image
  void addFeature(Feature* ftr);//将特征ftr存储在fts_中。
  /// The KeyPoints are those five features which are closest to the 4 image corners
  /// and to the center and which have a 3D point assigned. These points are used
  /// to quickly check whether two frames have overlapping field of view.
  void setKeyPoints(); //遍历fts_中的所有特征，对每个特这个进行checkKeyPoint，选出5个关键点，对应于key_pts_;这5个点的feature是位于图像4个角和中心的点，且必须有对应的已知3D points，这些点用于快速检测两帧是否有共是
  /// Check if we can select five better key-points.
  void checkKeyPoints(Feature* ftr);// 中间点要尽可能离中心近，四个角的点要尽量离中心点远。key_pts_中的五个点的顺序一次是中间，右上，右下，左下，左上
  /// If a point is deleted, we must remove the corresponding key-point.
  void removeKeyPoint(Feature* ftr);//将keyPoint移除后，进行setKeyPoints(),保证key_pts_始终有5个点
  /// Return number of point observations.
  inline size_t nObs() const { return fts_.size(); }
  /// Check if a point in (w)orld coordinate frame is visible in the image.
  bool isVisible(const Vector3d& xyz_w) const;
  /// Full resolution image stored in the frame.
  inline const cv::Mat& img() const { return img_pyr_[0]; }
  /// Was this frame selected as keyframe?
  inline bool isKeyframe() const { return is_keyframe_; }
  /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
  inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }//世界到像素
  /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
  inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }//像素到相机
  /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
  inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }//像素到相机
  /// Transforms point coordinates in world-frame (w) to camera-frams (f).
  inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; } //世界到相机
  /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
  inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }//相机到世界
  /// Projects Point from unit sphere (f) in camera pixels (c).
  inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }//相机到像素
  /// Return the pose of the frame in the (w)orld coordinate frame.
  inline Vector3d pos() const { return T_f_w_.inverse().translation(); } //返回该帧在世界坐标系下的位移
}
/// Some helper functions for the frame object.
namespace frame_utils {
/// Creates an image pyramid of half-sampled images.
void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
/// Get the average depth of the features in the image.
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min);//获得图像中features的平均深度和frame中所能观测到的points的最小深度
} // namespace frame_utils

feature.h

/// A salient image region that is tracked across frames.
struct Feature
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum FeatureType {
    CORNER,
    EDGELET
  };
  FeatureType type;     //!< Type can be corner or edgelet.
  Frame* frame;         //!< Pointer to frame in which the feature was detected.
  Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
  Vector3d f;           //!< Unit-bearing vector of the feature.   // f = [(u-ox)/fx , (v-oy)/fy , 1] ;  f * depth --> [x , y , z] : 3d in this frame      f为像素反投影到归一化平面
  int level;            //!< Image pyramid level where feature was extracted.
  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
  Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
  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)
  {}
  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
    type(CORNER),
    frame(_frame),
    px(_px),
    f(_f),
    level(_level),
    point(_point),
    grad(1.0,0.0)
  {}
};

Point.h

typedef g2o::VertexSBAPointXYZ g2oPoint;
class Feature;
typedef Matrix<double, 2, 3> Matrix23d;
/// A 3D point on the surface of the scene.
class Point : boost::noncopyable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum PointType {
    TYPE_DELETED,
    TYPE_CANDIDATE,
    TYPE_UNKNOWN,
    TYPE_GOOD
  };
  static int                  point_counter_;           //!< Counts the number of created points. Used to set the unique id.
  int                         id_;                      //!< Unique ID of the point.
  Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
  Vector3d                    normal_;                  //!< Surface normal at point.
  Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
  bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
  list<Feature*>              obs_;                     //!< References to keyframes which observe the point.  该Point能被多个关键帧观测到；obs_存储了能观测到该点的关键帧中对应的feature
  size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
  g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
  int                         last_published_ts_;       //!< Timestamp of last publishing.
  int                         last_projected_kf_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
  PointType                   type_;                    //!< Quality of the point.
  int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
  int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
  int                         last_structure_optim_;    //!< Timestamp of last point optimization
  Point(const Vector3d& pos);
  Point(const Vector3d& pos, Feature* ftr);   //每初始化一个点，point_count_++,其对应的id 为point_counter_当前值
  ~Point();
  /// Add a reference to a frame.
  void addFrameRef(Feature* ftr);  //obs_.push_front(ftr), n_obs_++;
  /// Remove reference to a frame.
  bool deleteFrameRef(Frame* frame); //将frame对应的feature从obs_中除
  /// Initialize point normal. The inital estimate will point towards the frame.
  void initNormal();//初始化点的normal_和 normal_information
  /// Check whether mappoint has reference to a frame.
  Feature* findFrameRef(Frame* frame);//对obs_便利，如果obs_中的feature对应的frame是该传入的帧，则返回对应的feature
  /// Get Frame with similar viewpoint.
  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;  //pos为frame的空间位置。遍历obs_, 用于检测与当前帧同时观测到某3D点的夹角最小的关键帧，并将夹角最小的关键帧对应的feature传给obs。（在Fearue alignment中需要）。 当角度大于60度，认为无相近关键帧，返回false。
  /// Get number of observations.
  inline size_t nRefs() const { return obs_.size(); }
  /// Optimize point position through minimizing the reprojection error.
  void optimize(const size_t n_iter);//用point->obs_来进行优化。  
  /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
  inline static void jacobian_xyz2uv(
      const Vector3d& p_in_f,//相机坐标系下的3d点
      const Matrix3d& R_f_w,
      Matrix23d& point_jac)
  {
    const double z_inv = 1.0/p_in_f[2];
    const double z_inv_sq = z_inv*z_inv;
    point_jac(0, 0) = z_inv;
    point_jac(0, 1) = 0.0;
    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
    point_jac(1, 0) = 0.0;
    point_jac(1, 1) = z_inv;
    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
    point_jac = - point_jac * R_f_w;
  }
};

feature_detection.h

namespace svo {
/// Implementation of various feature detectors.
namespace feature_detection {
/// Temporary container used for corner detection. Features are initialized from these.
struct Corner
{
  int x;        //!< x-coordinate of corner in the image.
  int y;        //!< y-coordinate of corner in the image.
  int level;    //!< pyramid level of the corner.
  float score;  //!< shi-tomasi score of the corner  //gftt
  float angle;  //!< for gradient-features: dominant gradient 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> Corners;
/// All detectors should derive from this abstract class.
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;
  /// Flag the grid cell as occupied
  void setGridOccpuancy(const Vector2d& px);  //如果点px在某一个cell中，将该cell对应的grid_occupanvy_置为true
  /// Set grid cells of existing features as occupied
  void setExistingFeatures(const Features& fts);// 遍历fts,如果fts的像素坐标在某一个cell中，就将对应的grid_occupancy置为true。
protected:
  static const int border_ = 8; //!< no feature should be within 8px of border.
  //以下变量都通过AbstractDetector进行初始化
  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();  //将grid_occupancy_全置为false。
  inline int getCellIndex(int x, int y, int level)  //对不同层的点xy，确定其在第0层的cell的索引
    const int scale = (1<<level);
    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
  }
};
typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
/// FAST detector by Edward Rosten.
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);
};  //创建一个大小为网格数的corners并初始化。对每一层的图像都重新提取corner，将其放nm_corners中，遍历nm_corners，如果corner所在的位置对应的cell已经有特征点了，则跳过，否则，计算corner的分数，如果当前corner的分数大于corners【k】，则用当前的corner替换掉corners[k]中原来的数值。最终得到一个大小为网格数的corners。遍历corners，当其中的corner的分数大于设定的门限值时，将对应的特征点放在fts. 然后resetgrid。        
} // namespace feature_detection

sparse_img_align.h

namespace vk {
class AbstractCamera;
}
namespace svo {
class Feature;
/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
class SparseImgAlign : public vk::NLLSSolver<6, SE3>
{
  static const int patch_halfsize_ = 2; //匹配块的大小                 
  static const int patch_size_ = 2*patch_halfsize_;
  static const int patch_area_ = patch_size_*patch_size_;
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  cv::Mat resimg_;
  SparseImgAlign(
      int n_levels,
      int min_level,
      int n_iter,
      Method method,
      bool display,
      bool verbose); //初始化。其中n_iter,method,verbose用于初始化vk::NLLSSolver的相关成员变量。n_levels对应max_level。   
  size_t run(
      FramePtr ref_frame,
      FramePtr cur_frame); //初始化cache中的变量，   返回n_meas/patch_area.(得到的应该是观测到的fts的数量）                      
  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
  /// at the converged state.
  Matrix<double, 6, 6> getFisherInformation();
protected:
  FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
  FramePtr cur_frame_;            //!< only the image is known!
  int level_;                     //!< current pyramid level on which the optimization runs.
  bool display_;                  //!< display residual image.
  int max_level_;                 //!< coarsest pyramid level for the alignment                 
  int min_level_;                 //!< finest pyramid level for the alignment.
  // cache:                                 
  Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;   //列数为ref_patch_cache的大小。每一列为一个像素对应的jacobian。                  
  bool have_ref_patch_cache_; 
  cv::Mat ref_patch_cache_;  //行数为参考帧中的fts数，列数为patch_area的大小
  std::vector<bool> visible_fts_;//大小为参考帧的fts数
  void precomputeReferencePatches(); //计算ref中每个fts对应的patch的雅克比。将所有的雅克比存在jacobian_cahe_中。并将have_ref_patch_cache_置为true（在执行computeResiduals中，需要保证该步已经执行）    
  //以下的5个函数是对其父类NLSSLOVE对应成员函数，但是在NLSSOLVE中没有写具体的实现，是纯虚函数，在该类中对这些纯虚函数进行实现！！   ！！！！                          
  virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false);  //直接法的目标函数的构造。包括将ref中3D点转化到cur中，再投影到cur中，然后计算两帧中对应点的光度误差；计算H和b
  virtual int solve(); // 用ldlt来求解Hx=b;
  virtual void update (const ModelType& old_model, ModelType& new_model); //更新相对位姿 
  virtual void startIteration();
  virtual void finishIteration();
};

feature_alignment.h

namespace svo {
/// Subpixel refinement of a reference feature patch with the current image.
/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on"
/// paper by Baker.
namespace feature_alignment {
bool align1D(
    const cv::Mat& cur_img,
    const Vector2f& dir,                  // direction in which the patch is allowed to move
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate,
    double& h_inv);
bool align2D(
    const cv::Mat& cur_img,
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate,
    bool no_simd = false);
bool align2D_SSE2(
    const cv::Mat& cur_img,
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate);
bool align2D_NEON(
    const cv::Mat& cur_img,
    uint8_t* ref_patch_with_border,
    uint8_t* ref_patch,
    const int n_iter,
    Vector2d& cur_px_estimate);
} // namespace feature_alignment
} // namespace svo

pose_optimizer

namespace svo {
using namespace Eigen;
using namespace Sophus;
using namespace std;
typedef Matrix<double,6,6> Matrix6d;
typedef Matrix<double,2,6> Matrix26d;
typedef Matrix<double,6,1> Vector6d;
class Point;
/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
namespace pose_optimizer {                       
void optimizeGaussNewton(
    const double reproj_thresh,
    const size_t n_iter,
    const bool verbose,
    FramePtr& frame,
    double& estimated_scale,
    double& error_init,
    double& error_final,
    size_t& num_obs);  //用BA来优化位姿。迭代优化结束后，用优化后的位姿来计算重投影误差，误差大的点删除,得到最终观测到的点num_obs。记录esitmated_scale,error_init,error_final,num_obs.  
} // namespace pose_optimizer
} // namespace svo

matcher.h

namespace vk {
  class AbstractCamera;
  namespace patch_score {
    template<int HALF_PATCH_SIZE> class ZMSSD;
  }
}
namespace svo {
class Point;
class Frame;
class Feature;
/// Warp a patch from the reference view to the current view.
namespace warp {
void getWarpMatrixAffine(
    const vk::AbstractCamera& cam_ref,
    const vk::AbstractCamera& cam_cur,
    const Vector2d& px_ref,
    const Vector3d& f_ref,
    const double depth_ref,
    const SE3& T_cur_ref,
    const int level_ref,
    Matrix2d& A_cur_ref);    //计算放射矩阵A_cur_ref
int getBestSearchLevel(                                 
    const Matrix2d& A_cur_ref,
    const int max_level);   //{
  // Compute patch level in other image
  /*int search_level = 0;
  double D = A_cur_ref.determinant();
  while(D > 3.0 && search_level < max_level)
  {
    search_level += 1;
    D *= 0.25;
  }
  return search_level;
}  */    
void warpAffine(
    const Matrix2d& A_cur_ref,
    const cv::Mat& img_ref,
    const Vector2d& px_ref,
    const int level_ref,
    const int level_cur,
    const int halfpatch_size,
    uint8_t* patch);  //对参考帧像素对应的patch进行仿射变化， uint8_t *patch 依次指向仿射变化后像素对应的值. in practice, patch为Matcher的成员变量patch_with_border_
} // namespace warp             
/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
class Matcher
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static const int halfpatch_size_ = 4;
  static const int patch_size_ = 8;
  typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;  //用于块儿匹配相似性打分    
  struct Options
  {
    bool align_1d;              //!< in epipolar search: align patch 1D along epipolar line
    int align_max_iter;         //!< number of iterations for aligning the feature patches in gauss newton
    double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align 也就是说，当极线长度小于这个值时，不用进行极线搜索，直接进行img_align        
    size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
    bool subpix_refinement;     //!< do gauss newton feature patch alignment after epipolar search
    bool epi_search_edgelet_filtering;
    double epi_search_edgelet_max_angle;
    Options() :
      align_1d(false),
      align_max_iter(10),
      max_epi_length_optim(2.0),
      max_epi_search_steps(1000),
      subpix_refinement(true),
      epi_search_edgelet_filtering(true),
      epi_search_edgelet_max_angle(0.7)
    {}
  } options_;
  uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
  uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
  Matrix2d A_cur_ref_;          //!< affine warp matrix
  Vector2d epi_dir_;
  double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
  double h_inv_;                //!< hessian of 1d image alignment along epipolar line
  int search_level_;
  bool reject_;
  Feature* ref_ftr_;
  Vector2d px_cur_;
  Matcher() = default;
  ~Matcher() = default;
  /// Find a match by directly applying subpix refinement.
  /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
  bool findMatchDirect(
      const Point& pt,
      const Frame& frame,
      Vector2d& px_cur);                         
  /// Find a match by searching along the epipolar line without using any features.
  bool findEpipolarMatchDirect(
      const Frame& ref_frame,
      const Frame& cur_frame,
      const Feature& ref_ftr,
      const double d_estimate,
      const double d_min,
      const double d_max,
      double& depth);    //如果极线长度小于2，则px_cur为 （px_A+px_B)/2; otherwise,进行极线搜索，在极线上每间隔一段距离采样，然后计算该采样点对应的patch和ref的相似度，对相似度进行打分，选择分值最高的。 如果option.subpixle_refine=true, 则进行feature_align。   
  void createPatchFromPatchWithBorder();
};
} // namespace svo

depth_filter.h

namespace svo {
class Frame;                                                        
class Feature;
class Point;
/// A seed is a probabilistic depth estimate for a single pixel.
struct Seed
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static int batch_counter;
  static int seed_counter;
  int batch_id;                //!< Batch id is the id of the keyframe for which the seed was created.
  int id;                      //!< Seed ID, only used for visualization.
  Feature* ftr;                //!< Feature in the keyframe for which the depth should be computed.
  float a;                     //!< a of Beta distribution: When high, probability of inlier is large.
  float b;                     //!< b of Beta distribution: When high, probability of outlier is large.
  float mu;                    //!< Mean of normal distribution.
  float z_range;               //!< Max range of the possible depth.
  float sigma2;                //!< Variance of normal distribution.
  Matrix2d patch_cov;          //!< Patch covariance in reference image.
  Seed(Feature* ftr, float depth_mean, float depth_min);   //mu=1/depth_mean; z_range=1/depth_min;sigma2=(z_range^2)/36； 用逆深度进行计算。                       
};
/// Depth filter implements the Bayesian Update proposed in:
/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández.
/// In Image and Vision Computing, 29(7):434-441, 2011.
///
/// The class uses a callback mechanism such that it can be used also by other
/// algorithms than nslam and for simplified testing.
class DepthFilter
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef boost::unique_lock<boost::mutex> lock_t;
  typedef boost::function<void ( Point*, double )> callback_t;
  /// Depth-filter config parameters
  struct Options
  {
    bool check_ftr_angle;                       //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
    bool epi_search_1d;                         //!< restrict Gauss Newton in the epipolar search to the epipolar line.
    bool verbose;                               //!< display output.
    bool use_photometric_disparity_error;       //!< use photometric disparity error instead of 1px error in tau computation.
    int max_n_kfs;                              //!< maximum number of keyframes for which we maintain seeds.
    double sigma_i_sq;                          //!< image noise.
    double seed_convergence_sigma2_thresh;      //!< threshold on depth uncertainty for convergence.
    Options()
    : check_ftr_angle(false),
      epi_search_1d(false),
      verbose(false),
      use_photometric_disparity_error(false),
      max_n_kfs(3),
      sigma_i_sq(5e-4),
      seed_convergence_sigma2_thresh(200.0)
    {}
  } options_;
  DepthFilter(
      feature_detection::DetectorPtr feature_detector,
      callback_t seed_converged_cb);
  virtual ~DepthFilter();
  /// Start this thread when seed updating should be in a parallel thread.
  void startThread();
  /// Stop the parallel thread that is running.
  void stopThread();
  /// Add frame to the queue to be processed.
  void addFrame(FramePtr frame) //  updateseeds
  /// Add new keyframe to the queue
  void addKeyframe(FramePtr frame, double depth_mean, double depth_min); //initializeSeed
  /// Remove all seeds which are initialized from the specified keyframe. This
  /// function is used to make sure that no seeds points to a non-existent frame
  /// when a frame is removed from the map.
  void removeKeyframe(FramePtr frame);   //当要除去某一个关键帧时，除去seeds_中关键帧所包含的seeds。 
  /// If the map is reset, call this function such that we don't have pointers
  /// to old frames.
  void reset();   //将seeds_和frame_queue_都清空。 
  /// Returns a copy of the seeds belonging to frame. Thread-safe.
  /// Can be used to compute the Next-Best-View in parallel.
  /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
  /// so it is not being deleted while you use it.
  void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
  /// Return a reference to the seeds. This is NOT THREAD SAFE!
  std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }
  /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
  static void updateSeed(
      const float x,
      const float tau2,
      Seed* seed);      //this function is used in the function of updateSeeds. this fuction update mu and sigma2     
  /// Compute the uncertainty of the measurement.
  static double computeTau(
      const SE3& T_ref_cur,
      const Vector3d& f,
      const double z,
      const double px_error_angle)
protected:
  feature_detection::DetectorPtr feature_detector_;
  callback_t seed_converged_cb_;
  std::list<Seed, aligned_allocator<Seed> > seeds_;
  boost::mutex seeds_mut_;          //互斥量，用来实现线程同步        
  bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
  boost::thread* thread_;
  std::queue<FramePtr> frame_queue_;
  boost::mutex frame_queue_mut_;
  boost::condition_variable frame_queue_cond_;
  FramePtr new_keyframe_;               //!< Next keyframe to extract new seeds.
  bool new_keyframe_set_;               //!< Do we have a new keyframe to process?.
  double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
  double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
  vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
  Matcher matcher_;
  /// Initialize new seeds from a frame.
  void initializeSeeds(FramePtr frame);     //传入新来的关键帧frame，对frame进行特征点提取，并停止updateseeds。遍历提取的特征点fts，并用fts，new_keyframe_min_depth,new_keyframe_mean_depth来初始化新的seed，并加入到seeds_中 ，然后继续进行 updateseeds。  
  /// Update all seeds with a new measurement frame.
  virtual void updateSeeds(FramePtr frame);    //仅针对能在该帧（frame）中能看到的seeds进行updateSeed,并将深度值收敛的seed加入到地图点中，同时清除该seed。每传入一个frame，才进行一次深度滤波更新。 
  /// When a new keyframe arrives, the frame queue should be cleared.
  void clearFrameQueue();
  /// A thread that is continuously updating the seeds.
  void updateSeedsLoop();
};
} // namespace svo

map.h

namespace svo {
class Point;
class Feature;
class Seed;
/// Container for converged 3D points that are not already assigned to two keyframes.
class MapPointCandidates
{
public:
  typedef pair<Point*, Feature*> PointCandidate;
  typedef list<PointCandidate> PointCandidateList;
  /// The depth-filter is running in a parallel thread and fills the canidate list.
  /// This mutex controls concurrent access to point_candidates.
  boost::mutex mut_;
  /// Candidate points are created from converged seeds.
  /// Until the next keyframe, these points can be used for reprojection and pose optimization.
  PointCandidateList candidates_;
  list< Point* > trash_points_;
  MapPointCandidates();
  ~MapPointCandidates();
  /// Add a candidate point                  
  void newCandidatePoint(Point* point, double depth_sigma2);  //第二个参数没用。将point和对应的在latest关键帧中的feature加入candidates_.   
  /// Adds the feature to the frame and deletes candidate from list.
  void addCandidatePointToFrame(FramePtr frame);    //遍历candidates_,将frame对应的特征进行frame::addFeature(),同时将该特征点从candidates_中删除。 
  /// Remove a candidate point from the list of candidates.
  bool deleteCandidatePoint(Point* point); //遍历candidate_,找到point对应的PointCandidate，用deleteCandidate（）函数删除，同时从candidates_中删除。
  /// Remove all candidates that belong to a frame.
  void removeFrameCandidates(FramePtr frame);  //  和deleteCandidatePoint相似。
  /// Reset the candidate list, remove and delete all points.
  void reset(); //用于～MapPointCandidates();
  void deleteCandidate(PointCandidate& c);    //{delete c.second; c.second=NULL; c.first->type_ = Point::TYPE_DELETED; trash_points_.push_back(c.first);}
  void emptyTrash();  //清空trashpoint
};
/// Map object which saves all keyframes which are in a map.     
class Map : boost::noncopyable
{
public:
  list< FramePtr > keyframes_;          //!< List of keyframes in the map.
  list< Point* > trash_points_;         //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
  MapPointCandidates point_candidates_;
  Map();
  ~Map();
  /// Reset the map. Delete all keyframes and reset the frame and point counters.
  void reset();
  /// Delete a point in the map and remove all references in keyframes to it.
  void safeDeletePoint(Point* pt);  //用于removePtFrameRef中。 如果是关键点，将关键点删除，将pt->obs_清空，调用deletePoint删除该点 
  /// Moves the point to the trash queue which is cleaned now and then.
  void deletePoint(Point* pt);  //将pt的状态置为DELETE，并加入到trash_points_中。
  /// Moves the frame to the trash queue which is cleaned now and then.
  bool safeDeleteFrame(FramePtr frame);   //删除关键帧frame。具体来说，调用removePTFramRef,并将该frame从keyframes中erase。 调用point_candidates_.removeFrameCandidates(frame);  
  /// Remove the references between a point and a frame.
  void removePtFrameRef(Frame* frame, Feature* ftr);// 如果只有两帧观测到了该特征ftr，则进行safeDeletePoint。否测， 删除该关键帧frame所看到的该特征ftr， 如果该ftr对应keypoints，则将keypoints也删除。       
  /// Add a new keyframe to the map.                    
  void addKeyframe(FramePtr new_keyframe);   //push_back new_keyframe   
  /// Given a frame, return all keyframes which have an overlapping field of view.
  void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const; //该函数用于getClosestKeyframe中。 遍历keyframes，对每个keyframe中的keypoint测试是否能在当前帧frame中观测到，如果能，则在close_kfs中pushback该keyframe以及frame和keyframe间的距离。  
  /// Return the keyframe which is spatially closest and has overlapping field of view.
  FramePtr getClosestKeyframe(const FramePtr& frame) const;   对getCloseKeyframes中得到的close_kfs进行排序，选出最近的keyframe。
  /// Return the keyframe which is furthest apart from pos.
  FramePtr getFurthestKeyframe(const Vector3d& pos) const; //遍历keyframes，通过keyframes->pos(),计算与pos最远的关键帧。  
  bool getKeyframeById(const int id, FramePtr& frame) const;   //寻找与id相对应的keyframe
  /// Transform the whole map with rotation R, translation t and scale s.
  void transform(const Matrix3d& R, const Vector3d& t, const double& s）  //计算通过R，t，s变化后每个keyframe的T_f_w, 同时将每个keyframe中的fts所对应的3D点进行变换。              
  /// Empty trash bin of deleted keyframes and map points. We don't delete the
  /// points immediately to ensure proper cleanup and to provide the visualizer
  /// a list of objects which must be removed.
  void emptyTrash(); //清空trash_points_,并调用points_candidates_.emptytrash     
  /// Return the keyframe which was last inserted in the map.
  inline FramePtr lastKeyframe() { return keyframes_.back(); }
  /// Return the number of keyframes in the map
  inline size_t size() const { return keyframes_.size(); }              
};
/// A collection of debug functions to check the data consistency.
namespace map_debug {                
void mapStatistics(Map* map);   //计算每个关键帧平均观测到多少个点，每个点平均被多少个关键帧观测到。
void mapValidation(Map* map, int id);     //调用frameCalidation和pointValidation来验证帧和点是否存在。  
void frameValidation(Frame* frame, int id);
void pointValidation(Point* point, int id);
} // namespace map_debug
} // namespace svo

reprojector.h

namespace vk {
class AbstractCamera;                        
}
namespace svo {                                                       
class Map;
class Point;         
/// Project points from the map into the image and find the corresponding
/// feature (corner). We don't search a match for every point but only for one
/// point per cell. Thereby, we achieve a homogeneously distributed(均匀分布） set of
/// matched features and at the same time we can save processing time by not
/// projecting all points.
class Reprojector
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /// Reprojector config parameters
  struct Options {
    size_t max_n_kfs;   //!< max number of keyframes to reproject from
    bool find_match_direct;
    Options()
    : max_n_kfs(10),
      find_match_direct(true)
    {}
  } options_;
  size_t n_matches_;
  size_t n_trials_;
  Reprojector(vk::AbstractCamera* cam, Map& map);  //初始化map，调用initializeGrid              
  ~Reprojector();
  /// Project points from the map into the image. First finds keyframes with
  /// overlapping field of view and projects only those map-points.
  void reprojectMap(
      FramePtr frame,
      std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);//先用map_.getcloseKeyframes来获得与frame相近的关键帧，将它们按距离由小到大排序，得到最近的N个关键帧。将每个关键帧对应的fts_用reprojectPoint重投影到frame中。然后将map_中的candidates用reprojectPoint重投影到frame中。 最后，对于frame，每个cell中有许多投影点。用reprojectCell对每个cell选出一个点。  
private:
  /// A candidate is a point that projects into the image plane and for which we
  /// will search a maching feature in the image.
  struct Candidate {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Point* pt;       //!< 3D point.
    Vector2d px;     //!< projected 2D pixel location.
    Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
  };
  typedef std::list<Candidate, aligned_allocator<Candidate> > Cell;
  typedef std::vector<Cell*> CandidateGrid;
  /// The grid stores a set of candidate matches. For every grid cell we try to find one match.
  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大小，cells大小和数量，给cell_order赋值后打乱顺序          
  void resetGrid();
  bool reprojectCell(Cell& cell, FramePtr frame);  //对于某一个cell，可能会有很多points在其中。对这些points按照类型好坏进行排序，然后遍历这些points，对每个point进行matcher_.findMatchDirect来校准该point。 当有一个point通过了筛选，则将该point从cell中删除，同时return true。
  bool reprojectPoint(FramePtr frame, Point* point); //将point进行投影，得到px，如果px在frame中，则计算落在哪个cell中，然后grid_.cells.at(k)->push_back(Candidate(point, px));
};
} // namespace svo

bundle_adjustment.h

namespace g2o {
class EdgeProjectXYZ2UV;
class SparseOptimizer;
class VertexSE3Expmap;
class VertexSBAPointXYZ;
}
namespace svo {
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE3Expmap g2oFrameSE3;
typedef g2o::VertexSBAPointXYZ g2oPoint;
class Frame;
class Point;
class Feature;
class Map;
/// Local, global and 2-view bundle adjustment with g2o
namespace ba {
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3
{
  g2oEdgeSE3*     edge;
  Frame*          frame;
  Feature*        feature;
  bool            is_deleted;
  EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) :
    edge(e), frame(frame), feature(feature), is_deleted(false)
  {}
};
/// Optimize two camera frames and their observed 3D points.
/// Is used after initialization.
void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map);// 将两帧设为vertice,遍历第一帧中的点，将这些点设为vertice，然后将这些points对应的vertice和第一帧对应的vertice相连产生边。 对每一个point，用findFrameRef找到在第二帧中对应的feature.然后将这些点和第二帧相连产生边。 然后整体优化，并取出误差较大   
/// Local bundle adjustment.
/// Optimizes core_kfs and their observed map points while keeping the
/// neighbourhood fixed.
void localBA(
    Frame* center_kf,
    set<FramePtr>* core_kfs,
    Map* map,
    size_t& n_incorrect_edges_1,
    size_t& n_incorrect_edges_2,
    double& init_error,
    double& final_error);  //对core_kfs和core_kfs观测到的点进行优化。先对点进行structure only的优化，然后整体优化。     
/// Global bundle adjustment.
/// Optimizes the whole map. Is currently not used in SVO.
void globalBA(Map* map); //对map中所有的kyeframes和keyframes中的ftrs_进行优化。              
/// Initialize g2o with solver type, optimization strategy and camera model.
void setupG2o(g2o::SparseOptimizer * optimizer); //设置求解器和相机参数。  
/// Run the optimization on the provided graph.
void runSparseBAOptimizer(
    g2o::SparseOptimizer* optimizer,
    unsigned int num_iter,
    double& init_error,
    double& final_error);  //进行优化。
/// Create a g2o vertice from a keyframe object.
g2oFrameSE3* createG2oFrameSE3(
    Frame* kf,
    size_t id,
    bool fixed); 
/// Creates a g2o vertice from a mappoint object.
g2oPoint* createG2oPoint(
    Vector3d pos,
    size_t id,
    bool fixed);      
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2oEdgeSE3* createG2oEdgeSE3(
    g2oFrameSE3* v_kf,
    g2oPoint* v_mp,
    const Vector2d& f_up,
    bool robust_kernel,
    double huber_width,
    double weight = 1);
} // namespace ba
} // namespace svo                

Initialization.h

namespace svo {
class FrameHandlerMono;
/// Bootstrapping the map from the first two views.
namespace initialization {
enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
/// Tracks features using Lucas-Kanade tracker and then estimates a homography.
class KltHomographyInit {
  friend class svo::FrameHandlerMono;
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW                            
  FramePtr frame_ref_;
  KltHomographyInit() {};
  ~KltHomographyInit() {};
  InitResult addFirstFrame(FramePtr frame_ref); //调用detectFeatures,将检测到的特征存入px_ref_中。如果检测到的特征数小于100，则返回FAILURE，如果大于100，则将frame_ref存为frame_ref_,将px_ref_中的特征存入px_cur_;    
  InitResult addSecondFrame(FramePtr frame_ref);    //调用trackKlt对px_ref进行跟踪。跟踪数量小于一定值，返回FAILURE; disparity的均值小于一定值，说明两帧太近，返回NO_KEYFRAME。 调用computeHomography。如果inliers数量小于一定值，返回FAULURE。 以上筛选条件都通过后，用xyz_in_cur来计算在当前帧中的点的平均深度，用于求尺度因子，然后算出当前帧的位姿。对每一个inliers，计算它在世界坐标系下的3D点坐标，并将对应的feature加入到ref_frame和cur_frame中，同时将feature加入point->obs_中， 返回SUCCESS。
  void reset(); // px_cur_.clear(); frame_ref_.reset();       
protected:
  vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
  vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
  vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
  vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
  vector<double> disparities_;      //!< disparity between first and second frame.
  vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
  vector<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
  SE3 T_cur_from_ref_;              //!< computed transformation between the first two frames.
};
/// Detect Fast corners in the image.
void detectFeatures(
    FramePtr frame,
    vector<cv::Point2f>& px_vec,
    vector<Vector3d>& f_vec);    //对frame进行fastdetect，得到new_features,然后将其对应的px和f存入px_vec,f_vec                
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
    FramePtr frame_ref,
    FramePtr frame_cur,
    vector<cv::Point2f>& px_ref,
    vector<cv::Point2f>& px_cur,
    vector<Vector3d>& f_ref,
    vector<Vector3d>& f_cur,
    vector<double>& disparities); //用opencv自带函数进行KLT，对于追踪失败的点，会从px_cur,px_ref,f_ref中抹去，对于追踪成功的点，会存在px_cur,f_cur中，并计算disparities。
void computeHomography(
    const vector<Vector3d>& f_ref,
    const vector<Vector3d>& f_cur,
    double focal_length,
    double reprojection_threshold,
    vector<int>& inliers,
    vector<Vector3d>& xyz_in_cur,
    SE3& T_cur_from_ref);  //从f_ref,f_cur得到像素坐标，然后通过vk::Homography计算两帧的相对位姿T_c_f,并用vk::computeInliers计算inliers和xyz_in_cur.  
} // namespace initialization
} // namespace nslam

frame_handler_base.h

namespace vk
{
class AbstractCamera;
class PerformanceMonitor;
}
namespace svo
{
class Point;
class Matcher;
class DepthFilter;
/// Base class for various VO pipelines. Manages the map and the state machine.
class FrameHandlerBase : boost::noncopyable
{
public:
  enum Stage {
    STAGE_PAUSED,
    STAGE_FIRST_FRAME,
    STAGE_SECOND_FRAME,
    STAGE_DEFAULT_FRAME,
    STAGE_RELOCALIZING
  };
  enum TrackingQuality {
    TRACKING_INSUFFICIENT,
    TRACKING_BAD,
    TRACKING_GOOD
  };
  enum UpdateResult {
    RESULT_NO_KEYFRAME,
    RESULT_IS_KEYFRAME,
    RESULT_FAILURE
  };
  FrameHandlerBase(); //  stage_(STAGE_PAUSED),set_reset_(false),set_start_(false),acc_frame_timings_(10),acc_num_obs_(10),num_obs_last_(0),tracking_quality_(TRACKING_INSUFFICIENT). 如果定义了SVO_TRACE,对performerMonitor做一些初始化。   
  virtual ~FrameHandlerBase();
  /// Get the current map.
  const Map& map() const { return map_; }
  /// Will reset the map as soon as the current frame is finished processing.
  void reset() { set_reset_ = true; }
  /// Start processing.
  void start() { set_start_ = true; }
  /// Get the current stage of the algorithm.
  Stage stage() const { return stage_; }
  /// Get tracking quality.
  TrackingQuality trackingQuality() const { return tracking_quality_; }
  /// Get the processing time of the previous iteration.
  double lastProcessingTime() const { return timer_.getTime(); }
  /// Get the number of feature observations of the last frame.
  size_t lastNumObservations() const { return num_obs_last_; }
protected:
  Stage stage_;                 //!< Current stage of the algorithm.
  bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
  bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
  Map map_;                     //!< Map of keyframes created by the slam system.
  vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
  vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
  vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
  size_t num_obs_last_;                         //!< Number of observations in the previous frame.
  TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
  /// Before a frame is processed, this function is called.
  bool startFrameProcessingCommon(const double timestamp);
  /// When a frame is finished processing, this function is called.
  int finishFrameProcessingCommon(
      const size_t update_id,
      const UpdateResult dropout,
      const size_t num_observations);
  /// Reset the map and frame handler to start from scratch.
  void resetCommon();
  /// Reset the frame handler. Implement in derived class.
  virtual void resetAll() { resetCommon(); }
  /// Set the tracking quality based on the number of tracked features.
  virtual void setTrackingQuality(const size_t num_observations);
  /// Optimize some of the observed 3D points.
  virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
};
} // namespace nslam 

frame_handler_mono.h

namespace svo {
/// Monocular Visual Odometry Pipeline as described in the SVO paper.
class FrameHandlerMono : public FrameHandlerBase
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  FrameHandlerMono(vk::AbstractCamera* cam);
  virtual ~FrameHandlerMono();
  /// Provide an image.
  void addImage(const cv::Mat& img, double timestamp); //先清理core_kfs,overlap_kfs. 然后用传入的图片初始化new_frame. 然后通过系统所处的不同stage_进行不同处理，然后last_frame_=new_frame_;new_frame_.reset().    
  /// Set the first frame (used for synthetic datasets in benchmark node)
  void setFirstFrame(const FramePtr& first_frame);  // resetAll();last_frame_ = first_frame; last_frame_->setKeyframe();map_.addKeyframe(last_frame_);stage_ = STAGE_DEFAULT_FRAME;         
  /// Get the last frame that has been processed.
  FramePtr lastFrame() { return last_frame_; }
  /// Get the set of spatially closest keyframes of the last frame.
  const set<FramePtr>& coreKeyframes() { return core_kfs_; }
  /// Return the feature track to visualize the KLT tracking during initialization.
  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
  /// Access the depth filter.
  DepthFilter* depthFilter() const { return depth_filter_; }
  /// An external place recognition module may know where to relocalize.
  bool relocalizeFrameAtPose(
      const int keyframe_id,
      const SE3& T_kf_f,
      const cv::Mat& img,
      const double timestamp);      ///通过keyframe_id,用map_中的getKeyframeById得到挂ref_keyframe.通过img重新初始化一个new_frame_. 然后调用relocalizeFrame。只要返回结果不是REUSLT_FAILURE,说明重定位成功，将last_frame_=new_frame_, return true.
protected:
  vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
  Reprojector reprojector_;                     //!< Projects points from other keyframes into the current frame
  FramePtr new_frame_;                          //!< Current frame.
  FramePtr last_frame_;                         //!< Last frame, not necessarily a keyframe.
  set<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
  vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
  initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
  DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.        
  /// Initialize the visual odometry algorithm.
  virtual void initialize(); //初始化feature_detector，depth_filter_              
  /// Processes the first frame and sets it as a keyframe.
  virtual UpdateResult processFirstFrame(); //将第一帧的位姿设为单位阵作为参考。调用initialize中的addFirstFrame来提取特征点，如果成功，设置关键帧，stage_=STAGE_SECOND_FRAME        
  /// Processes all frames after the first frame until a keyframe is selected.
  virtual UpdateResult processSecondFrame();//调用initialize中的addSecondFrame来跟踪第一帧的特征点，并计算出特征点对应的3D坐标。将该帧new_frame_设为关键帧。通过frame_utils中的getSceneDepth来计算new_frame_中的场景深度值，并调用depth_filter和map_中的addKeyFrame，   
  /// Processes all frames after the first two keyframes.
  virtual UpdateResult processFrame(); //用上一帧的位姿初始化new_frame_的位姿。然后依次进行sparse_image_align,map rprojector&feature alignment,pose optimization, structure optimization.                                然后判断new_frame_是否是关键帧，如果不是，该帧用于depth_filter中updateSeeds,如果是，则设为关键帧，并判断删除旧的关键帧。      
  /// Try relocalizing the frame at relative position to provided keyframe.
  virtual UpdateResult relocalizeFrame                
      const SE3& T_cur_ref,
      FramePtr ref_keyframe);  //通过ref_keyframe来和new_frame_进行sparse_image_alignment      如果返回的追踪上的点的数量大于30，则将ref_keyframe设为last_frame, 进行processFrame(), 只要返回值不是RESULT_FAILURE,就说明重定位成功。 如果小于30，则重定位失败。
  /// Reset the frame handler. Implement in derived class.
  virtual void resetAll(); 
  /// Keyframe selection criterion.
  virtual bool needNewKf(double scene_depth_mean); //通过遍历overlap_kfs_来计算每个overlap_kf与new_frame的相对距离，如果这个距离大于一定的值，则返回true
  void setCoreKfs(size_t n_closest);//从overlap_kfs_中选出与new_frame_共视点最多的n_closest个关键帧作为core_kfs_ .                                   
};
} // namespace svo