详谈ORB-SLAM2的单目初始化器Initializer

单目初始化器Initializer类，这个类只用于单目初始化，因为这是ORB-SLAM里遗留的一个类，也是祖传代码，双目和RGBD相机只需要一帧就能初始化，因为双目和RGBD相机拍到的点都是有信息的，但是单目相机就不一定了，单目相机必须至少有两副图像才能初始化，对单目初始化器来说有两个帧，一个是参考帧，另一个是当前帧，这是连续的两帧。

一、各成员变量/函数

成员变量名中：1代表参考帧(reference frame)中特征点编号,2代表当前帧(current frame)中特征点编号

各成员函数/变量	访问控制	意义
vectorcv::KeyPoint mvKeys1	private	参考帧(reference frame)中的特征点
vectorcv::KeyPoint mvKeys2	private	当前帧(current frame)中的特征点
vector<pair<int,int>> mvMatches12	private	从参考帧到当前帧的匹配特征点对
vector mvbMatched1	private	参考帧特征点是否在当前帧存在匹配特征点
cv::Mat mK	private	相机内参
float mSigma, mSigma2	private	重投影误差阈值及其平方
int mMaxIterations	private	RANSAC迭代次数
vector<vector<size_t>> mvSets	private	二维容器N✖8 每一层保存RANSAC计算H和F矩阵所需的八对点

二、初始化函数: Initialize()

在参考帧创建Initializer对象，并且把创建参考的帧，在当前帧就是参考帧临近的下一帧，在这个帧里调用Initialize这个函数，就是把当前两个帧之间进行特征点匹配，然后计算f矩阵和h矩阵。单目相机初始化有两种方法：单应矩阵和基础矩阵。

首先设置RANSAC算法用到的点对，单目相机初始化有两种途径，而实际上在初始化成功之前那个矩阵演算出来的结果是准确的，所以使用暴力算法，把两个矩阵都解算出来，对两个矩阵根据冲突点误差计算得分，哪个矩阵的冲突点误差较小（得分比较高），就是用其恢复运动，如果恢复的特征点数目足够多视差角足够大，就算初始化成功（实际上如果恢复100个特征点视差角在比较可靠的情况下认为是恢复成功的）。

bool Initializer::Initialize(const Frame &CurrentFrame,
                             const vector<int> &vMatches12,
                             cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D,
                             vector<bool> &vbTriangulated) {

    // 初始化器Initializer对象创建时就已指定mvKeys1,调用本函数只需指定mvKeys2即可
    mvKeys2 = CurrentFrame.mvKeysUn;		// current frame中的特征点
    mvMatches12.reserve(mvKeys2.size());
    mvbMatched1.resize(mvKeys1.size());

    // step1. 将vMatches12拷贝到mvMatches12,mvMatches12只保存匹配上的特征点对
    for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {
        if (vMatches12[i] >= 0) {
            mvMatches12.push_back(make_pair(i, vMatches12[i]));
            mvbMatched1[i] = true;
        } else
            mvbMatched1[i] = false;
    }

	// step2. 准备RANSAC运算中需要的特征点对
    const int N = mvMatches12.size();
    vector<size_t> vAllIndices;
    for (int i = 0; i < N; i++) {
        vAllIndices.push_back(i);
    }
    mvSets = vector<vector<size_t> >(mMaxIterations, vector<size_t>(8, 0));
    for (int it = 0; it < mMaxIterations; it++) {
    	vector<size_t> vAvailableIndices = vAllIndices;
        for (size_t j = 0; j < 8; j++) {
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);
            int idx = vAvailableIndices[randi];
            mvSets[it][j] = idx;
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

	// step3. 计算F矩阵和H矩阵及其置信程度
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;

    thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));
    threadH.join();
    threadF.join();

	// step4. 根据比分计算使用哪个矩阵恢复运动
    float RH = SH / (SH + SF); 
    if (RH > 0.40)
        return ReconstructH(vbMatchesInliersH, H, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);
    else
        return ReconstructF(vbMatchesInliersF, F, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);
}

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》