ORB-SLAM2源码学习：Initializer.cc(13): Initializer::ReconstructF用F矩阵恢复R，t及三维点

前言

这部分函数的实现依赖于之前学习的的检查三角化结果的函数：

ORB-SLAM2源码学习：Initializer.cc⑧: Initializer::CheckRT检验三角化结果-CSDN博客

这里通过基础矩阵F来恢复位姿和三维点实际上是借助本质矩阵来进行的，相对于用单应矩阵H恢复位姿和三维点来说，它的解的结构较为简单其解的讨论形式也是比较简单的。

1.函数声明 

bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)

2.函数定义

具体流程如下：

1.统计有效内点的数量

 // Step 1 统计有效匹配点个数，并用 N 表示// vbMatchesInliers 中存储匹配点对是否是有效int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i]) N++;

2.计算本质矩阵E

调用DecomposeE函数来分解本质矩阵来构造四组解。

ORB-SLAM2源码学习：Initializer.cc(12): Initializer::DecomposeE分解本质矩阵得到R，t-CSDN博客

// Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵cv::Mat E21 = K.t()*F21*K;// 定义本质矩阵分解结果，形成四组解,分别是：// (R1, t) (R1, -t) (R2, t) (R2, -t)cv::Mat R1, R2, t;// Step 3 从本质矩阵求解两个R解和两个t解，共四组解// 不过由于两个t解互为相反数，因此这里先只获取一个// 虽然这个函数对t有归一化，但并没有决定单目整个SLAM过程的尺度. // 因为 CreateInitialMapMonocular 函数对3D点深度会缩放，然后反过来对 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 = -U3DecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;

3.验证四组解选出最佳解

这里调用了CheckRT函数来获取三角化成功的三维点数。

// Reconstruct with the 4 hyphoteses and check// Step 4 分别验证求解的4种R和t的组合，选出最佳组合// 原理：若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合// 实现：根据计算的解组合成为四种情况,并依次调用 Initializer::CheckRT() 进行检查,得到可以进行三角化测量的点的数目// 定义四组解分别在对同一匹配点集进行三角化测量之后的特征点空间坐标vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;// 定义四组解分别对同一匹配点集的有效三角化结果，True or Falsevector<bool> vbTriangulated1,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<int>(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(maxGood<nMinGood || nsimilar>1)	{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;

完整的代码分析

/*从基础矩阵F中求解位姿R，t及三维点F分解出E，E有四组解，选择计算的有效三维点（在摄像头前方、投影误差小于阈值、视差角大于阈值）最多的作为最优的解vbMatchesInliers          匹配好的特征点对的Inliers标记F21                       从参考帧到当前帧的基础矩阵K                         相机的内参数矩阵R21                 计算好的相机从参考帧到当前帧的旋转t21                 计算好的相机从参考帧到当前帧的平移vP3D                三角化测量之后的特征点的空间坐标vbTriangulated      特征点三角化成功的标志minParallax               认为三角化有效的最小视差角minTriangulated           最小三角化点数量return true                         成功初始化return false                        初始化失败*/
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{// Step 1 统计有效匹配点个数，并用 N 表示// vbMatchesInliers 中存储匹配点对是否是有效int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i]) N++;// Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵cv::Mat E21 = K.t()*F21*K;// 定义本质矩阵分解结果，形成四组解,分别是：// (R1, t) (R1, -t) (R2, t) (R2, -t)cv::Mat R1, R2, t;// Step 3 从本质矩阵求解两个R解和两个t解，共四组解// 不过由于两个t解互为相反数，因此这里先只获取一个// 虽然这个函数对t有归一化，但并没有决定单目整个SLAM过程的尺度. // 因为 CreateInitialMapMonocular 函数对3D点深度会缩放，然后反过来对 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 = -U3DecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;// Reconstruct with the 4 hyphoteses and check// Step 4 分别验证求解的4种R和t的组合，选出最佳组合// 原理：若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合// 实现：根据计算的解组合成为四种情况,并依次调用 Initializer::CheckRT() 进行检查,得到可以进行三角化测量的点的数目// 定义四组解分别在对同一匹配点集进行三角化测量之后的特征点空间坐标vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;// 定义四组解分别对同一匹配点集的有效三角化结果，True or Falsevector<bool> vbTriangulated1,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<int>(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(maxGood<nMinGood || nsimilar>1)	{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;
}

结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。