ORB-SLAM2之单目地图初始化详解

ORB-SLAM2地图初始化

​ SLAM中的初始化通常是指地图的初始化，也就是生成最初的地图。在ORB-SLAM2中，初始化和使用的传感器类型有关，其中在单目相机模式下初始化相对复杂，需要运行一段时间才能成功。

​ 对于RGB-D相机初始化来说，因为该相机可以直接输出RGB图像和对应的深度图像，所以每个像素点对应的深度值是确定的。也就是说，在第一帧提取了特征点后，特征点对应的三维点在空间中的绝对坐标是可以计算出来的（需要用到内参）。

​ 对于双目相机初始化来说，也可以通过第一帧左右目图像立体匹配来得到特征点对应的三维点在空间中的绝对坐标。因为第一帧的三维点是作为地图来实现跟踪的，所以这些三维点也称为地图点。

​ 所以，从理论上来说，双目相机、RGB-D相机在第一帧就可以完成初始化。而对于单目相机始化来说，仅有第一帧还无法得到三维点，要想初始化，需要像双目相机那样进行立体匹配。

​ 除此以外，单目相机缺乏尺度。

• 没有绝对尺度会导致位姿和地图点都和真实世界相差一个未知的比例
• 我们可以在初始化时固定某个参考尺度，但是在跟踪过程中很可能会产生尺度漂移
• 在闭环时需要计算当前帧和闭环候选帧之间的尺度，并进行误差均摊

1 单目相机地图初始化

1.1 Initializer类成员

成员变量	意义
vector<cv::KeyPoint> mvKeys1	Reference Frame的特征点（第一帧_特征点数 >100）
vector<cv::KeyPoint> mvKeys2	current Frame的特征点（第二帧_特征点数 >100）
typedef pair<int,int> Match	键值对(RF特征点索引—cF特征点索引)
vector<Match> mvMatches12	匹配好的特征点对(索引)
vector<bool> mvbMatched1	参考帧的特征点在当前帧匹配成功与否
cv::Mat mK	相机内参
float mSigma, mSigma2	标准差、方差
int mMaxIterations = 200	Ransac最大迭代次数(随机选取多少个8对点)
vector<vector<size_t> > mvSets	外层容器的大小为迭代次数，内层为随机的点对

1.2 初始化过程

​ 首先重新记录特征点对的匹配关系(实际上最开始两帧原先的关系就是空的)，然后随机选取200组8对点，再按下图所示初始化整个过程。

1.3 源码分析

    /**
     * @brief 计算基础矩阵F和单应性矩阵H，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点
     * Step 1 重新记录特征点对的匹配关系
     * Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
     * Step 3 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算    并行计算
     * Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
     * 
     * @param[in] CurrentFrame          当前帧2，也就是SLAM意义上的第二帧    1表示参考帧  2 表示当期帧
     * @param[in] vMatches12            当前帧（2）和参考帧（1）图像中特征点的匹配关系
     *                                  vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
     *                                  没有匹配关系的话，vMatches12[i]值为 -1
     * @param[in & out] R21                   相机从参考帧到当前帧的旋转
     * @param[in & out] t21                   相机从参考帧到当前帧的平移
     * @param[in & out] vP3D                  三角化测量之后的三维地图点
     * @param[in & out] vbTriangulated        标记三角化点是否有效，有效为true
     * @return true                     该帧可以成功初始化，返回true
     * @return false                    该帧不满足初始化条件，返回false
     */
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{

    // Fill structures with current keypoints and matches with reference frame
    // Reference Frame: 1, Current Frame: 2

    // Step 1 重新记录特征点对的匹配关系
    mvKeys2 = CurrentFrame.mvKeysUn;    // get当前帧的特征点

    mvMatches12.clear();    // 类成员对象vector<Match> mvMatches12   先清空匹配关系
    mvMatches12.reserve(mvKeys2.size());    // 至少为当前帧的关键点数大小
    mvbMatched1.resize(mvKeys1.size());     // 记录参考帧的特征点在当前帧匹配成功与否

    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    {
        if(vMatches12[i]>=0)    // 说明RF的特征点i在CF中有对应的匹配点vMatches12[i]
        {
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
            mvbMatched1[i]=true;
        }
        else
            mvbMatched1[i]=false;   // 说明RF的特征点i在CF中没有对应的匹配点
    }

    // 成功匹配的特征点对数
    const int N = mvMatches12.size();

    // Indices for minimum set selection
    vector<size_t> vAllIndices; // 就是存储了0~N-1的特征点对索引  就是指第i个特征点对  不是特征点的索引
    vAllIndices.reserve(N);

    // 在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引
    vector<size_t> vAvailableIndices;
    
    //初始化所有特征点对的索引，索引值0到N-1
    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

    // Generate sets of 8 points for each RANSAC iteration
    // Step 2 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
    // mMaxIterations就是说选200组  即200个vector<size_t>(8,0)  每个里面有8个数，指8对匹配点的索引
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    // 用于进行随机数据样本采样，设置随机数种子
    DUtils::Random::SeedRandOnce(0);

    for(int it=0; it<mMaxIterations; it++)
    {
        // 迭代开始的时候，所有的点都是可用的
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        // 选择最小的数据样本集，使用八点法求，所以这里就循环了八次
        for(size_t j=0; j<8; j++)
        {
            // 随机产生一个整数randi,范围从0到N-1
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            // idx表示被选种的特征点对的索引   
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;

            // 把这个随机整数randi放在数组最后面，然后删除
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }// 完成一组，总共200组
    }// 200组完毕



    // Launch threads to compute in parallel a fundamental matrix and a homography
    // Step 3 计算fundamental 矩阵 和homography 矩阵，并行计算 
    vector<bool> vbMatchesInliersH, vbMatchesInliersF; //这两个变量用于标记在H和F的计算中哪些特征点对被认为是Inlier
    float SH, SF; // 得分score for H and F
    cv::Mat H, F; //这两个是经过RANSAC算法后计算出来的单应矩阵和基础矩阵

    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

    // Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
    // 通过这个规则来判断谁的评分占比更多一些
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    if(RH>0.40)
        //更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果
        return ReconstructH(vbMatchesInliersH,  //输入，匹配成功的特征点对Inliers标记
                            H,                  //输入，前面RANSAC计算后的单应矩阵
                            mK,                 //输入，相机的内参数矩阵
                            R21,t21,            //输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
                            vP3D,               //特征点对经过三角测量之后的空间坐标，也就是地图点
                            vbTriangulated,     //特征点对是否成功三角化的标记
                            1.0,                //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时
                                                //需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
                            50);                //为了进行运动恢复，所需要的最少的三角化测量成功的点个数
    else //if(pF_HF>0.6)
        // 更偏向于非平面，从基础矩阵恢复
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;
}

1.4 RANSAC算法

​ 少数外点会极大影响计算结果的准确度随着采样数量的增加，外点数量也会同时增加，这是一种系统误差，无法通过增加采样点来解决
​ RANSAC(Random sample consensus)随机采样一致性算法的思路是少量多次重复实验，每次实验仅使用尽可能少的点来计算，并统计本次计算中的内点数.只要尝试次数足够多的话，总会找到一个包含所有内点的解。

​ RANSAC算法的核心是减少每次迭代所需的采样点数.从原理上来说，计算基础矩阵F最少只需要7对匹配点，计算单应矩阵H最少只需要4对匹配点，计算本质矩阵$E$至少需要5对匹配点。至少需要多少对匹配点都是根据它的自由度来计算的，单应矩阵H是8个自由度，4对匹配点提供8个约束方程即可求解；基础矩阵F虽然是7自由度，但一对匹配点只能提供1个约束方程，本质矩阵E同理。

​ 为什么会采用RANSAC算法而不是直接用最小二乘法，是因为这些匹配点中存在误匹配！

