3.2ORBSLAM3之双目初始化和单目初始化

0总述

ORB-SLAM3算法中视觉的初始化依旧放在tracking线程中，因此在tracking中没有为imu模式设置单独的初始化函数，而IMU的初始化是在localMapping中实现的。

很有用的参考链接：https://cloud.tencent.com/developer/article/1761043

1双目初始化

1.1初始化前置条件

首先，双目初始化需要满足当前帧特征点的数量大于500,才认为具备双目初始化的前提条件，这里的特征点数量在双目模式指左目图像的特征点数量，在双鱼眼组合双目模式下是指左目+右目特征点的数量。

if(mCurrentFrame.N>500){...}

在双目+IMU模式时，需满足当前帧和上一帧都要具备IMU预积分信息，也就是说IMU模式下需要三帧才能初始化。
同时，如果当前帧和上一帧的加速度相差太小同样认为不满足初始化条件

if (!mCurrentFrame.mpImuPreintegrated || !mLastFrame.mpImuPreintegrated)
{
    cout << "not IMU meas" << endl;
    return;
}

if (!mFastInit && (mCurrentFrame.mpImuPreintegratedFrame->avgA-mLastFrame.mpImuPreintegratedFrame->avgA).norm()<0.5)
{
    cout << "not enough acceleration" << endl;
    return;
}

如果满足IMU模式下的初始化条件了，先为当前帧构造一个预积分器，初始的默认偏置为0

mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;

1.2 设置初始位姿

对于双目模式来说因为在图像帧构造这一步就直接得到了特征点对应的深度信息，因此初始化过程比较简单，当满足系统初始化条件后就直接设置当前帧对应的初始位姿。

对于纯双目视觉SLAM，设置初始旋转为单位阵，初始平移为0；

对于双目+IMU模式，设置初始旋转和平移为相机在IMU的body坐标系下的位置，设置初始速度为0

// Set Frame pose to the origin (In case of inertial SLAM to imu)
if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
{
    Eigen::Matrix3f Rwb0 = mCurrentFrame.mImuCalib.mTcb.rotationMatrix();
    Eigen::Vector3f twb0 = mCurrentFrame.mImuCalib.mTcb.translation();
    Eigen::Vector3f Vwb0;
    Vwb0.setZero();
    mCurrentFrame.SetImuPoseVelocity(Rwb0, twb0, Vwb0);
}
else
    mCurrentFrame.SetPose(Sophus::SE3f());

1.3创建关键帧和3D地图点，并将两者进行关联

初始位姿设置完成后就是将3D的特征点构造成一个Mappoint类对象，然后和关键帧进行关联。

则根据特征点的像素坐标和深度值进行反投影得到在相机坐标系下的坐标，然后根据当前帧的位姿转换为世界坐标系下的坐标，然后做以下事情：

• 1.构造Mappoint
• 2.关联关键帧和地图点
• 3.更新地图点的描述子
• 4.更新地图点的平均观测方向和观测距离
• 5.在地图中添加地图点
• 6.将地图点关联到图像帧

if(z>0)
{
    Eigen::Vector3f x3D;
    // 反投影变换和位姿转换
    mCurrentFrame.UnprojectStereo(i, x3D);
    // 构造一个3D地图点，需要世界坐标系下的坐标，关键帧，当前所在的活跃地图
    MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap());
    // 表示当前帧第i个地图点pNewMP被关键帧pKFini观测到了
    pNewMP->AddObservation(pKFini,i);
    // 表示当前关键帧pKFini观测到了当前帧第i个地图点pNewMP
    pKFini->AddMapPoint(pNewMP,i);
    // 为该地图点选择一个描述子，找到观测到所有该地图点的关键真->在每一个关键帧找到pNewMP对应的特征点的描述子->计算所有描述子两两之间的距离
    // ->将每个描述子和其他描述子的距离进行排序并寻找中值 ->比较所有描述子对应的中值，选择最小中值对应的描述子作为pNewMP的描述子
    pNewMP->ComputeDistinctiveDescriptors();
    // 更新地图点的平均观测方向和观测距离，观测方向是指世界坐标系下3D点到相机光心的方向，观测距离是指3D点到相机光心的距离
    pNewMP->UpdateNormalAndDepth();
    // 在当前活跃地图中添加地图点
    mpAtlas->AddMapPoint(pNewMP);
    // 将地图点关联到当前图像帧
    mCurrentFrame.mvpMapPoints[i]=pNewMP;
}

