目录

1.函数作用

2.函数解析 

2.1 调用函数解析

2.2 Initializer::ReconstructF函数总体思路

2.2.1 代码

2.2.2 总体思路解析 

2.2.3 根据基础矩阵和相机的内参数矩阵计算本质矩阵 

2.2.4 从本质矩阵求解两个R解和两个t解，共四组解

2.2.5 分别验证求解的4种R和t的组合，选出最佳组合

1.函数作用

        从基础矩阵F中求解位姿R，t及三维点。

2.函数解析 

2.1 调用函数解析

return ReconstructF(vbMatchesInliersH,	//输入，匹配成功的特征点对Inliers标记H,					//输入，前面RANSAC计算后的单应矩阵mK,					//输入，相机的内参数矩阵R21,t21,			//输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换vP3D,				//特征点对经过三角测量之后的空间坐标，也就是地图点vbTriangulated,		//特征点对是否成功三角化的标记1.0,				//这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时//需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度50);				//为了进行运动恢复，所需要的最少的三角化测量成功的点个数

        该函数的调用函数为Initializer::Initialize，该函数的目的是初始化SLAM系统，即用单目初始化器的第一帧作为SLAM系统的基点并计算出第一帧和第二帧的变换矩阵并初始化地图点。此函数是在计算出F矩阵的前提下，我们想通过H矩阵来恢复单目初始化器两帧间的位姿。

        输入参数为匹配成功的特征点对Inliers标记、RANSAC计算出的单应矩阵H、相机的内参、认为某对特征点的三角化测量中有效时需要满足的最小视差角、为了进行运动恢复，所需要的最少的三角化测量成功的点个数（如果恢复的3D点小于这个则认为初始化失败）

        输出参数为计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换、特征点对经过三角测量之后的空间坐标，也就是地图点。

2.2 Initializer::ReconstructF函数总体思路

2.2.1 代码

/*** @brief 从基础矩阵F中求解位姿R，t及三维点* F分解出E，E有四组解，选择计算的有效三维点（在摄像头前方、投影误差小于阈值、视差角大于阈值）最多的作为最优的解* @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记* @param[in] F21                       从参考帧到当前帧的基础矩阵* @param[in] K                         相机的内参数矩阵* @param[in & out] R21                 计算好的相机从参考帧到当前帧的旋转* @param[in & out] t21                 计算好的相机从参考帧到当前帧的平移* @param[in & out] vP3D                三角化测量之后的特征点的空间坐标* @param[in & out] vbTriangulated      特征点三角化成功的标志* @param[in] minParallax               认为三角化有效的最小视差角* @param[in] minTriangulated           最小三角化点数量* @return true                         成功初始化* @return false                        初始化失败*/
bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{// Step 1 统计有效匹配点个数，并用 N 表示// vbMatchesInliers 中存储匹配点对是否是有效int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i vP3D1, vP3D2, vP3D3, vP3D4;// 定义四组解分别对同一匹配点集的有效三角化结果，True or Falsevector ,vbTriangulated2,vbTriangulated3, vbTriangulated4;// 定义四种解对应的比较大的特征点对视差角float parallax1,parallax2, parallax3, parallax4;// Step 4.1 使用同样的匹配点分别检查四组解，记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数，记为有效3D点个数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);// Step 4.2 选取最大可三角化测量的点的数目int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));// 重置变量，并在后面赋值为最佳R和TR21 = cv::Mat();t21 = cv::Mat();// Step 4.3 确定最小的可以三角化的点数 // 在0.9倍的内点数 和 指定值minTriangulated =50 中取最大的，也就是说至少50个int nMinGood = max(static_cast(0.9*N), minTriangulated);// 统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目// 如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回falseint nsimilar = 0;if(nGood1>0.7*maxGood)nsimilar++;if(nGood2>0.7*maxGood)nsimilar++;if(nGood3>0.7*maxGood)nsimilar++;if(nGood4>0.7*maxGood)nsimilar++;// Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败// 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败// 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败if(maxGood1)	{return false;}//  Step 4.5 选择最佳解记录结果// 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多// 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定//看看最好的good点是在哪种解的条件下发生的if(maxGood==nGood1){//如果该种解下的parallax大于函数参数中给定的最小值if(parallax1>minParallax){// 存储3D坐标vP3D = vP3D1;// 获取特征点向量的三角化测量标记vbTriangulated = vbTriangulated1;// 存储相机姿态R1.copyTo(R21);t1.copyTo(t21);// 结束return true;}}else if(maxGood==nGood2){if(parallax2>minParallax){vP3D = vP3D2;vbTriangulated = vbTriangulated2;R2.copyTo(R21);t1.copyTo(t21);return true;}}else if(maxGood==nGood3){if(parallax3>minParallax){vP3D = vP3D3;vbTriangulated = vbTriangulated3;R1.copyTo(R21);t2.copyTo(t21);return true;}}else if(maxGood==nGood4){if(parallax4>minParallax){vP3D = vP3D4;vbTriangulated = vbTriangulated4;R2.copyTo(R21);t2.copyTo(t21);return true;}}// 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败return false;
}

2.2.2 总体思路解析 

        我们先根据基础矩阵和相机的内参数矩阵计算本质矩阵，利用多视图几何的知识分解本质矩阵E得到两个R解和两个t解，共四组解。再分别验证求解的4种R和t的组合，选出最佳组合作为初始化两帧的位姿变换，并用三角化得到的地图点作为地图初始化的地图点。

2.2.3 根据基础矩阵和相机的内参数矩阵计算本质矩阵 

        对极约束如下：具体推导见博客对极约束推导

        我们定义本质矩阵为，我们定义基础矩阵为。

        由于我们之前计算出了举出矩阵E，于是通过下列矩阵变换我们可以求出本质矩阵：

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

2.2.4 从本质矩阵求解两个R解和两个t解，共四组解

         通过Initializer::DecomposeE函数可以通过本质矩阵计算出两组共四组解，具体推导见多视图几何书，这个我们不需要掌握，只需知道通过这个函数可以解出四组解。

2.2.5 分别验证求解的4种R和t的组合，选出最佳组合

        我们对于每组解调用Initializer::CheckRT函数，求得从每组解中通过三角化恢复的3D地图点坐标数量，再由一系列的判定手段判断该三角化的地图点是否满足一系列要求。
CheckRT函数解析https://blog.csdn.net/qq_41694024/article/details/127945657

        我们得到这四组解成功初始化的地图点的数量nGood1、nGood2、nGood3、nGood4。        

        我们记录最大可三角化测量的点的数目maxGood、确定最小的可以三角化的点数（在0.9倍的内点数和 指定值minTriangulated =50 中取最大的，也就是说至少50个） ，再统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目nsimilar，这是因为如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回false。四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败。最后记录最佳结果（初始化的3D点、最优的R,t，特征点是否三角化成功的bool向量vbTriangulated）并返回给上层函数调用表示地图初始化成功！