​ ORB-SLAM2中为了编程方便，每次迭代使用8对匹配点计算F和H.

void DUtils::Random::SeedRandOnce(int seed)
{
  if(!m_already_seeded)
  {
    DUtils::Random::SeedRand(seed);
    m_already_seeded = true;
  }
}// 使用0作为种子（Seed）只初始化一次随机数生成器（Rand

int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1); // DBow库实现

2 求解单应矩阵H

​ 在初始化中，以下两种情况更适合使用单应矩阵进行初始化位姿估计：

1 相机看到的场景是一个平面

2 连续两帧间没发生平移，只发生旋转

​ 求单应矩阵的步骤如下。

第1步 对特征匹配点坐标进行归一化。见2.1

第2步 开始迭代，每次选择8对点计算单应矩阵。见2.2

第3步 根据当次计算的单应矩阵的重投影误差对结果进行评分。见2.3

第4步 重复第2、3步，以得分最高的单应矩阵作为最优的单应矩阵。整体代码如下：

/**
 * @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
 * 原理参考Multiple view geometry in computer vision  P109 算法4.4
 * 
 * @param[in & out] vbMatchesInliers          标记是否是外点
 * @param[in & out] score                     计算单应矩阵的得分
 * @param[in & out] H21                       单应矩阵结果
 */
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
    // 匹配的特征点数
    const int N = mvMatches12.size();


    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换

    vector<cv::Point2f> vPn1, vPn2; // 归一化后的参考帧1和当前帧2中的特征点坐标
    cv::Mat T1, T2;     // 记录各自的归一化矩阵
    Normalize(mvKeys1,vPn1, T1);    // 参考值关键点坐标归一化vPn1  T1是归一化矩阵
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2inv = T2.inv();   // 辅助进行原始尺度的恢复

    // Best Results variables
    score = 0.0;    // 最佳得分
    vbMatchesInliers = vector<bool>(N,false); // 取得历史最佳评分时,特征点对的inliers标记

    // Iteration variables
    vector<cv::Point2f> vPn1i(8);   // 某次迭代中，参考帧的特征点坐标  8表示初始化8个Point2f
    vector<cv::Point2f> vPn2i(8);   // 某次迭代中，当前帧的特征点坐标
    cv::Mat H21i, H12i;             // 单应矩阵


    vector<bool> vbCurrentInliers(N,false); // 每次RANSAC记录Inliers与得分
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
    // 200次迭代，记录得分最高的单应矩阵H和基础矩阵F
    for(int it=0; it<mMaxIterations; it++)
    {
        // Step 2 选择8个归一化之后的点对进行迭代
        for(size_t j=0; j<8; j++)
        {
            int idx = mvSets[it][j];    // 那200组随机8特征点对
            // mvMatches12[idx].first、second是特征点索引  vPn1、2特征点向量
            vPn1i[j] = vPn1[mvMatches12[idx].first];    // first参考帧的特征点坐标(归一化后)
            vPn2i[j] = vPn2[mvMatches12[idx].second];   // second当前帧
        }

        // 已经选好了8对点
        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);   // 求得单应矩阵H21



        //  ！！！！！！！！！！！！！！！！！！！！！！！！！！！！
        // 单应矩阵原理：X2=H21*X1，其中X1,X2 为归一化后的特征点    
        // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  得到:T2 * mvKeys2 =  Hn * T1 * mvKeys1   
        // 进一步得到:mvKeys2  = T2.inv * Hn * T1 * mvKeys1
        H21i = T2inv*Hn*T1; // 归一化后的单应矩阵H21 !!! 思路真的赞
        // 然后计算逆
        H12i = H21i.inv();

        // Step 4 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckHomography(H21i, H12i,            //输入，单应矩阵的计算结果
                                       vbCurrentInliers,    // 输出，特征点对的Inliers标记
                                       mSigma);             //TODO  测量误差，在Initializer类对象构造的时候，由外部给定的

    
        // Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
        if(currentScore>score)
        {
            // 如果当前的结果得分更高，那么就更新最优计算结果
            H21 = H21i.clone();
            // 保存匹配好的特征点对的Inliers标记
            vbMatchesInliers = vbCurrentInliers;
            // 更新历史最优评分
            score = currentScore;
        }
    }
}

2.1 对特征匹配点坐标归一化

2.1.1 算法流程

​ 因为8对点是随机选取的，所以特征点可能分布在图像的任何地方，这可能会导致坐标值的量级差别比较大。如果不对数据进行预处理，直接求解单应矩阵或基础矩阵，则会带来较大的误差。

第1步，假设共有$N$对特征点，计算特征点坐标$(x_i,y_i),i=1,...,N$分别在两个坐标轴方向上的均值**$meanX、meanY$😗*

$$\begin{array}{rl}\mathrm{mean}X& =\frac{{\sum }_{i=1}^N{x}_i}{N}\\ \mathrm{mean}Y& =\frac{{\sum }_{i=1}^N{y}_i}{N}\end{array}$$

第2步，求出平均到每个点上，其坐标偏离均值$meanX、meanY$的程度，记为$meanDevX、meanDevY$,并将其倒数作为一个尺度缩放因子$sX、sY$:

$$\begin{array}{rl}\text{meanDev}X& =\sum _{i=1}^N\frac{||x_i-\text{mean}X||}{N}=\frac{1}{sX}\\ \text{meanDev}Y& =\sum _{i=1}^N\frac{||y_i-\text{mean}Y||}{N}=\frac{1}{sY}\end{array}$$

第3步，用均值和尺度缩放因子对坐标进行归一化，归一化后的坐标记为$(x_i^{{}^{\prime }},y_i^{{}^{\prime }}),i=1,...,N$

$$\begin{array}{c}{x}_i^{\prime }=sX(x_i-\text{mean}X)\\ y_i^{\prime }=sY(y_i-\text{mean}Y)\end{array}$$

第4步，用矩阵表示上述变换关系，得到归一化矩阵$T$:

$$\left[\begin{array}{c}{x}_i^{\prime }\\ y_i^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}sX& 0& -sX\ast \text{mean}X\\ 0& sY& -sY\ast \text{mean}Y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]$$

2.1.2 代码分析

/**
 * @brief 归一化特征点到同一尺度，作为后续normalize DLT的输入
 * 
 *  Step 1 计算特征点X,Y坐标的均值 
 *  Step 2 计算特征点X,Y坐标离均值的平均偏离程度
 *  Step 3 将x坐标和y坐标分别进行尺度归一化，
 *  Step 4 计算归一化矩阵：其实就是前面做的操作用矩阵变换来表示而已
 * 
 * @param[in] vKeys                               待归一化的特征点
 * @param[in & out] vNormalizedPoints             特征点归一化后的坐标
 * @param[in & out] T                             归一化特征点的变换矩阵
 */
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
    float meanX = 0;    // 平均值
    float meanY = 0;
    const int N = vKeys.size(); // 有多少个特征点

    vNormalizedPoints.resize(N);   // 归一化以后的特征点坐标

    for(int i=0; i<N; i++)
    {
        meanX += vKeys[i].pt.x; // 累加每一个坐标
        meanY += vKeys[i].pt.y;
    }

    meanX = meanX/N;    // 求取平均
    meanY = meanY/N;

    float meanDevX = 0; // 计算特征点X,Y坐标离均值的平均偏离程度
    float meanDevY = 0;

    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX; // 偏离均值程度
        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;

        meanDevX += fabs(vNormalizedPoints[i].x);   // 累加
        meanDevY += fabs(vNormalizedPoints[i].y);
    }

    // 求出平均到每个点上，其坐标偏离横纵坐标均值的程度；将其倒数作为一个尺度缩放因子
    meanDevX = meanDevX/N;  
    meanDevY = meanDevY/N;

    // 缩放尺度
    float sX = 1.0/meanDevX;
    float sY = 1.0/meanDevY;

    // 最终的归一化坐标
    for(int i=0; i<N; i++)
    {
        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
    }

    // 归一化矩阵
    T = cv::Mat::eye(3,3,CV_32F);
    T.at<float>(0,0) = sX;
    T.at<float>(1,1) = sY;
    T.at<float>(0,2) = -meanX*sX;
    T.at<float>(1,2) = -meanY*sY;
}