1.4变量更新

最后就是把关键帧送给局部建图线程，然后更新参考关键帧等一系列变量。

2 单目初始化

单目模式的初始化主要包括并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云，计算初始两帧的匹配、相对运动、初始MapPoints,在单目初始化时，IMU并未起到作用，只是更新了预积分器。

主要包括以下步骤：

• Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
• Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
• Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
• Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
• Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
• Step 6：删除那些无法进行三角化的匹配点
• Step 7：将三角化得到的3D点包装成MapPoints

2.1 得到用于初始化的第一帧，初始化需要两帧

这一步只是更新了mInitialFrame ，mLastFrame，和mvbPrevMatched等成员变量，然后直接返回等待在下一帧进行单目的初始化。

2.2 在mInitialFrame与mCurrentFrame中找匹配的特征点对

输入：初始化帧(第一帧)、当前帧(第二帧)、初始帧特征点std::vector<cv::Point2f> mvbPrevMatched、搜索半径100
输出：匹配特征点索引mvIniMatches、匹配数量nmatches

SearchForInitialization函数主要作用就是寻找并返回初始帧和当前帧特征点的匹配以及匹配数量，这里使用了一个小技巧，即根据初始帧特征点的坐标，在匹配帧相应坐标附近搜索初始帧中该特征点的匹配点，这里选择的搜索窗口是100

为了获得比较好的匹配，需要满足最佳匹配的描述子距离要比较明显地小于次佳匹配的描述子距离

同时，对所有的匹配点对之间的角度差进行统计并构建一个基于角度差的分布直方图，排除角度差和大多数相差较大的匹配对

如果最后得到的两帧之间的匹配小于100,则认为初始化失败，继续等待下一帧。

int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

2.3 计算基础矩阵和单应矩阵选择合适的初始化模型

基于H或者F模型计算两帧之间运动以及三角化特征点主要通过以下函数进行

mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated)

函数的输入：

• mInitialFrame.mvKeysUn:初始帧特征点
• mCurrentFrame.mvKeysUn：当前帧特征点
• mvIniMatches：表示参考帧F1和F2匹配关系
  mvIniMatches[i]的索引i是mInitialFrame特征点序号
  mvIniMatches[i]值保存的是匹配好的F2特征点索引
  函数的输出：
• Tcw: 待输出 初始化得到的相机的位姿
• mvIniP3D：std::vectorcv::Point3f待输出 进行三角化得到的空间点集合
• vbTriangulated: 待输出，对应于mvIniMatches来说，其中哪些点被三角化了

对于三角化这一过程，ORB-SLAM3将其放到了相机模型中，因此对于针孔相机模型和鱼眼相机模型各自有其三角化的方式，这里仅以针孔相机模型的三角化进行分析。

核心函数：tvr->Reconstruct(vKeys1,vKeys2,vMatches12,T21,vP3D,vbTriangulated);，返回值为是否成功三角化的标志

2.3.1 函数入口

在计算之前，函数构造了一个大小为200的向量mvSets，这个向量将用于后面计算基础矩阵F和单应矩阵H时的RANSAC迭代，向量的每一行都包括8个元素，其中200是指迭代次数，8是指计算F或者H时用到的8点法。

对应论文中的叙述，代码中创建了两个并行的线程同时计算这两种矩阵。

