基本流程

单目初始化程序存储在Initializer.cc中
  需要注意，对于双目/RGB-D相机，初始化时，由于可以直接获得相机的深度信息，因此无需求H/F，直接作为关键帧插入就行。
  使用RANSAC+DLT求解H，RANSAC+八点法求解F。选择评分高的，然后进行分解获得R，t。分解时会有多组解，选择最多深度为正的路标点的解。

构建初始化器

nitializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)

特征点坐标归一化处理

前辈发现计算单应矩阵时变换特征点的坐标会得到更好的效果，包括坐标的平移和尺度缩放，并且这一步骤必须放在DLT之前。DLT之后再还原到原坐标系。

（1）将点进行平移使其形心（x,y的均值）位于原点。
（2）对点进行缩放使特征点到原点的距离为根号2，即所有点“平均”位于（1，1，1）
（3）对两幅图进行独立的上述变换

 

计算单应矩阵

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &vP2) //归一化后的点, in current frame

有关SVD分解过程可以参考
https://blog.csdn.net/sinat_28309919/article/details/80134985
  奇异值分解后，V的最后一列向量即为解。

    // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置cv::Mat u,w,vt;//使用opencv提供的进行奇异值分解的函数cv::SVDecomp(A,							//输入，待进行奇异值分解的矩阵w,							//输出，奇异值矩阵u,							//输出，矩阵Uvt,						//输出，矩阵V^Tcv::SVD::MODIFY_A | 		//输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存cv::SVD::FULL_UV);		//FULL_UV=把U和VT补充成单位正交方阵// 返回最小奇异值所对应的右奇异向量// 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8return vt.row(8).reshape(0, 			//转换后的通道数，这里设置为0表示是与前面相同3); 			//转换后的行数,对应V的最后一列

计算H的评分

使用对称转移误差为评判标准。据此来判断特征点是内点还是外点。

        currentScore = CheckHomography(H21i, H12i, 			//输入，单应矩阵的计算结果vbCurrentInliers, 	//输出，特征点对的Inliers标记mSigma);	

通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差

（1）计算重投影误差：

        const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;

（2）判断是内点还是外点
  误差小于阈值的话，为内点。内点的评分与误差的大小有关，误差越大，评分越小。

        // Step 2.3 用阈值标记离群点，内点的话累加得分if(chiSquare1>th)bIn = false;    else// th为设定的阈值，误差越大，得分越低score += th - chiSquare1;

设定内点：

        // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier pointif(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;

寻找单应矩阵

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)

对特征点坐标进行归一化处理

将当前帧和参考帧中的特征点坐标进行归一化。

	//归一化后的参考帧1和当前帧2中的特征点坐标vector<cv::Point2f> vPn1, vPn2;// 记录各自的归一化矩阵cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);