2.2 迭代计算单应矩阵H

​ 计算单应矩阵H最少只需要4对匹配点，至少需要多少对匹配点都是根据它的自由度来计算的，单应矩阵H是8个自由度($3\ast 3$共$9$个元素，即9自由度，但单应矩阵有尺度不变性，减去$1$个自由度即$8$个自由度)，4对匹配点提供8个约束方程即可求解。但是再ORB-SLAM2中为了求解方便，统一使用8对点。

2.2.1 数学推导

​ 特征匹配点分别为$p1=[u_1,v_1,1{]}^T,p2=[u_2,v_2,1{]}^T$,三维空间点(地图点)$P$落在平面上，平面满足方程如下。之前忘记了平面方程写法，所以不知道$d$是什么。$d$可以理解为空间平面内的任意一点与法向量的乘积。

$$n^{\mathrm{T}}P+d=0.$$
​ 上式中，$n$是平面的法向量，$R$ 和$t$ 表示旋转和平移向量
$$-\frac{n^{\mathrm{T}}P}{d}=1$$
注意$P$是相机坐标系下的点，从参考帧(1)投影到当前帧(2),$p_1,p_2$是像素坐标系下的点
$$p_1\simeq KP,p_2\simeq K\left(R_{21}P+t\right)$$

$$\begin{array}{rl}{p}_2& \simeq \mathbf{K}({\mathbf{R}}_{21}\mathbf{P}+\mathbf{t})\\ & \simeq K\left(R_{21}P+\mathbf{t}\cdot (-\frac{{\mathbf{n}}^{\mathrm{T}}\mathbf{P}}{d})\right)\\ & \simeq K\left(R_{21}-\frac{t{\mathbf{n}}^{\mathrm{T}}}{d}\right)\mathbf{P}\\ & \simeq K\left(R_{21}-\frac{t{n}^{\mathrm{T}}}{d}\right)K^{-1}{\mathbf{p}}_1.\end{array}$$

​

​ 用单应矩阵$H_{21}$描述特征点对之间的变换关系如下，我们也能看出单应矩阵$H$的尺度不变性(尺度不变性—等式两边乘以非0的数，等式恒成立)。这里就是利用单应矩阵简化了上面的等式，注意并不是绝对相等。后面要利用单应矩阵求解位姿$T$也是通过这里的等式！
$$p_2=H_{21}{p}_1$$
​ 写为矩阵形式：
$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]$$
​ 上面等式坐标叉乘一个$p_2$
$$p_2\times H_{21}{p}_1=0$$
​ 我们知道，叉积可以表示为反对称矩阵，所以就可以得到下面的矩阵乘法
$$\left[\begin{array}{ccc}0& -1& v_2\\ 1& 0& -u_2\\ -v_2& u_2& 0\end{array}\right]\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
​ 展开整理：
$$\begin{array}{rl}{v}_2& =(h_4{u}_1+h_5{v}_1+h_6)/(h_7{u}_1+h_8{v}_1+h_9)\\ u_2& =(h_1{u}_1+h_2{v}_1+h_3)/(h_7{u}_1+h_8{v}_1+h_9)\end{array}$$
​ 写为齐次方程
$$\begin{array}{rl}-(h_4{u}_1+h_5{v}_1+h_6)+(h_7{u}_1{v}_2+h_8{v}_1{v}_2+h_9{v}_2)& =0\\ h_1{u}_1+h_2{v}_1+h_3-(h_7{u}_1{u}_2+h_8{v}_1{u}_2+h_9{u}_2)& =0\end{array}$$
​ 转化为矩阵形式：
$$\left[\begin{array}{ccccccccc}0& 0& 0& -u_1& -v_1& -1& u_1{v}_2& v_1{v}_2& v_2\\ u_1& v_1& 1& 0& 0& 0& -u_1{u}_2& -v_1{u}_2& -u_2\end{array}\right]\left[\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\\ h_9\end{array}\right]=0$$

$$Ah=0$$

​ 实际上，上面等式并不是严格为0的。所以我们可以等价定义(求最接近于0，最小值，利用最小二乘法)一个代价函数：
$$f(\mathbf{h})=\frac{1}{2}(\mathbf{A}\mathbf{h}{)}^{\top }(\mathbf{A}\mathbf{h})=\frac{1}{2}{\mathbf{h}}^{\top }{\mathbf{A}}^{\top }\mathbf{A}\mathbf{h}$$
​ 等式**(矩阵)求导(分母布局)：
$$\begin{array}{c}\frac{\mathrm{d}f(\mathbf{h})}{\mathrm{d}\mathbf{h}}=0\\ A^{\top }A\mathbf{h}=0\end{array}$$
​ 这个时候把$A^{T}A$看作一个矩阵，再乘以一个列向量，我们可以联想到矩阵的特征值**，类似下面形式的
$$A^{T}Ah=\lambda h$$
也就是说，我们可以转化问题为求$A^{T}A$的最小特征值对应的特征向量h

​ 为了简化问题，对矩阵$A$进行SVD分解(注意$U、V$都是正交矩阵,$D$是对角矩阵)
$$A^{\top }A=(UD{V}^{\top }{)}^{\top }(UD{V}^{\top })=V{D}^{\top }{U}^{\top }UD{V}^{\top }=V{D}^{\top }D{V}^{\top }$$
​ 其中$D^{T}D$后还是对角阵，一个矩阵可以由下面的特征分解表示，$X$是特征向量组成矩阵。
$$B=X\Sigma X^{-1}$$
​ 所以这里我们可以得到$A^{T}A$的特征向量矩阵$V$。然后前面已经推出，找到其最小特征值对应的特征向量就是最优解！而且，SVD分解的对角矩阵$D$的奇异值是从左向右从大到小排列的，所以V的最后一列(对应最小奇异值)就是我们要找的最优解！

​ 这里用到了很多的矩阵理论中知识，请参考这里。

2.2.2 代码分析

/**
 * @brief 用DLT方法求解单应矩阵H————DLT即直接线性变换
 * 这里最少用4对点就能够求出来，不过这里为了统一还是使用了8对点求最小二乘解
 * 
 * @param[in] vP1               参考帧中归一化后的特征点
 * @param[in] vP2               当前帧中归一化后的特征点
 * @return cv::Mat              计算的单应矩阵H
 */