// 计算单应矩阵
thread threadH(&TwoViewReconstruction::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 计算基础矩阵
thread threadF(&TwoViewReconstruction::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

2.3.2.特征点坐标归一化

因为噪声、数值的四舍五入、误匹配等情况的存在，直接基于像素点计算得到的基础矩阵F会存在较大的偏差，因此在求解基础矩阵前需要对特征点像素坐标进行归一化处理，最终得到一组去均值去中心化的归一化坐标，和一个由特征点像素坐标到归一化坐标的变换矩阵T。

这里的归一化处理只是对特征点像素进行去均值和去中心化，并没有使用内参进行反投影，因此计算出来的依然是基础矩阵。

Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);

2.3.3 直接线性变换(DLT)求解

前面已经得到用于RANSAC的vector向量mvSets，mvSets有200个元素即RANSAC最高迭代200次，每个元素有8对匹配点，每次迭代都基于8个点计算矩阵。核心函数如下：

Eigen::Matrix3f Fn = ComputeF21(vPn1i,vPn2i);

首先,对于基础矩阵F，是一个3x3的矩阵，现在最终目的是通过匹配点的信息求解这个3x3矩阵，也即求解这个矩阵的9个元素。
根据对极约束，对于每一对匹配点都有p2t*F*p1=0,p1=[u1,v1],p2=[u2,v2]是匹配特征点的像素坐标，这样p2t*F*p1=0就可以写成以下形式：

            |f1 f2 f3|   |u1|
[u2,v2,1] * |f4 f5 f6| * |v1| = 0
            |f7 f8 f9|   |1 |

将矩阵F的各元素当作一个向量进行处理，则f=[f1,f2,f3,f4,f5,f6,f7,f8,f9]

上面的式子就变成：

[u2u1, u2v1, u2, v2u1, v2v1, v2, u1, v1, 1] * f = 0;

每一对点都可以写成如下形式，这样就可以得到一组8×9的矩阵A，即：

对应代码如下：

// N*9，每一行9个元素，对应一组点的方程信息
Eigen::MatrixXf A(N, 9);

for(int i=0; i<N; i++)
{
    const float u1 = vP1[i].x;
    const float v1 = vP1[i].y;
    const float u2 = vP2[i].x;
    const float v2 = vP2[i].y;

    A(i,0) = u2*u1;
    A(i,1) = u2*v1;
    A(i,2) = u2;
    A(i,3) = v2*u1;
    A(i,4) = v2*v1;
    A(i,5) = v2;
    A(i,6) = u1;
    A(i,7) = v1;
    A(i,8) = 1;
}

对于8对匹配点求解向量f就变成了求解最小二乘问题：

在代码中可以看到进行了两次奇异值分解

首先第一次进行SVD分解我们得到一个初步的基础矩阵F，F的9个元素对应V_T的第9个列向量

// svd分解器，F的9个元素对应V的第9列向量
Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
// 将向量f转换成3x3的基础矩阵
Eigen::Matrix<float,3,3,Eigen::RowMajor> Fpre(svd.matrixV().col(8).data());

对于基础矩阵来说，F的另一个性质就是其秩为2,即Rank(F) = 2，根据这一性质对F再进行进一步约束，方法就是将上面得到的基础矩阵再一次进行SVD分解，另第3个奇异值v3为0,这样使用v3=0的奇异值矩阵算出的F就是最终的基础矩阵。

// svd分解
Eigen::JacobiSVD<Eigen::Matrix3f> svd2(Fpre, Eigen::ComputeFullU | Eigen::ComputeFullV);
// 奇异值矩阵，将最后一个值取0
Eigen::Vector3f w = svd2.singularValues();
w(2) = 0;
//return F = U*奇异值矩阵(v3=0)*V
return svd2.matrixU() * Eigen::DiagonalMatrix<float,3>(w) * svd2.matrixV().transpose();

至此，一次迭代的基础矩阵计算完成。对于单应矩阵的求解来说，上述特征点坐标归一化，求解过程和基础矩阵求解一致，但是单应矩阵是可逆矩阵，也就是满秩矩阵，因此在SVD分解求对应的单应矩阵向量时只需要求解一次就可以了。

//对系数矩阵A进行SVD分解
Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullV);
// V的最后一列向量为单应矩阵对应解向量，将其转换为3x3矩阵
Eigen::Matrix<float,3,3,Eigen::RowMajor> H(svd.matrixV().col(8).data());
return H;

