ORB-SLAM2源码笔记（5）——地图初始化

Initializer类仅用于单目相机初始化,双目/RGBD相机初始化不用这个类

因为相比于双目相机和RGBD相机，单目相机无法在第一帧就获得地图点的深度，想要初始化,需要像双目相机那样去进行立体匹配。

ORB-SLAM2代码详解06: 单目初始化器Initializer_ncepu_Chen的博客-CSDN博客

 单目初始化函数Initialize（）

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);
}

RANdom SAmple Consensus（随机采样一致），从一组包含“局外点”的观测数据集中，通过迭代方式估计参数。

方法：

1. 假设一个适用于局内点的模型，用一组随机子集数据计算未知参数

2. 用1中的模型测试所有其他数据，如果在阈值内就认为是局内点，将局内点添加到子集中

3. 如果子集内有足够多的局内点，则说明模型是合理的

4. 将子集里面的所有数据重新估计模型参数

5. 使用局内点与模型的错误率评估模型

RANSAC算法讲解_fandq1223的博客-CSDN博客_ransac算法迭代次数

 先对特征点进行归一化，再使用RANSAC算法结合八点法计算基础矩阵，最后卡方检验

使用归一化可以提高运算精度、消除了坐标系变换对结果的影响

卡方检验是通过构造统计量比较期望与实际结果之间的差别，如果重投影误差构造统计量
越大,观察结果和期望结果之间的差别越显著,某次计算越可能用到了外点

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21) {

    const int N = vbMatchesInliers.size();

    // step1. 特征点归一化
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1, vPn1, T1);
    Normalize(mvKeys2, vPn2, T2);
    cv::Mat T2t = T2.t();		// 用于恢复原始尺度

    // step2. RANSAC循环
    score = 0.0;									// 最优解得分
    vbMatchesInliers = vector<bool>(N, false);		// 最优解对应的内点
    for (int it = 0; it < mMaxIterations; it++) {
        vector<cv::Point2f> vPn1i(8);
        vector<cv::Point2f> vPn2i(8);
        cv::Mat F21i;
		vector<bool> vbCurrentInliers(N, false);
    	float currentScore;

        for (int j = 0; j < 8; j++) {
            int idx = mvSets[it][j];
            vPn1i[j] = vPn1[mvMatches12[idx].first];        // first存储在参考帧1中的特征点索引
            vPn2i[j] = vPn2[mvMatches12[idx].second];       // second存储在当前帧2中的特征点索引
        }

        // step3. 八点法计算单应矩阵H
       	cv::Mat Fn = ComputeF21(vPn1i, vPn2i);

        // step4. 恢复原始尺度
        F21i = T2t * Fn * T1;

        // step5. 根据重投影误差进行卡方检验
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

        // step6. 记录最优解
        if (currentScore > score) {
            F21 = F21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

使用基础矩阵F分解R、t，再通过三角化检验R、t，checkRT（）