cv::Mat Initializer::ComputeH21(
    const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
    const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{

    //获取参与计算的特征点的数目
    const int N = vP1.size();

    // 构造用于计算的矩阵 A 
    cv::Mat A(2*N,              //行，注意每一个点的数据对应两行
              9,                //列
              CV_32F);          //float数据类型

    // 构造矩阵A，一个点对组成矩阵两行，一共8对，所以是16行，A_16*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.at<float>(2*i,0) = 0.0;
        A.at<float>(2*i,1) = 0.0;
        A.at<float>(2*i,2) = 0.0;
        A.at<float>(2*i,3) = -u1;
        A.at<float>(2*i,4) = -v1;
        A.at<float>(2*i,5) = -1;
        A.at<float>(2*i,6) = v2*u1;
        A.at<float>(2*i,7) = v2*v1;
        A.at<float>(2*i,8) = v2;

        //生成这个点的第二行
        A.at<float>(2*i+1,0) = u1;
        A.at<float>(2*i+1,1) = v1;
        A.at<float>(2*i+1,2) = 1;
        A.at<float>(2*i+1,3) = 0.0;
        A.at<float>(2*i+1,4) = 0.0;
        A.at<float>(2*i+1,5) = 0.0;
        A.at<float>(2*i+1,6) = -u2*u1;
        A.at<float>(2*i+1,7) = -u2*v1;
        A.at<float>(2*i+1,8) = -u2;

    }

    // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
    cv::Mat u,w,vt;

    //使用opencv提供的进行奇异值分解的函数
    cv::SVDecomp(A,                         //输入，待进行奇异值分解的矩阵
                 w,                         //输出，奇异值矩阵
                 u,                         //输出，矩阵U
                 vt,                        //输出，矩阵V^T
                 cv::SVD::MODIFY_A |        //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
                     cv::SVD::FULL_UV);     //FULL_UV=把U和VT补充成单位正交方阵

    // 返回最小奇异值所对应的右奇异向量
    // 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8(0-8)
    return vt.row(8).reshape(0,     // 转换后的通道数，这里设置为0表示是与前面相同
                             3);    // 原来是1行9列  现在行设置为3  就变成3*3矩阵，即我们要求解的单应矩阵！！
                                    // 真的很妙
}

2.3 单应矩阵H重投影评分

2.3.1 理论分析

​ 评分就是检验每一次计算的单应矩阵的好坏。

​ 通过单应矩阵对已经判断为内点的特征点进行双向投影，计算加权重投影误差，最终选择误差最小的单应矩阵作为最优解。

​ 双向投影就是指下式（注意，这个公式是尺度意义下相等，必须把单应矩阵投影后坐标归一化才满足下式！所以不是简单的像素坐标之差----面试失败的教训）：
$$\begin{array}{rl}{p}_2-H_{21}{p}_1& =0\\ p_1-H_{12}{p}_2& =0\end{array}$$
​ 实际上，理想情况下，$H_{21}=H_{12}$，上式右端都是0；但是我们计算是由误差的，也就是说通过单应矩阵进行的双向投影是有误差的，如下式（把误差标准化为标准正态分布，阈值即可采用卡方分布值）：
$$\begin{array}{rl}{e}_1& =\frac{||p_2-H_{21}{p}_1|{|}^2}{{\sigma }^2}\\ e_2& =\frac{||p_1-H_{12}{p}_2|{|}^2}{{\sigma }^2}\end{array}$$
​ $\sigma$是方差，设置为1。
​ 累计所有特征点对中的内点误差(内点就是匹配好的点对)，误差越大，评分越低。

$$\begin{array}{rl}scor{e}_1& =\sum _{i=1}^N\left(\mathrm{th}-e_1(i)\right),e_1(i)<\mathrm{th}\\ scor{e}_2& =\sum _{i=1}^N\left(\mathrm{th}-e_2(i)\right),e_2(i)<\mathrm{th}\\ \text{score}& ={\mathrm{score}}_1+{\mathrm{score}}_2\end{array}$$
​ $th$表示自由度为$2$的卡方分布在显著性水平为0.05时对应的临界阈值,就是阈值以内是可信的，超过阈值就不在可信！

2.3.2 补充：卡方检验

​ 卡方检验通过构造检验统计量${\chi }^2$来比较期望结果和实际结果之间的差别,这里的统计量就是双向投影误差。
$${\chi }^2=\Sigma \frac{(O-E{)}^2}{E}$$
取95%置信度下的卡方检验统计量阈值th = 5.991

• 若投影误差大于该阈值，则认为计算矩阵使用到了外点，将其分数设为0
• 若投影误差小于该阈值，则将统计量裕量th设为该解的置信度分数，

2.3.2 代码分析