详细的推到过程如下，参考链接：https://cloud.tencent.com/developer/article/1761043

2.3.4对求解的基础矩阵或单应矩阵进行检验并转换为对应得分

检验原理：基于卡方检验将投影误差转换为对应的得分

相互匹配的一对特征点在空间几何上是满足对极约束关系的，如下图。理想状况下，根据计算得到的基础矩阵，当前帧上某一个特征点p1在另一帧上的投影点p2，会落在另一帧图像对应的极线e上。但是因为光线、噪声等因素的存在导致出现投影误差，即投影点到极线的距离不为0。

在这里，会经过两次判断，首先基于投影误差计算得到标准差要服从卡方分布(具体原理见参考链接)，然后对于服从卡方分布的点会计算一个得分，然后遍历所有匹配点对会得到一个总的分值，以此检验当前基础矩阵的准确性。

假设相邻两帧图像分别为I1、I2,具体实现过程如下：

1.根据基础矩阵计算I2上的极线方程对应参数：l2=F21x1 -> (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;

2.根据极线方程计算投影误差，并计算p2到极线的距离，也即标准差信息

    const float num2 = a2*u2+b2*v2+c2;// p2的投影误差
    const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 点到直线的距离公式，p2到极限l2的距离，距离越小误差越小

3.根据标准差计算卡方值

const float chiSquare1 = squareDist1*invSigmaSquare;// 基于标准差计算卡方值

4.判断当前匹配点对的投影误差是否服从卡方分布，服从则用于计算得分，不服从则判定当前匹配为误匹配

    // 如果p1,p2任一投影误差对应的卡方值大于阈值，认为当前匹配点对是无效匹配
    if(chiSquare1>th)
        bIn = false;
    else
        score += thScore - chiSquare1;// 根据卡方值与阈值的差值计算当前对应基础矩阵的得分

    ... ...

    // 根据投影误差的情况判定内点和外点，即是否为误匹配
    if(bIn)
        vbMatchesInliers[i]=true;
    else
        vbMatchesInliers[i]=false;

5.对反过来对p2在I1上的投影也进行一次相同的计算和检验，遍历所有匹配点对后返回得分

2.3.5 更新基础矩阵和对应得分

最后，如果得分满足阈值，停止RANSAC的迭代，更新基础矩阵和得分情况，基础矩阵用于后面的三角化恢复地图点，得分用于初始化模型的选择。

    // 选择的分最高时对应的基础矩阵，匹配点对和分数
    // 这个分数用于后面的模型判断
    if(currentScore>score)
    {
        F21 = F21i;
        vbMatchesInliers = vbCurrentInliers;
        score = currentScore;
    }

综上，就完成了基于匹配点对进行基础矩阵的计算，对于单应矩阵的计算和检验过程同基础矩阵，最后得到和投影误差相关的两个得分SF和SH，然后算一个比例，选择得分所占比例较小对应的变换矩阵,完成后续的三角化重构。

    // Step5:计算得分比例，选取某个模型
    float RH = SH/(SH+SF);

    float minParallax = 1.0;

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 从H矩阵或F矩阵中恢复T21以及恢复出来的3D点
    if(RH>0.50) // if(RH>0.40)
    {
        cout << "+++++++++++++++Initialization from Homography" << endl;
        return ReconstructH(vbMatchesInliersH,H, mK,T21,vP3D,vbTriangulated,minParallax,50);
    }
    else //if(pF_HF>0.6)
    {
        cout << "+++++++++++++++Initialization from Fundamental" << endl;
        return ReconstructF(vbMatchesInliersF,F,mK,T21,vP3D,vbTriangulated,minParallax,50);
    }

2.4 从基础矩阵F中恢复相机运动和3D点

基本过程比较清晰：

1. 基于基础矩阵直接计算得到本质矩阵E
2. 对本质矩阵E进行SVD分解得到4组R,t解
3. 使用4组解将特征点三角化为3D地图点(无尺度，在相机坐标系下)，并计算视差
4. 检验3D点数量和视差是否符合要求，得到唯一解

2.4.1基于基础矩阵直接计算得到本质矩阵E

基础矩阵F和本质矩阵之间的关系就差了一个内参矩阵，公式为：E = k^T F k

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

2.4.2对本质矩阵E进行SVD分解得到4组R,t解

参考链接：https://blog.csdn.net/kokerf/article/details/72911561

本质矩阵性质:1个3×3矩阵是本质矩阵的充要条件是它的奇异值有两个相等且第三个为0，根据这一性质反推知道本质矩阵的奇异值分解有两种形式，可求得两个对应的旋转矩阵和2个平移向量，进行排列组合则有4组对应解。

如果 E=t^R 的SVD分解为 Udiag(1,1,0)V^⊤，E = SR有两种分解形式，分别是： S = UZU^⊤ R = UWVTor UW^TV^⊤，S是t的反对称矩阵。

又因为St=0（自己和自己叉乘为0）以 ||t||=1（对两个摄像机矩阵的基线的一种常用归一化），因此 t = U(0,0,1)^T = u3​

应为t的符号不确定，R矩阵有两种可能，因此其分解有如下四种情况
P′=[UWV^T ∣ +u3​] or [UWV^T ∣ −u3​] or [UW^TV^T ∣ +u3​] or [UW^TV^T ∣ −u3​]

    void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t)
    {
        // 对本质矩阵进行SVD分解
        Eigen::JacobiSVD<Eigen::Matrix3f> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);

        // 分别拿到SVD分解的U和Vt矩阵
        Eigen::Matrix3f U = svd.matrixU();
        Eigen::Matrix3f Vt = svd.matrixV().transpose();

        // 平移矩阵为U矩阵对应最后一行列向量
        // 对 t 有归一化，但是这个地方并没有决定单目整个SLAM过程的尺度
        // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
        t = U.col(2);
        t = t / t.norm();

        
        Eigen::Matrix3f W;
        W.setZero();
        W(0,1) = -1;
        W(1,0) = 1;
        W(2,2) = 1;

        R1 = U * W * Vt;// 旋转矩阵有行列式为1的约束
        if(R1.determinant() < 0)
            R1 = -R1;

        R2 = U * W.transpose() * Vt;
        if(R2.determinant() < 0)
            R2 = -R2;
    }

