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 &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 False
    vector ,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(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 根据基础矩阵和相机的内参数矩阵计算本质矩阵 

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

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

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