/**
 * @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
 * 
 * @param[in] H21                (归一化后的)从参考帧到当前帧的单应矩阵
 * @param[in] H12                (归一化后的)从当前帧到参考帧的单应矩阵
 * @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
{   

    const int N = mvMatches12.size();   // 成功匹配点数

    const float h11 = H21.at<float>(0,0);   // H21
    const float h12 = H21.at<float>(0,1);
    const float h13 = H21.at<float>(0,2);
    const float h21 = H21.at<float>(1,0);
    const float h22 = H21.at<float>(1,1);
    const float h23 = H21.at<float>(1,2);
    const float h31 = H21.at<float>(2,0);
    const float h32 = H21.at<float>(2,1);
    const float h33 = H21.at<float>(2,2);

    const float h11inv = H12.at<float>(0,0);    // H12
    const float h12inv = H12.at<float>(0,1);
    const float h13inv = H12.at<float>(0,2);
    const float h21inv = H12.at<float>(1,0);
    const float h22inv = H12.at<float>(1,1);
    const float h23inv = H12.at<float>(1,2);
    const float h31inv = H12.at<float>(2,0);
    const float h32inv = H12.at<float>(2,1);
    const float h33inv = H12.at<float>(2,2);

    vbMatchesInliers.resize(N); // Inliers标记

    float score = 0;

     // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 5.991;

    const float invSigmaSquare = 1.0/(sigma*sigma); // 信息矩阵----方差的逆
    // 信息矩阵一般都是表示某个误差占的权重，但这种单变量的，就是一个一维的数。一般都设置为1


     // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差
    for(int i=0; i<N; i++)
    {
        // Step 2.1 提取参考帧和当前帧之间的特征匹配点对
        bool bIn = true;
        // mvMatches12[i].first就是参考帧的特征点索引  second即当前帧索引
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];

        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        // 计算重投影误差---参考帧

        // Step 2.2 计算 img2 到 img1 的重投影误差
        // x1 = H12*x2
        // 将图像2中的特征点通过单应变换投影到图像1中  
        // |u1|   |h11inv h12inv h13inv||u2|   |u2in1|
        // |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv
        // |1 |   |h31inv h32inv h33inv||1 |   |  1  |
        // 把式子乘出来就是上面结果  第三维算深度，所以要除以第三个维度坐标  即乘以w2in1inv
        // 因为这里的u，v是像素坐标系下坐标，第三维要归一化
        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;

        // 计算重投影误差 = ||p1(i) - H12 * p2(i)||2
        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);

         // Step 2.3 用阈值标记离群点，是内点的话累加得分  
        const float chiSquare1 = squareDist1*invSigmaSquare;

        if(chiSquare1>th)
            bIn = false;    // 误差e>th，直接不满足卡方检验，那么得分0
        else
            score += th - chiSquare1;   


        // 计算从img1 到 img2 的投影变换误差
        // x1in2 = H21*x1
        // 将图像2中的特征点通过单应变换投影到图像1中
        // |u2|   |h11 h12 h13||u1|   |u1in2|
        // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
        // |1 |   |h31 h32 h33||1 |   |  1  |
        // 计算投影归一化坐标
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;

        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);

        const float chiSquare2 = squareDist2*invSigmaSquare;

        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;


         // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
        if(bIn)
            vbMatchesInliers[i]=true;   // 是内点
        else
            vbMatchesInliers[i]=false;
    }

    return score;
}

3 求解基础矩阵F

​ 常见都是非平面场景，此时需要用基础矩阵求解位姿。求基础矩阵的步骤如下。

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
    // 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似.
    // 匹配的特征点对 总数
    const int N = mvMatches12.size();
    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
    // ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同，两者去归一化的计算也不相同
    cv::Mat T2t = T2.t();


    // 最高得分
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
    vector<cv::Point2f> vPn1i(8); // 某次迭代中，参考帧的特征点坐标
    vector<cv::Point2f> vPn2i(8); // 某次迭代中，当前帧的特征点坐标
    cv::Mat F21i;   //  某次迭代中，计算的基础矩阵

    // 每次RANSAC记录的Inliers与得分
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;


    // 下面进行每次的RANSAC迭代  mMaxIterations = 200 随机的8点对
    for(int it=0; it<mMaxIterations; it++)
    {

        // Step 2 选择8个归一化之后的点对进行迭代
        for(int j=0; j<8; j++)
        {
            int idx = mvSets[it][j];

            // vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
            // 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
            vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存储在参考帧1中的特征点索引
            vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存储在参考帧1中的特征点索引
        }

        // Step 3 八点法计算基础矩阵
        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);

        // 基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标    
        // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  
        // 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   
        // 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0
        F21i = T2t*Fn*T1;

        // Step 4 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

        // Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记
        if(currentScore>score)
        {
            //如果当前的结果得分更高，那么就更新最优计算结果
            F21 = F21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

3.1 使用八点法求基础矩阵F

3.1.1 数学推导

​ 计算基础矩阵F最少只需要7对匹配点。至少需要多少对匹配点都是根据它的自由度来计算的，基础矩阵F虽然是7自由度（去除尺度等价性这一个自由度，秩为2，再减去1个自由度），但一对匹配点只能提供1个约束方程。但是在ORB-SLAM2中为了求解方便，统一使用8对点。下面分析其求解的数学原理

​ 特征匹配点${\mathbf{p}}_1=[u_1,v_1,1{]}^{\top },{\mathbf{p}}_2=[u_2,v_2,1{]}^{\top }$，直接给出基础矩阵F描述特征点对之间的变换关系如下，从这里能看出，基础矩阵不满秩(秩为2)
$$p_2^{\top }{F}_{21}{p}_1=0$$
​ 展开，写为矩阵形式
$$\left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]\underset{F_{21}}{\underbrace{\left[\begin{array}{ccc}{f}_1& f_2& f_3\\ f_4& f_5& f_6\\ f_7& f_8& f_9\end{array}\right]}}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
​ 将前两个矩阵乘积表示为$\left[\begin{array}{ccc}a& b& c\end{array}\right]$，则知
$$\begin{array}{c}a=f_1{u}_2+f_4{v}_2+f_7\\ b=f_2{u}_2+f_5{v}_2+f_8\\ c=f_3{u}_2+f_6{v}_2+f_9\end{array}$$
​ 矩阵简化为：
$$\left[\begin{array}{ccc}a& b& c\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
​ 相乘后展开
$$a{u}_1+b{v}_1+c=0$$

$$f_1{u}_1{u}_2+f_2{v}_1{u}_2+f_3{u}_2+f_4{u}_1{v}_2+f_5{v}_1{v}_2+f_6{v}_2+f_7{u}_1+f_8{v}_1+f_9=0$$

​ 也就是说，我们可以从一个特征点对得到一个上面的等式，写成矩阵形式如下
$$\left[\begin{array}{ccccccccc}{u}_1{u}_2& v_1{u}_2& u_2& u_1{v}_2& v_1{v}_2& v_2& u_1& v_1& 1\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\\ f_5\\ f_6\\ f_7\\ f_8\\ f_9\end{array}\right]=0$$
​ 假设我们有多对特征点对，那么就可以得到一个矩阵$A_{8\times 9}$，进而求出$f_{9\times 1}$
$$Af=0$$
​ 利用SVD方法求解基础矩阵(方法和求解单应矩阵类似，构建二乘法，这里不具体写了)
$$A=UD{V}^{\top }$$
​ 因为使用8对点求解，因此$A_{8\times 9}$。SVD分解后，$U$是左奇异向量，它是一个8×8的正交矩阵；$V$是右奇异向量，它是一个9×9的正交矩阵，其中$V^T$是$V$的转置；$D$是一个8×9的对角矩阵，除了对角线，其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照从大到小的顺序降序排列的。

​ 因为每个奇异值都是一个残差项，所以最后一个奇异值最小，对应的残差也最小。因此最后一个奇异值对应的奇异向量就是最优解。

​ $V$中的每个列向量对应$D$中的每个奇异值，最小二乘最优解就是$V^T$对应的第9个行向量，也就是基础矩阵F的元素(把行向量转换为$3\ast 3$的矩阵就是$F$)。这里我们先记作$F_{pre}$,因为它还不是最终的$F$。

​ 基础矩阵$F$有一个很重要的性质，就是秩为$2$，利用这个约束可以进一步求解准确的$F$。使用上面的方法构造的$F_{pre}$不能保证满足秩为$2$的约束，我们需要进行第二次$SVD$分解：
$$F_{\mathrm{pre}}=U_{\mathrm{pre}}{D}_{\mathrm{pre}}{V}_{\mathrm{pre}}^{\top }=U_{\mathrm{pre}}\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right]V_{\mathrm{pre}}^{\top }$$
​ 为了保证最终的基础矩阵秩为2，我们强制将最小的奇异值置为0，如下所示：

$$F=U_{\mathrm{pre}}{D}_{\mathrm{pre}}{V}_{\mathrm{pre}}^{\top }=U_{\mathrm{pre}}\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& 0\end{array}\right]V_{\mathrm{pre}}^{\top }$$

3.2 代码分析

/**
 * @brief 根据特征点匹配求fundamental matrix（normalized 8点法）
 * 注意F矩阵有秩为2的约束，所以需要两次SVD分解
 * 
 * @param[in] vP1           参考帧中归一化后的特征点
 * @param[in] vP2           当前帧中归一化后的特征点
 * @return cv::Mat          最后计算得到的基础矩阵F
 */
cv::Mat Initializer::ComputeF21(
    const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
    const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{

    //获取参与计算的特征点对数
    const int N = vP1.size();

    //初始化A矩阵
    cv::Mat A(N,9,CV_32F); // N*9维  8*9

    // 构造矩阵A，将每个特征点添加到矩阵A中的元素
    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.at<float>(i,0) = u2*u1;
        A.at<float>(i,1) = u2*v1;
        A.at<float>(i,2) = u2;
        A.at<float>(i,3) = v2*u1;
        A.at<float>(i,4) = v2*v1;
        A.at<float>(i,5) = v2;
        A.at<float>(i,6) = u1;
        A.at<float>(i,7) = v1;
        A.at<float>(i,8) = 1;
    }

    //存储奇异值分解结果的变量
    cv::Mat u,w,vt;

    
    // 定义输出变量，u是左边的正交矩阵U， w为对角矩阵(奇异)，vt中的t表示是右正交矩阵V的转置
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    // 转换成基础矩阵的形式
    cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列

    //基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2
    // 对初步得来的基础矩阵进行第2次奇异值分解
    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    // 秩2约束，强制将第3个奇异值设置为0
    w.at<float>(2)=0; 
    
    // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 即F
    return  u*cv::Mat::diag(w)*vt;
}

3.2 基础矩阵F重投影评分

3.2.1 理论分析

​ 使用已经判断为内点的特征点到对应极线的距离来衡量当前基础矩阵的好坏。说白了就是满足下面这个式子，理论上它是0，实际求出来的有误差。
$$p_2^{\top }{F}_{21}{p}_1=0$$
​ 极线定义
$$\begin{array}{rl}{l}_2& ={\mathbf{F}}_{21}{\mathbf{p}}_1={\left[\begin{array}{ccc}{a}_2& b_2& c_2\end{array}\right]}^{\top }\\ l_1& ={{\mathbf{p}}_2}^{\top }{\mathbf{F}}_{21}=\left[\begin{array}{ccc}{a}_1& b_1& c_1\end{array}\right]\end{array}$$

