ORB-SLAM2 ---- Initializer::ReconstructF函数

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<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
                            cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
    // Step 1 统计有效匹配点个数，并用 N 表示
    // vbMatchesInliers 中存储匹配点对是否是有效
    int N=0;
    for(size_t i=0, iend = vbMatchesInliers.size() ; 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 = -U3
    DecomposeE(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 vP3D1, vP3D2, vP3D3, vP3D4;
 
	// 定义四组解分别对同一匹配点集的有效三角化结果，True or False
    vector<bool> ,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和T
    R21 = 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就认为有问题了，后面返回false
    int 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 根据基础矩阵和相机的内参数矩阵计算本质矩阵

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

$p_{2}^{T}K^{-T}EK^{-1}p_{1}=0$

我们定义本质矩阵为 $E= t^{\wedge }R$ ，我们定义基础矩阵为 $F=K^{-T}EK^{-1}$ 。

由于我们之前计算出了举出矩阵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）并返回给上层函数调用表示地图初始化成功！

相关阅读:
IOTE 2023盛况回顾，美格智能聚连接之力促数字新生长
 Gmail邮箱怎么获取授权码？熟悉一下
 前端CSS零碎知识点
 Flink部署——弹性扩缩容
 7-38 最小生成树的唯一性
 基于LMI的非线性混沌系统滑模控制
 k8s configMap中subPath字段和items字段详解
 软件测试技术之如何编写测试用例（5）
047：mapboxGL本地上传shp文件，在map上解析显示图形
 react 响应式栅格布局
原文地址：https://blog.csdn.net/qq_41694024/article/details/127945735