2.4.3 对基础矩阵进行校验并将特征点三角化为3D地图点

1.定义两个相机的投影矩阵和相机光心

第一个相机旋转矩阵为内参矩阵，平移矩阵为0,相机光心为世界坐标系原点
第二个相机旋转矩阵和平移矩阵为估计得到的R和t,相机光心为当前的平移乘以变换矩阵

    // Camera 1 Projection Matrix K[I|0]
    // 步骤1：得到一个相机的投影矩阵
    // 以第一个相机的光心作为世界坐标系
    Eigen::Matrix<float,3,4> P1;
    P1.setZero();
    P1.block<3,3>(0,0) = K;

    Eigen::Vector3f O1;
    O1.setZero();

    // Camera 2 Projection Matrix K[R|t]
    // 步骤2：得到第二个相机的投影矩阵
    Eigen::Matrix<float,3,4> P2;
    P2.block<3,3>(0,0) = R;
    P2.block<3,1>(0,3) = t;
    P2 = K * P2;

    // 第二个相机的光心在世界坐标系下的坐标
    Eigen::Vector3f O2 = -R.transpose() * t;

2.对内点进行三角化

// 步骤3：利用三角法恢复三维点p3dC1
GeometricTools::Triangulate(x_p1, x_p2, P1, P2, p3dC1);

核心思想是将投影关系转化为一个AX=0的线性方程组求解问题，通过将A矩阵进行SAD分解得到特征点对应的3D坐标。