​ 误差定义为点到对应极线的距离(点到直线距离公式)，按理想情况来讲，这两个像素点都是极线的端点，那么它们到极线的距离就应该是0！所以像素点$p_1$到极线$l_1$的距离误差大小决定了基础矩阵的好坏！！
$$\begin{array}{rl}{e}_1& =\frac{l_1{\mathbf{p}}_1}{{\sigma }^2\sqrt{a_1^2+b_1^2}}=\frac{a_1{u}_1+b_1{v}_1+c_1}{{\sigma }^2\sqrt{a_1^2+b_1^2}}\\ e_2& =\frac{p_2^{\top }{\mathbf{l}}_2}{{\sigma }^2\sqrt{a_2^2+b_2^2}}=\frac{a_2{u}_2+b_2{v}_2+c_2}{{\sigma }^2\sqrt{a_2^2+b_2^2}}\end{array}$$
​ $th$表示自由度为1的卡方分布在显著性水平为0.05时对应的临界阈值；$thScore$表示自由度为2的卡方分布在显著性水平为0.05时对应的临界阈值。
$$\begin{array}{rl}scor{e}_1& =\sum _{i=1}^N\left(\mathrm{thScore}-e_1(i)\right),e_1(i)<\mathrm{th}\\ scor{e}_2& =\sum _{i=1}^N\left(\text{thScore}-e_2(i)\right),e_2(i)\text{<th}\\ \text{score}& ={\mathrm{score}}_1+{\mathrm{score}}_2\end{array}$$

3.2.2 代码分析

