设计前端（二）

1.特征提取和匹配

前面我们所说的，仅凭俩帧的估计是不够的。我们会将特征点缓存成一个小地图，计算当前帧和地图之间的位置关系。

忽略掉很多关键点，只关心当前帧与上一时刻帧(即参考帧)，即我们只考虑如下图所示的运动：

以传统的匹配特征点(ORB-slam)——求PnP的方法（接下来可以使用光流法/直接法——ICP求解ICP），流程如下图：

1.对新来的当前帧，提取关键点和描述子。
2.如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的位置(为什么要深度图?其他不可以吗)，返回第一步。（该步是防止没有参考帧）
3.估计参考帧与当前帧间的运动。
4.判断上述估计是否成功。
5.若成功，把当前帧作为新的参考帧，返回第一步。
6.若失败，记录连续丢失帧数。当丢失超过一定帧数时，置VO状态为丢失，算法结束。若未超过，返回第一步。

visual_odometry.h

#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H

#include "myslam/common_include.h"
#include "myslam/map.h"

#include <opencv2/features2d/features2d.hpp>

namespace myslam 
{
class VisualOdometry
{
public:
    typedef shared_ptr<VisualOdometry> Ptr;//定义了一个智能指针
    enum VOState {       //枚举,本身有若干种状态：设定第一帧、顺利跟踪或丢失，可以看做一个有限状态机，当然状态也可以有多种，考虑最简单的三种状态：初始化、正常、丢失
        INITIALIZING=-1,
        OK=0,
        LOST
    };

    VOState     state_;     // current VO status
    Map::Ptr    map_;       // map with all frames and map points
    Frame::Ptr  ref_;       // reference frame 
    Frame::Ptr  curr_;      // current frame 

    cv::Ptr<cv::ORB> orb_;  // orb detector and computer 
    vector<cv::Point3f>     pts_3d_ref_;        // 3d points in reference frame 
    vector<cv::KeyPoint>    keypoints_curr_;    // keypoints in current frame
    Mat                     descriptors_curr_;  // descriptor in current frame 
    Mat                     descriptors_ref_;   // descriptor in reference frame 
    vector<cv::DMatch>      feature_matches_;

    SE3 T_c_r_estimated_;  // the estimated pose of current frame 
    int num_inliers_;        // number of inlier features in icp
    int num_lost_;           // number of lost times

    // parameters 
    int num_of_features_;   // number of features
    double scale_factor_;   // scale in image pyramid
    int level_pyramid_;     // number of pyramid levels  金字塔
    float match_ratio_;      // ratio for selecting  good matches   比率
    int max_num_lost_;      // max number of continuous lost times
    int min_inliers_;       // minimum inliers

    double key_frame_min_rot;   // minimal rotation of two key-frames
    double key_frame_min_trans; // minimal translation of two key-frames

public: // functions 
    VisualOdometry();
    ~VisualOdometry();

    bool addFrame( Frame::Ptr frame );      // add a new frame 

protected:  
    // inner operation 
    void extractKeyPoints();
    void computeDescriptors(); 
    void featureMatching();
    void poseEstimationPnP(); 
    void setRef3DPoints();

    void addKeyFrame();
    bool checkEstimatedPose(); 
    bool checkKeyFrame();

};
}

#endif // VISUALODOMETRY_H