假设某个特征点的尺度为s,特征点像素坐标为x，投影矩阵为T(包含了内参矩阵)，3D点为X，则有：

`sx = TX` -> `sx x(叉乘) TX = 0` -> `x^TX = 0` ->
|0  -1   v| |r1|
|1   0  -u| |r2| X = 0  ->  
|-v  u   0| |r3|

|-r2 + vr3 |
|r1 - ur3  | X = 0 ,  第3行和前面两行线性相关 ->
|-vr1 + ur2|

|-r2 + vr3|  X = 0 ,  将相匹配的两个特征点全部带入 ->
|r1 - ur3 |

|-r22 + v2r23|
|r21 - u2r23 | X = 0
|-r12 + v1r13|
|r11 - u1r13 |

代码部分：

bool GeometricTools::Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2,Eigen::Matrix<float,3,4> &Tc1w ,Eigen::Matrix<float,3,4> &Tc2w , Eigen::Vector3f &x3D)
{
    Eigen::Matrix4f A;
    // x = a*P*X， 左右两面乘Pc的反对称矩阵 a*[x]^ * P *X = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度
    //  0 -1 v    P(0)     -P.row(1) + v*P.row(2)
    //  1 0 -u *  P(1)  =   P.row(0) - u*P.row(2) 
    // -v u  0    P(2)    u*P.row(1) - v*P.row(0)
    // 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式
    A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0);
    A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0);
    A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0);
    A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0);
    // 解方程 AX=0
    Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV);

    Eigen::Vector4f x3Dh = svd.matrixV().col(3);
    // 如果解出的是0向量该匹配点不好
    if(x3Dh(3)==0)
        return false;

    // Euclidean coordinates
    x3D = x3Dh.head(3)/x3Dh(3);

    return true;
}

3.计算视差角余弦值

    Eigen::Vector3f normal1 = p3dC1 - O1;
    float dist1 = normal1.norm();

    Eigen::Vector3f normal2 = p3dC1 - O2;
    float dist2 = normal2.norm();

    float cosParallax = normal1.dot(normal2) / (dist1*dist2);

4.根据3D点的深度是否在相机前方以及视差角余弦值进行筛选

    // 步骤5：判断3D点是否在两个摄像头前方
    // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
    // 步骤5.1：3D点深度为负，在第一个摄像头后方，淘汰
    if(p3dC1(2)<=0 && cosParallax<0.99998)
        continue;

    // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
    // 步骤5.2：3D点深度为负，在第二个摄像头后方，淘汰
    Eigen::Vector3f p3dC2 = R * p3dC1 + t;

    if(p3dC2(2)<=0 && cosParallax<0.99998)
        continue;

5.分别计算3D点在两帧图像上的重投影误差，排除误差较大的点

    // Check reprojection error in first image
    // 步骤6：计算重投影误差
    // Check reprojection error in first image
    // 计算3D点在第一个图像上的投影误差
    float im1x, im1y;
    float invZ1 = 1.0/p3dC1(2);
    im1x = fx*p3dC1(0)*invZ1+cx;
    im1y = fy*p3dC1(1)*invZ1+cy;

    float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
    // 步骤6.1：重投影误差太大，跳过淘汰
    // 一般视差角比较小时重投影误差比较大
    if(squareError1>th2)
        continue;

    // Check reprojection error in second image
    // 计算3D点在第二个图像上的投影误差
    float im2x, im2y;
    float invZ2 = 1.0/p3dC2(2);
    im2x = fx*p3dC2(0)*invZ2+cx;
    im2y = fy*p3dC2(1)*invZ2+cy;

    float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
    // 步骤6.1：重投影误差太大，跳过淘汰
    // 一般视差角比较小时重投影误差比较大
    if(squareError2>th2)
        continue;

6.最后统计三角化出的3D点的数量

对于每一组R，t都可以恢复出一组3D点，如果4组解得到的3D点的数量都不是很多，则判定初始化失败。如果有较优解(大于90%的匹配点数量)，则返回对应的旋转矩阵R和平移矩阵t