/**
 * @brief 对给定的Fundamental matrix打分
 * 
 * @param[in] F21                       当前帧和参考帧之间的基础矩阵
 * @param[in] vbMatchesInliers          匹配的特征点对属于inliers的标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckFundamental(
    const cv::Mat &F21,             //当前帧和参考帧之间的基础矩阵
    vector<bool> &vbMatchesInliers, //匹配的特征点对属于inliers的标记
    float sigma)                    //方差
{

    // 获取匹配的特征点对的总对数
    const int N = mvMatches12.size();

    // Step 1 提取基础矩阵中的元素数据3*3
    const float f11 = F21.at<float>(0,0);
    const float f12 = F21.at<float>(0,1);
    const float f13 = F21.at<float>(0,2);
    const float f21 = F21.at<float>(1,0);
    const float f22 = F21.at<float>(1,1);
    const float f23 = F21.at<float>(1,2);
    const float f31 = F21.at<float>(2,0);
    const float f32 = F21.at<float>(2,1);
    const float f33 = F21.at<float>(2,2);

    // 预分配空间
    vbMatchesInliers.resize(N);

    // 设置评分初始值（因为后面需要进行这个数值的累计）
    float score = 0;

    // 基于卡方检验计算出的阈值
    // 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 3.841;

    // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float thScore = 5.991;

    // 信息矩阵，或 协方差矩阵的逆矩阵
    const float invSigmaSquare = 1.0/(sigma*sigma);


    // Step 2 计算img1 和 img2 在估计 F 时的score值
    for(int i=0; i<N; i++)
    {
        //默认为这对特征点是Inliers
        bool bIn = true;

        // Step 2.1 提取参考帧和当前帧之间的特征匹配点对
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];

        // 提取出特征点的坐标
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        // Reprojection error in second image
        // Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (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;
    
        // Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)
        const float num2 = a2*u2+b2*v2+c2;
        const float squareDist1 = num2*num2/(a2*a2+b2*b2);
        // 带权重误差
        const float chiSquare1 = squareDist1*invSigmaSquare;
        
        // Step 2.4 误差大于阈值就说明这个点是Outlier 
        if(chiSquare1>th)
            bIn = false;
        else
            // 误差越大，得分越低
            score += thScore - chiSquare1;

        // 计算img2上的点在 img1 上投影得到的极线 l1= p2 * F21 = (a1,b1,c1)
        const float a1 = f11*u2+f21*v2+f31;
        const float b1 = f12*u2+f22*v2+f32;
        const float c1 = f13*u2+f23*v2+f33;

        // 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)
        const float num1 = a1*u1+b1*v1+c1;
        const float squareDist2 = num1*num1/(a1*a1+b1*b1);

        // 带权重误差
        const float chiSquare2 = squareDist2*invSigmaSquare;

        // 误差大于阈值就说明这个点是Outlier 
        if(chiSquare2>th)
            bIn = false;
        else
            score += thScore - chiSquare2;
        
        // Step 2.5 保存结果
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    //  返回评分
    return score;
}

4 位姿恢复

// Step 4 计算得分比例来判断选取哪个模型来求位姿R,t
// 通过这个规则来判断谁的评分占比更多一些
float RH = SH/(SH+SF);	
	RH>0.40 // 选择单应矩阵，更倾向于平面
    RH<0.40	// 选择基础矩阵，倾向于非平面

4.1 利用单应矩阵H恢复位姿

4.1.1 理论推导

​ 推导参考论文 Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988

​ 我们在上面2.2.1小节利用位姿变换推出了单应矩阵$H_{21}$，这里我们要反过来求解位姿$R，t$
$$\begin{array}{rl}{p}_2& \simeq \mathbf{K}({\mathbf{R}}_{21}\mathbf{P}+\mathbf{t})\\ & \simeq K\left(R_{21}P+\mathbf{t}\cdot (-\frac{{\mathbf{n}}^{\mathrm{T}}\mathbf{P}}{d})\right)\\ & \simeq K\left(R_{21}-\frac{t{\mathbf{n}}^{\mathrm{T}}}{d}\right)\mathbf{P}\\ & \simeq K\left(R_{21}-\frac{t{n}^{\mathrm{T}}}{d}\right)K^{-1}{\mathbf{p}}_1\\ & \simeq {\mathbf{H}}_{21}{\mathbf{p}}_1\end{array}$$
所以我们可以得到
$${\mathbf{H}}_{21}=K\left(R_{21}-\frac{t{n}^{\mathrm{T}}}{d}\right)K^{-1}$$
即
$$A=\left(R_{21}-\frac{t{n}^{\mathrm{T}}}{d}\right)=K^{-1}{\mathbf{H}}_{21}K$$
然后对$A$进行SVD分解
$$A=UD{V}^{\top }$$

对角矩阵$D$有三个奇异值$d_1>=d_2>=d_3$
$$D=d^{\prime }{R}^{\prime }+t^{\prime }{n^{\prime }}^t=\left(\begin{array}{ccc}{d}_1& 0& 0\\ 0& d_2& 0\\ 0& 0& d_3\end{array}\right)$$
然后利用SVD的分解结果，论文中公式(8)给出了位姿的求解方式
$$\{\begin{array}{l}\mathbf{R}=s\mathbf{U}{\mathbf{R}}^{\prime }{\mathbf{V}}^t\\ \mathbf{t}=\mathbf{U}{\mathbf{t}}^{\prime }\\ \mathbf{n}=\mathbf{V}{\mathbf{n}}^{\prime }\\ d=s{d}^{\prime }\\ s=\det U\det V\end{array}(8)$$
注意这里$d$就是奇异值向量，然后通过s来判断$d^{\prime }$的正负来确定$R^{\prime }$等的具体计算方式

​ 下面是中间变量的计算方法，因为${\epsilon }_1,{\epsilon }_3=\pm 1$，所以如果要把$x_1,x_3$进行组合，那么一共2*2=4种组合方式。

float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));	// 写为代码形式
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};// aux1和aux3 aux1和-aux3 -aux1和aux3 -aux1和-aux3
float x3[] = {aux3,-aux3,aux3,-aux3};   // 4种组合 

$$\{\begin{array}{llc}{x}_1& =& {\epsilon }_1\sqrt{\frac{d_1^2-d_2^2}{d_1^2-d_3^2}}\\ x_2& =& 0\\ x_3& =& {\epsilon }_3\sqrt{\frac{d_2^2-d_3^2}{d_1^2-d_3^2}}\end{array}{\epsilon }_1,{\epsilon }_3=\pm 1(12)$$

① $d^{\prime }>0$情况

• 计算$R^{\prime }$

$$R^{\prime }=\left(\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right)$$

其中$\sin \theta ,\cos \theta$计算方式如下，可以看出$R^{\prime }$有两种
$$\{\begin{array}{llccc}\sin \theta & =& (d_1-d_3)\frac{x_1{x}_3}{d_2}& =& {\epsilon }_1{\epsilon }_3\frac{\sqrt{(d_1^2-d_2^2)(d_2^2-d_3^2)}}{(d_1+d_3)d_2}\\ \cos \theta & =& \frac{d_1{x}_3^2+d_3{x}_1^2}{d_2}& =& \frac{d_2^2+d_1{d}_3}{(d_1+d_3)d_2}\end{array}{\epsilon }_1,{\epsilon }_3=\pm 1(13)$$

• 计算$t^{\prime }$

$t^{\prime }$则是4种(上面的x有4种搭配)
$$t^{\prime }=(d_1-d_3)\left(\begin{array}{c}{x}_1\\ 0\\ -x_3\end{array}\right)=(d_1-d_3)n^{\prime }$$
​ 所以在 $d^{\prime }>0$的情况下，根据不同的$\epsilon$组合所得出来的4种$R^{\prime },t^{\prime }$的解，把这些解带入公式(8)即可得到我们要求的位姿$R,t$

② $d^{\prime }<0$情况

​ 原理和上面一致，不再分析
$$R^{\prime }=\left(\begin{array}{rcl}\cos \phi & 0& \sin \phi \\ 0& -1& 0\\ \sin \phi & 0& -\cos \phi \end{array}\right)$$

$$\begin{array}{rlr}& & \\ \{\begin{array}{rclcc}\sin \phi & =& \frac{d_1+d_3}{d_2}{x}_1{x}_3& =& {\epsilon }_1{\epsilon }_3\frac{\sqrt{(d_1^2-d_2^2)(d_2^2-d_3^2)}}{(d_1-d_3)d_2}\\ \cos \phi & =& \frac{d_3{x}_1^2-d_1{x}_3^2}{d_2}& =& \frac{d_1{d}_3-d_2^2}{(d_1-d_3)d_2}\end{array}& & (15)\end{array}$$

$$t^{\prime }=(d_1+d_3)\left(\begin{array}{c}{x}_1\\ 0\\ x_3\end{array}\right)=(d_1+d_3)n^{\prime }$$

4.1.2 代码分析

/**
 * @brief 用H矩阵恢复R, t和三维点
 * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
 * 代码使用了Faugeras SVD-based decomposition算法，参考文献
 * Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988 
 * 
 * @param[in] vbMatchesInliers          匹配点对的内点标记
 * @param[in] H21                       从参考帧到当前帧的单应矩阵
 * @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::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
     // 统计匹配的特征点对中属于内点(Inlier)或有效点个数
    int N=0;
    for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
        if(vbMatchesInliers[i])
            N++;


    // 参考SLAM十四讲第二版p170-p171
    // H = K * (R - t * n / d) * K_inv
    // 其中: K表示内参数矩阵
    //       K_inv 表示内参数矩阵的逆
    //       R 和 t 表示旋转和平移向量
    //       n 表示平面法向量
    //       d是深度
    // 令 H = K * A * K_inv
    // 则 A = k_inv * H * k
    cv::Mat invK = K.inv();
    cv::Mat A = invK*H21*K;


    // 对矩阵A进行SVD分解
    // A 等待被进行奇异值分解的矩阵
    // w 奇异值矩阵
    // U 奇异值分解左矩阵
    // Vt 奇异值分解右矩阵，注意函数返回的是转置
    // cv::SVD::FULL_UV 全部分解
    // A = U * w * Vt
    cv::Mat U,w,Vt,V;
    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
    V=Vt.t();

    // 计算变量s = det(U) * det(V)
    // 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt)  行列式
    float s = cv::determinant(U)*cv::determinant(Vt);

    // 获取矩阵的奇异值 3*3的矩阵即有3个奇异值
    float d1 = w.at<float>(0);
    float d2 = w.at<float>(1);
    float d3 = w.at<float>(2);

    // SVD分解正常情况下奇异值di应该是正的，且满足d1>=d2>=d3
    if(d1/d2<1.00001 || d2/d3<1.00001)
    {
        return false;
    }


    // 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算
    // 定义8中情况下的旋转矩阵、平移向量和空间向量
    vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);
    vt.reserve(8);
    vn.reserve(8);


    // Step 1.1 讨论 d' > 0 时的 4 组解
    // 根据论文eq.(12)有
    // x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
    // x2 = 0
    // x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))
    // 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))
    //    aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))
    // 则
    // x1 = e1 * aux1
    // x3 = e3 * aux2

    // 因为 e1,e2,e3 = 1 or -1
    // 所以有x1和x3有2*2=4种组合
    // x1 =  {aux1,aux1,-aux1,-aux1}
    // x3 =  {aux3,-aux3,aux3,-aux3}


    // 根据论文eq.(13)有
    // sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2
    // cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 
    // 令  aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)
    // 则  sin(theta) = e1 * e3 * aux_stheta
    //     cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)
    // 因为 e1 e2 e3 = 1 or -1
    // 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}
    //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
    float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
    float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
    float x1[] = {aux1,aux1,-aux1,-aux1};
    float x3[] = {aux3,-aux3,aux3,-aux3};   // 4种组合

    // case d'=d2
    float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);

    float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);  // cos(theta)
    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};    // 又是4种组合



    // 计算旋转矩阵 R'
    //根据不同的e1 e3组合所得出来的四种R t的解
    //      | ctheta      0   -aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0    aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       | aux3|

    //      | ctheta      0    aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0   -aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       | aux3|
    // 开始遍历这四种情况中的每一种
    for(int i=0; i<4; i++)
    {
        // 生成Rp，就是eq.(13) 的 R'   p表示就是'
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=ctheta;
        Rp.at<float>(0,2)=-stheta[i];
        Rp.at<float>(2,0)=stheta[i];
        Rp.at<float>(2,2)=ctheta;

         // eq.(8) 计算R
        cv::Mat R = s*U*Rp*Vt;
        vR.push_back(R);     // 保存

        // eq. (14) 生成tp  
        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=-x3[i];
        tp*=d1-d3;

        // eq.(8) 计算t
        cv::Mat t = U*tp;
        // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
        // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
        vt.push_back(t/cv::norm(t));

        // 构造法向量np  eq. (14)上下
        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        // eq.(8) 计算法向量n
        cv::Mat n = V*np;
        if(n.at<float>(2)<0)
            n=-n;   // 保持平面法向量向上
        vn.push_back(n);
    }

    // Step 1.2 讨论 d' < 0 时的 4 组解
    float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
    // cos_theta项
    float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
    // 考虑到e1,e2的取值，这里的sin_theta有两种可能的解
    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};

    // 对于每种由e1 e3取值的组合而形成的四种解的情况
    for(int i=0; i<4; i++)
    {
        // 计算旋转矩阵 R'
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=cphi;
        Rp.at<float>(0,2)=sphi[i];
        Rp.at<float>(1,1)=-1;
        Rp.at<float>(2,0)=sphi[i];
        Rp.at<float>(2,2)=-cphi;

        // 恢复出原来的R
        cv::Mat R = s*U*Rp*Vt;
        // 然后添加到vector中
        vR.push_back(R);

        // 构造tp
        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=x3[i];
        tp*=d1+d3;

        // 恢复出原来的t
        cv::Mat t = U*tp;
        // 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的
        vt.push_back(t/cv::norm(t));

        // 构造法向量np
        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        // 恢复出原来的法向量
        cv::Mat n = V*np;
        // 保证法向量指向上方
        if(n.at<float>(2)<0)
            n=-n;
        // 添加到vector中
        vn.push_back(n);
    }


    int bestGood = 0; // 最好的good点
   
    int secondBestGood = 0;  // 其次最好的good点   
    
    int bestSolutionIdx = -1; // 最好的解的索引，初始值为-1
    
    float bestParallax = -1; // 最大的视差角
    // 存储最好解对应的，对特征点对进行三角化测量的结果
    vector<cv::Point3f> bestP3D;
    // 最佳解所对应的，那些可以被三角化测量的点的标记
    vector<bool> bestTriangulated;



    // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
    
     // Step 2. 对 8组解(上面求得R每一个都是4个解，2*4=8)进行验证，并选择产生相机前方最多3D点的解为最优解
    for(size_t i=0; i<8; i++)
    {
        // 第i组解对应的比较大的视差角
        float parallaxi;
        // 三角化测量之后的特征点的空间坐标
        vector<cv::Point3f> vP3Di;
        // 特征点对是否被三角化的标记
        vector<bool> vbTriangulatedi;
    
        // 调用 Initializer::CheckRT(), 计算good点的数目
        int nGood = CheckRT(vR[i],vt[i],                    //当前组解的旋转矩阵和平移向量
                            mvKeys1,mvKeys2,                //特征点
                            mvMatches12,vbMatchesInliers,   //特征匹配关系以及Inlier标记
                            K,                              //相机的内参数矩阵
                            vP3Di,                          //存储三角化测量之后的特征点空间坐标的
                            4.0*mSigma2,                    //三角化过程中允许的最大重投影误差
                            vbTriangulatedi,                //特征点是否被成功进行三角测量的标记
                            parallaxi);                     // 这组解在三角化测量的时候的比较大的视差角
        
        // 更新历史最优和次优的解
        // 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出
        if(nGood>bestGood)
        {
            // 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优
            secondBestGood = bestGood;
            // 更新历史最优点
            bestGood = nGood;
            // 最优解的组索引为i（就是当前次遍历）
            bestSolutionIdx = i;
            // 更新变量
            bestParallax = parallaxi;
            bestP3D = vP3Di;
            bestTriangulated = vbTriangulatedi;
        }
        // 如果当前组的good计数小于历史最优但却大于历史次优
        else if(nGood>secondBestGood)
        {
            // 说明当前组解是历史次优点，更新之
            secondBestGood = nGood;
        }
    }



    // Step 3 选择最优解。要满足下面的四个条件
    // 1. good点数最优解明显大于次优解，这里取0.75经验值
    // 2. 视角差大于规定的阈值
    // 3. good点数要大于规定的最小的被三角化的点数量
    // 4. good数要足够多，达到总数的90%以上
    if(secondBestGood<0.75*bestGood &&      
       bestParallax>=minParallax &&
       bestGood>minTriangulated && 
       bestGood>0.9*N)
    {
        // 从最佳的解的索引访问到R，t
        vR[bestSolutionIdx].copyTo(R21);
        vt[bestSolutionIdx].copyTo(t21);
        // 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用
        vP3D = bestP3D;
        // 获取特征点的被成功进行三角化的标记
        vbTriangulated = bestTriangulated;

        //返回真，找到了最好的解
        return true;
    }
    return false;
}

4.2 利用基础矩阵F恢复位姿

4.2.1 数学推导—本质矩阵E

​ 引入本质矩阵$E$
$$E=K^{\mathrm{T}}{F}_{21}K$$
​ 对本质矩阵进行SVD分解
$$E=U\Sigma V^{\mathrm{T}}$$
${\mathbf{R}}_Z(\frac{\pi }{2})$表示沿着Z轴旋转90°得到的旋转矩阵。基础矩阵会分出四种位姿解如下：
$$\begin{array}{rl}& \text{t1}={\mathbf{UR}}_Z(\frac{\pi }{2}){\mathbf{\Sigma }\mathbf{U}}^{\mathrm{T}},{\mathbf{R}}_1={\mathbf{UR}}_Z^{\mathrm{T}}(\frac{\pi }{2}){\mathbf{V}}^{\mathrm{T}}\\ & t_2={\mathbf{UR}}_Z(-\frac{\pi }{2}){\mathbf{\Sigma }\mathbf{U}}^{\mathrm{T}},{\mathbf{R}}_2={\mathbf{UR}}_Z^{\mathrm{T}}(-\frac{\pi }{2}){\mathbf{V}}^{\mathrm{T}}.\end{array}$$
这里四种解可以简单理解：有两个点，有4种组合，两个点都在相机前面，两个点都在相机后面，两个点一前一后。

    //                          |0 -1  0|
    // E = U w V'       let W = |1  0  0|
    //                          |0  0  1|
	// 参考计算机视觉中的多视图几何200页结论
// 

4.2.2 代码分析

① 分解本质矩阵E

/**
 * @brief 分解Essential矩阵得到R,t
 * 分解E矩阵将得到4组解，这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t]
 * 参考：Multiple View Geometry in Computer Vision - Result 9.19 p259
 * @param[in] E                 本质矩阵
 * @param[in & out] R1          旋转矩阵1
 * @param[in & out] R2          旋转矩阵2
 * @param[in & out] t           平移向量，另外一个取相反数
 */
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{

    cv::Mat u,w,vt;
    //对本质矩阵进行奇异值分解
    cv::SVD::compute(E,w,u,vt);

    // 左奇异值矩阵U的最后一列就是t，对其进行归一化
    u.col(2).copyTo(t);
    t=t/cv::norm(t);    // 求出t

    // 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt
    cv::Mat W(3,3,CV_32F,cv::Scalar(0));
    W.at<float>(0,1)=-1;
    W.at<float>(1,0)=1;
    W.at<float>(2,2)=1;

    //计算
    R1 = u*W*vt;
    // 旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反
    if(cv::determinant(R1)<0) 
        R1=-R1;

    // 同理将矩阵W取转置来按照相同的公式计算旋转矩阵R2 = u*W.t()*vt

    R2 = u*W.t()*vt;
    // 旋转矩阵有行列式为1的约束
    if(cv::determinant(R2)<0)
        R2=-R2;
}