取出RANSAC获得的随机特征点对的索引

        for(size_t j=0; j<8; j++){//从mvSets中获取当前次迭代的某个特征点对的索引信息int idx = mvSets[it][j];// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标vPn1i[j] = vPn1[mvMatches12[idx].first];    //first存储在参考帧1中的特征点索引vPn2i[j] = vPn2[mvMatches12[idx].second];   //second存储在参考帧1中的特征点索引}//读取8对特征点的归一化之后的坐标

计算单应矩阵

&emps;需要注意，这里计算的单应矩阵使用的是归一化坐标。然后要恢复到原始坐标下。

        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 单应矩阵原理：X2=H21*X1，其中X1,X2 为归一化后的特征点    // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  得到:T2 * mvKeys2 =  Hn * T1 * mvKeys1   // 进一步得到:mvKeys2  = （T2.inv * Hn * T1） * mvKeys1H21i = T2inv*Hn*T1;//然后计算逆H12i = H21i.inv();

计算该次迭代获得的H的评分

        // Step 4 利用重投影误差为当次RANSAC的结果评分currentScore = CheckHomography(H21i, H12i, 			//输入，单应矩阵的计算结果vbCurrentInliers, 	//输出，特征点对的Inliers标记mSigma);	

保存最优的H

        // Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记if(currentScore>score){//如果当前的结果得分更高，那么就更新最优计算结果H21 = H21i.clone();//保存匹配好的特征点对的Inliers标记vbMatchesInliers = vbCurrentInliers;//更新历史最优评分score = currentScore;}

计算基础矩阵

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //归一化后的点, in reference frameconst vector<cv::Point2f> &vP2) //归一化后的点, in current frame

SVD分解，D的最后一列即为F的值

    //存储奇异值分解结果的变量cv::Mat u,w,vt;// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 转换成基础矩阵的形式cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2// 对初步得来的基础矩阵进行第2次奇异值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2约束，强制将第3个奇异值设置为0w.at<float>(2)=0;// 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 return  u*cv::Mat::diag(w)*vt;

计算F的评分

误差为像素点到极线的距离。

通过点到极线的距离计算误差

误差小于阈值的话，设为内点。误差越小，贡献越大。

        // Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;// Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num2 = a2*u2+b2*v2+c2;const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 带权重误差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 误差大于阈值就说明这个点是Outlier // ? 为什么判断阈值用的 th（1自由度），计算得分用的thScore（2自由度）// ? 可能是为了和CheckHomography 得分统一？if(chiSquare1>th)bIn = false;else// 误差越大，得分越低score += thScore - chiSquare1;

寻找基础矩阵

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

取出RANSAC获得的特征点索引

        for(int j=0; j<8; j++){int idx = mvSets[it][j];// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存储在参考帧1中的特征点索引vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存储在参考帧1中的特征点索引}

使用八点法计算基础矩阵F

        // Step 3 八点法计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标    // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  // 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   // 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0F21i = T2t*Fn*T1;

获得该基础矩阵的评分

        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

更新最优的基础矩阵

        if(currentScore>score){//如果当前的结果得分更高，那么就更新最优计算结果F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}

初始化主函数

Tracking.cc的947行
（1）匹配特征点
（2）获得基础矩阵与单应矩阵
（3）选择最佳的来恢复两帧间的位姿变换。
（4）三角化获得路标点

/*** @param[in] CurrentFrame          当前帧，也就是SLAM意义上的第二帧* @param[in] vMatches12            当前帧（2）和参考帧（1）图像中特征点的匹配关系*                                  vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值*                                  没有匹配关系的话，vMatches12[i]值为 -1* @param[in & out] R21                   相机从参考帧到当前帧的旋转* @param[in & out] t21                   相机从参考帧到当前帧的平移* @param[in & out] vP3D                  三角化测量之后的三维地图点* @param[in & out] vbTriangulated        标记三角化点是否有效，有效为true* @return true                     该帧可以成功初始化，返回true* @return false                    该帧不满足初始化条件，返回false*/bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)

获得两帧间的特征点匹配关系

    for(size_t i=0, iend=vMatches12.size();i<iend; i++){//vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值//没有匹配关系的话，vMatches12[i]值为 -1if(vMatches12[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;}

在所有匹配的特征点中随机取出8组点，求解单应矩阵与基础矩阵

（1）获得RANSAC算法中，每次进行计算的8组点的索引
mvSets：迭代次数索引—>每次迭代时特征点对的索引

    for(int it=0; it<mMaxIterations; it++){//迭代开始的时候，所有的点都是可用的vAvailableIndices = vAllIndices;// Select a minimum set//选择最小的数据样本集，使用八点法求，所以这里就循环了八次for(size_t j=0; j<8; j++){// 随机产生一对点的id,范围从0到N-1int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// idx表示哪一个索引对应的特征点对被选中int idx = vAvailableIndices[randi];//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中mvSets[it][j] = idx;// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出8个特征点对}//迭代mMaxIterations次，选取各自迭代时需要用到的最小数据集

（2）开两个线程，分别计算基础矩阵和单应矩阵

    // 构造线程来计算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();

根据评分选择F/ H，恢复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);

从F中恢复R,t

根据F矩阵获得E矩阵

cv::Mat E21 = K.t()*F21*K;

分解E矩阵，获得4组解

    DecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;

选择最佳的R，t解

若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合。**E矩阵分解有4中可能，H矩阵分解有8种可能。**在这个过程中，会进行三角化。

（1）选出最合适的R，t解（内点在相机的前面，有足够的视角）
（2）4组解中，如果最优的R，t解不够突出，就放弃。

    int nGood1 = CheckRT(R1,t1,							//当前组解mvKeys1,mvKeys2,				//参考帧和当前帧中的特征点mvMatches12, vbMatchesInliers,	//特征点的匹配关系和Inliers标记K, 							//相机的内参数矩阵vP3D1,							//存储三角化以后特征点的空间坐标4.0*mSigma2,					//三角化测量过程中允许的最大重投影误差vbTriangulated1,				//参考帧中被成功进行三角化测量的特征点的标记parallax1);					//认为某对特征点三角化测量有效的比较大的视差角int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);

保存分解结果

包括保存最佳R，t、三角化后的点的坐标等。

三角化

void Initializer::Triangulate(const cv::KeyPoint &kp1,    //特征点, in reference frameconst cv::KeyPoint &kp2,    //特征点, in current frameconst cv::Mat &P1,          //投影矩阵P1（K*T）const cv::Mat &P2,          //投影矩阵P2cv::Mat &x3D)               //三维点

进行SVD分解，D的列向量即为AX=0的解X。随后对X进行归一化处理。

	//构造参数矩阵AA.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);//奇异值分解的结果cv::Mat u,w,vt;//对系数矩阵A进行奇异值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定//别忘了我们更习惯用列向量来表示一个点的空间坐标x3D = vt.row(3).t();//为了符合其次坐标的形式，使最后一维为1x3D = x3D.rowRange(0,3)/x3D.at<float>(3);

三角化

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)

遍历特征点对，进行三角化

    for(size_t i=0, iend=vMatches12.size();i<iend;i++){// 跳过outliersif(!vbMatchesInliers[i])continue;// Step 2 获取特征点对，调用Triangulate() 函数进行三角化，得到三角化测量之后的3D点坐标// kp1和kp2是匹配好的有效特征点const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];//存储三维点的的坐标cv::Mat p3dC1;// 利用三角法恢复三维点p3dC1Triangulate(kp1,kp2,	//特征点P1,P2,		//投影矩阵p3dC1);		//输出，三角化测量之后特征点的空间坐标	.....................}

检查三角化后的路标点

（1）三角化后的路标点的X，Y，Z不能无限大。

        if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){//其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点vbGood[vMatches12[i].first]=false;//继续对下一对匹配点的处理continue;}

（2）Z大于0，且视差不能太小

        //得到向量PO1cv::Mat normal1 = p3dC1 - O1;//求取模长，其实就是距离float dist1 = cv::norm(normal1);//同理构造向量PO2cv::Mat normal2 = p3dC1 - O2;//求模长float dist2 = cv::norm(normal2);//根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子float cosParallax = normal1.dot(normal2)/(dist1*dist2);// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 如果深度值为负值，为非法三维点跳过该匹配点对// ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？// ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 // !可能导致初始化不稳定if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;

（3）路标点投影到图像中，重投影误差不能太大

        float im1x, im1y;//这个使能空间点的z坐标的倒数float invZ1 = 1.0/p3dC1.at<float>(2);//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合，因此这里就直接进行投影就可以了im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;//参考帧上的重投影误差，这个的确就是按照定义来的float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);// 重投影误差太大，跳过淘汰if(squareError1>th2)continue;// Check reprojection error in second image// 计算3D点在第二个图像上的投影误差，计算过程和第一个图像类似float im2x, im2y;// 注意这里的p3dC2已经是第二个相机坐标系下的三维点了float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;// 计算重投影误差float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影误差太大，跳过淘汰if(squareError2>th2)continue;

取出一个较小的视差角

在分解F矩阵获得R、t，随后进行三角化之后会对视差角进行判断。如果视差角太小，说明这次平移很小，ReconstructF()函数直接返回失败。

在这里插入代码片

    // Step 7 得到3D点中较大的视差角，并且转换成为角度制表示if(nGood>0){// 从小到大排序，注意vCosParallax值越大，视差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并没有取最小的视差角，而是取一个较小的视差角// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 size_t idx = min(50,int(vCosParallax.size()-1));//将这个选中的角弧度制转换为角度制parallax = acos(vCosParallax[idx])*180/CV_PI;}

SVD分解E矩阵，获得R,t

    //                          |0 -1  0|// E = U Sigma V'   let W = |1  0  0|//                          |0  0  1|// 得到4个解 E = [R|t]// R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{// 对本质矩阵进行奇异值分解//准备存储对本质矩阵进行奇异值分解的结果cv::Mat u,w,vt;//对本质矩阵进行奇异值分解cv::SVD::compute(E,w,u,vt);// 左奇异值矩阵U的最后一列就是t，对其进行归一化u.col(2).copyTo(t);t=t/cv::norm(t);// 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt//计算完成后要检查一下旋转矩阵行列式的数值，使其满足行列式为1的约束cv::Mat W(3,3,CV_32F,cv::Scalar(0));W.at<float>(0,1)=-1;W.at<float>(1,0)=1;W.at<float>(2,2)=1;//计算R1 = u*W*vt;//旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反if(cv::determinant(R1)<0) R1=-R1;// 同理将矩阵W取转置来按照相同的公式计算旋转矩阵R2 = u*W.t()*vtR2 = u*W.t()*vt;//旋转矩阵有行列式为1的约束if(cv::determinant(R2)<0)R2=-R2;
}