VO的初始化

利用LK光流和本質矩陣恢復的初始化：
待更

#include <opencv2/features2d.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include<iostream>
#include<vector>

using namespace std;

class Initialization
{
private:
    bool is_first_frame_;
    cv::Mat frame_ref_;
    cv::Mat frame_cur_;
    vector<cv::Point2f> p2d_ref_;
    vector<cv::Point2f> p2d_cur_;
    
public:
    Initialization():is_first_frame_(true){};
    ~Initialization(){};
    void addFrame(cv::Mat& frame);
    void detectFeatures(cv::Mat& frame,vector<cv::Point2f>& p2d);
    void trackFeatures(cv::Mat& frame_ref, cv::Mat& frame_cur,
        vector<cv::Point2f>& p2d_ref, vector<cv::Point2f>& p2d_cur);
    void recoverRT(vector<cv::Point2f>& p2d_ref, vector<cv::Point2f>& 
        p2d_cur,cv::Mat& cameraMatrix);
};


void Initialization::addFrame(cv::Mat& frame)
{
    if(is_first_frame_){
        frame_ref_ = frame;
        detectFeatures(frame_ref_,p2d_ref_);
        is_first_frame_ = false;
    }
    frame_cur_ = frame;
    detectFeatures(frame_cur_,p2d_cur_);
    return;
}

void Initialization::detectFeatures(cv::Mat& frame,vector<cv::Point2f>& p2d)
{
    auto detector = cv::FastFeatureDetector::create(10,true);
    vector<cv::KeyPoint> kpts;
    detector->detect(frame,kpts);
    p2d.resize(kpts.size());
    for(auto kpt:kpts) p2d.emplace_back(kpt.pt);
    return;
}


void Initialization::trackFeatures(cv::Mat& frame_ref, cv::Mat& frame_cur,
    vector<cv::Point2f>& p2d_ref, vector<cv::Point2f>& p2d_cur)
{
    const int win_size = 10;
    vector<uchar> features_found;
    cv::calcOpticalFlowPyrLK(frame_ref,frame_cur,p2d_ref,p2d_cur,features_found,
        cv::noArray(), cv::Size(win_size*2+1,win_size*2+1),3,cv::TermCriteria(
            cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,20,0.3));
    auto p2d_ref_it = p2d_ref.begin();
    auto p2d_cur_it = p2d_cur.begin();
    for(int i=0;i<features_found.size();i++,p2d_ref_it++,p2d_cur_it++){
        if(!features_found[i]){
            p2d_ref.erase(p2d_ref_it);
            p2d_cur.erase(p2d_cur_it);
        }
    }
    return;
}

void Initialization::recoverRT(vector<cv::Point2f>& p2d_ref, vector<cv::Point2f>& 
    p2d_cur,cv::Mat& cameraMatrix)
{
    cv::Mat E, R, t;
    E = findEssentialMat(p2d_ref, p2d_cur, cameraMatrix, cv::RANSAC, 0.999, 1.0, cv::noArray());
    recoverPose(E, p2d_ref, p2d_cur, cameraMatrix, R, t, cv::noArray());
    cout<<"R: "<<R<<endl;
    cout<<"t: "<<t<<endl;
}