ORB-SLAM2 ---- Initializer::Initialize函数

目录

1. 哪个函数调用此函数及此函数作用

2.该函数构造函数 

3.函数作用

---

1. 哪个函数调用此函数及此函数作用

        在Tracking::MonocularInitialization，即单目的初始化函数中进行调用，再调用该函数前我们已经将单目初始化的两帧（mInitialFrame，mCurrentFrame）匹配好了特征点，mvbPrevMatched本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标，mvIniMatches中保存了匹配好的特征点。

        我们调用Initializer::Initialize函数是为了通过H模型或F模型进行单目初始化，得到两帧间相对运动并初始MapPoints。

2.该函数构造函数 

bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector &vP3D, vector &vbTriangulated)

        该函数传入参数为：

        ①当前帧，也就是slam系统意义上的第二帧 CurrentFrame

        ②当前帧（2）和参考帧（1）图像中特征点的匹配关系：vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值，没有匹配关系的话，vMatches12[i]值为 -1。

        该函数传出参数为：

        ①R21：相机从参考帧到当前帧的旋转

        ②t21：相机从参考帧到当前帧的平移

        ③vP3D：三角化测量之后的三维地图点

        ④vbTriangulated：标记三角化点是否有效，有效为true

        ⑤return：该帧可以成功初始化，返回true；该帧不满足初始化条件，返回false

3.函数作用

bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector &vP3D, vector &vbTriangulated)
{
    // Reference Frame: 1, Current Frame: 2

    //获取当前帧的去畸变之后的特征点
    mvKeys2 = CurrentFrame.mvKeysUn;

	/*
	Match的数据结构是pair,mvMatches12只记录Reference到Current匹配上的特征点对  
	vector mvMatches12; 
	*/
    mvMatches12.clear();
	// 预分配空间，大小和关键点数目一致mvKeys2.size()
    mvMatches12.reserve(mvKeys2.size());

    // 记录参考帧1中的每个特征点是否有匹配的特征点
	/*
	记录Reference Frame的每个特征点在Current Frame是否有匹配的特征点 
	vector mvbMatched1;
	*/
    mvbMatched1.resize(mvKeys1.size());

    // Step 1 重新记录特征点对的匹配关系存储在mvMatches12，是否有匹配存储在mvbMatched1
    // 将vMatches12（有冗余） 转化为 mvMatches12（只记录了匹配关系）
    for(size_t i=0, iend=vMatches12.size();i=0)
        {
			//mvMatches12 中只记录有匹配关系的特征点对的索引值
            //i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
			//标记参考帧1中的这个特征点有匹配关系
            mvbMatched1[i]=true;
        }
        else
			//标记参考帧1中的这个特征点没有匹配关系
            mvbMatched1[i]=false;
    }

    // 有匹配的特征点的对数
    const int N = mvMatches12.size();
    // Indices for minimum set selection
    // 新建一个容器vAllIndices存储特征点索引，并预分配空间
    vector vAllIndices;
    vAllIndices.reserve(N);

	//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引
    vector vAvailableIndices;
	//初始化所有特征点对的索引，索引值0到N-1
    for(int i=0; i >(mMaxIterations,		//最大的RANSAC迭代次数
									  vector(8,0));	

	//用于进行随机数据样本采样，设置随机数种子
    DUtils::Random::SeedRandOnce(0);

	//开始每一次的迭代 
    for(int it=0; it vbMatchesInliersH, vbMatchesInliersF;
	//计算出来的单应矩阵和基础矩阵的RANSAC评分，这里其实是采用重投影误差来计算的
    float SH, SF; //score for H and F
    //这两个是经过RANSAC算法后计算出来的单应矩阵和基础矩阵
    cv::Mat H, F; 

    // 构造线程来计算H矩阵及其得分
    // thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
    thread threadH(&Initializer::FindHomography,	//该线程的主函数
				   this,							//由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针
				   ref(vbMatchesInliersH), 			//输出，特征点对的Inlier标记
				   ref(SH), 						//输出，计算的单应矩阵的RANSAC评分
				   ref(H));							//输出，计算的单应矩阵结果
    // 计算fundamental matrix并打分，参数定义和H是一样的，这里不再赘述
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
    // Wait until both threads have finished
	//等待两个计算线程结束
    threadH.join();
    threadF.join();

    // Compute ratio of scores
    // Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
	//通过这个规则来判断谁的评分占比更多一些，注意不是简单的比较绝对评分大小，而是看评分的占比
    float RH = SH/(SH+SF);			//RH=Ratio of Homography

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动
    if(RH>0.40)
		//更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果
        return ReconstructH(vbMatchesInliersH,	//输入，匹配成功的特征点对Inliers标记
							H,					//输入，前面RANSAC计算后的单应矩阵
							mK,					//输入，相机的内参数矩阵
							R21,t21,			//输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
							vP3D,				//特征点对经过三角测量之后的空间坐标，也就是地图点
							vbTriangulated,		//特征点对是否成功三角化的标记
							1.0,				//这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时
												//需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
							50);				//为了进行运动恢复，所需要的最少的三角化测量成功的点个数
    else //if(pF_HF>0.6)
        // 更偏向于非平面，从基础矩阵恢复
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

	//一般地程序不应该执行到这里，如果执行到这里说明程序跑飞了
    return false;
}

        我们用临时变量mvKeys2获取当前帧的去畸变之后的特征点。

    typedef pair Match;
    vector mvMatches12;

        我们用mvMatches12变量存放单目初始化第一帧到第二帧匹配的序对信息

vector mvbMatched1;  //初始化其容器大小为第一帧的特征点数量

        我们用mvbMatched1记录Reference Frame的每个特征点在Current Frame是否有匹配的特征点。

        我们遍历vMatches12向量，这里面存放着特征点的匹配信息。

        若第一帧和第二帧没有匹配关系，则vMatches12[i] = false，我们将mvbMatched1[i]设为false位，标记参考帧1中的这个特征点没有匹配关系。

        若第一帧和第二帧有匹配关系，则vMatches12[i] = true，我们将mvbMatched1[i]设为true位，标记参考帧1中的这个特征点有匹配关系，并创建对组给向量mvMatches12，记录匹配关系。

        我们得到了N个匹配关系记录在对组mvMatches12。新建一个容器vAllIndices存储特征点索引，并预分配空间N。初始化vAllIndices容器，索引值0到N-1。

mvSets = vector< vector >(mMaxIterations,		//最大的RANSAC迭代次数
									  vector(8,0));	

        mvsets初始化为一个200 * 8 的矩阵，用来在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵。

        我们进行mMaxIterations（200）次迭代，每次取出八对特征点对（帧1--帧2）存到mvSets容器中。

        计算H矩阵和F矩阵并计算出其评分，根据阈值选择用哪个矩阵恢复两帧间位姿。

        最后用H或者F矩阵恢复位姿，得到R，t以及三角化后的地图点。