特征点法构建视觉里程计（代码实践）

#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:
    //定义指向自身的智能指针，在后面传递参数是使用VisualOdometry::Ptr即可
    typedef shared_ptr<VisualOdometry> Ptr;
    //定义枚举来表征VO状态，分别为：初始化、正常、丢失
    enum VOState {
   
        INITIALIZING=-1,
        OK=0,
        LOST
    };
    //这里为两两帧VO所用到的参考帧和当前帧。还有VO状态和整个地图。
    VOState     state_;     // current VO status
    Map::Ptr    map_;       // map with all frames and map points
    
    Frame::Ptr  ref_;       // reference key-frame 
    Frame::Ptr  curr_;      // current frame 
    //这里是两帧匹配需要的：keypoints，descriptors，matches,相比0.2去掉了关于参考帧的东西，3D点，描述子等
    cv::Ptr<cv::ORB> orb_;  // orb detector and computer 
    vector<cv::KeyPoint>    keypoints_curr_;    // keypoints in current frame
    Mat                     descriptors_curr_;  // descriptor in current frame 
    //在匹配器中，所需要的匹配变成了地图点和帧中的关键点
    cv::FlannBasedMatcher   matcher_flann_;     // flann matcher
    vector<MapPoint::Ptr>   match_3dpts_;       // matched 3d points 
    vector<int>             match_2dkp_index_;  // matched 2d pixels (index of kp_curr)
    //这里为匹配结果T，还有表征结果好坏的内点数和丢失数
    SE3 T_c_w_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
    double  map_point_erase_ratio_; // remove map point ratio
    
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 optimizeMap();
    //这里是关键帧的一些功能函数
    //增加地图点函数
    void addKeyFrame();
    void addMapPoints();
    bool checkEstimatedPose(); 
    bool checkKeyFrame();
    //增加的取得视角函数
    double getViewAngle( Frame::Ptr frame, MapPoint::Ptr point );
    
};
}
 
#endif // VISUALODOMETRY_H

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>
 
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/g2o_types.h"
 
namespace myslam
{
   
//默认构造函数，提供默认值、读取配置参数
VisualOdometry::VisualOdometry() :
    state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 ), matcher_flann_ ( new cv::flann::LshIndexParams ( 5,10,2 ) )
{
   
    num_of_features_    = Config::get<int> ( "number_of_features" );
    scale_factor_       = Config::get<double> ( "scale_factor" );
    level_pyramid_      = Config::get<int> ( "level_pyramid" );
    match_ratio_        = Config::get<float> ( "match_ratio" );
    max_num_lost_       = Config::get<float> ( "max_num_lost" );
    min_inliers_        = Config::get<int> ( "min_inliers" );
    key_frame_min_rot   = Config::get<double> ( "keyframe_rotation" );
    key_frame_min_trans = Config::get<double> ( "keyframe_translation" );
    map_point_erase_ratio_ = Config::get<double> ( "map_point_erase_ratio" )