② 基础矩阵F恢复位姿

/**
 * @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<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 = -U3
    DecomposeE(E21,R1,R2,t);  
    cv::Mat t1=t;
    cv::Mat t2=-t;

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

    // 定义四组解分别对同一匹配点集的有效三角化结果，True or False
    vector<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和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(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;
}

5 初始化失败

5.1 原因1

​ 在KITTI数据01序列测试时，就会发现一开始都是下面这种图，这说明ORB系统初始化失败，这可能是由于这个序列数据在一开始的旋转比较大，导致特征匹配以及三角化的效果不是很好，达不到初始化的条件！（07序列一开始也是旋转）。

​ 这里在补充下为什么特征匹配效果差，对于ORB，专门写了初始化特征匹配，这会使得它在第一帧图像的一个方形（或圆形）中寻找匹配，但是运动速度快，变化大，就很有可能匹配失败！！！这个和基础的光流匹配是一个道理！ 如果相机运动较快，两张图像差异较明显，我们若还是按单层光流法取初始值，很容易达到一个局部极小值。所以说，换一种匹配方式可能对于这种情况更合适。

​ 更差的情况，即下面所示。假如第一次初始化，那么第一帧就是定义的初始化帧mInitialFrame，后续的帧如第2，3等都是要与初始化帧进行特征匹配！如果一直初始化失败，那么它们的差异就会不断变大，直到匹配数小于阈值100！

​ 一旦小于阈值，初始化类指针就会被delete，那么下一帧又会重新创建初始化类指针mpInitializer。这样保证了初始化的合理旋转，总不至于在原点一直等着，那样永远无法初始化！

​ 所以直接跑这个序列就会出现这种情况，但是旋转角度一小，即开始走直线这种（左右偏差较小），它的初始化就可以成功！所以如果是单目的话，初始化一定不能让其旋转很大角度！

5.2 原因2

​ 纯旋转情况，平移量t为0！参考十四讲176页。