vio前端图像匹配与对极几何

目录
1.特征点1.1什么是特征点？特征点有什么用处？特征点都有哪些？1.2ORB特征点，FAST关键点，BRIEF描述子，特征匹配（旋转不变性与尺度不变性）
2.利用opencv手写ORB特征代码讲解理论内容
3.对极几何3.1本质矩阵、基本矩阵的推导和原理3.2单应矩阵的推导和原理实践内容
4.对极约束求解相机运动代码讲解扩充的内容实践内容
5.ORB-SLAM2的特征提取与匹配策略

一、特征点
人眼如何识别图像？

简单来说，人眼看到的物体是由于物体发射的光或者物体反射的光。这些光被人眼所感知，所以形成了图像。
比如说，这张图片为例，我们用肉眼如何判断这两张图片是不是一样呢，或者说是不是有一部分的信息是相同的？
我们大概应该是这样的步骤：
首先我们判断这张图片的大小，这张图片是不是一样大小？
这张图片的颜色是不是一样的？图片中都包含了哪些成分？这些成分的结构、边缘是什么样子的？
在了解基本的信息之后发现好像差不多一样。
然后我们就会进一步观察更细致的成分，对图片中的内容做出人类的理解，这些信息形成独特的编码存放在我们的大脑中。例如电脑后面的黄色瓶子两张图片是不一样的，左上角的门是不一样的，最上面黑色背景的，面积是不一样的。因此我们可以判断出两张图片不是相同的，但是有部分的信息是重叠的。
计算机是如何识别图像的呢？
有一种非常重要的方法就是利用特征点信息。
1.1 什么是特征点？特征点有什么用处？特征点都有哪些？
定义：简单来说，特征点就是具有特征的点，是图像里一些特别的地方。比如图像中的一个像素，这个像素具有颜色，颜色就是这个像素点的特征。特征是图像信息的另一种数字表达形式，在上面的两张图像中，如何比较两张是不是一张照片。直白的来说，因为每个像素点都有颜色，那我们就可以逐点对比两张图片每个像素点的颜色。如果有一个颜色不一样，那我们就说这两张图片是不一样的。
但是由于光照的不同，例如早上和中午的光照是不一样的，对同一个物体在同一个位置的拍摄，如果只比较像素的颜色，那他们就会判断成为是不一样的图像。所以为了让特征点更加具有鲁棒性。我们希望特征在光照变化，物体形变或者由于一些动态物体的遮挡，仍然能够有效的判断两张图片的信息。于是就有了一些提取特征点的不同的算法。
具体来说，特征点是由关键点和描述子两部分组成的。当我们说“在一张图像中计算SIFT特征点”时，是指“提取SIFT关键点，并计算SIFT描述子”两件事情。关键点是指该特征点在图像中的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此。只要两个特征点的描述子在向量空间上的距离相近就可以认为它们是同样的特征点。
用处：最直观的感受是，通过对比两张图片的关键点与其描述子，我们可以判断这两张图片是否相同。在vSLAM中，前端的核心任务是如何根据图像估计相机运动。然而图像本身是一个由亮度和色彩组成的矩阵。如果直接从矩阵层面考虑这些估计，将会变的非常困难。所以我们会采取用特征点的方式，进行图像间的匹配与分析，从而估计相机位置的相对运动。这些特征点就是SLAM中地标点在相机平面上的投影点。

计算特征点方法的分类：Harris、FAST、SIFT、SUBF、ORB等、以及利用深度学习计算特征点
Harris、FAST角点早起提出的版本不具有旋转不变性与尺度不变性。SIFT方法速度太慢，SUBF在SIFT基础上提出了加速方法，但是仍然不够快。在质量和性能的综合衡量下，ORB是一种比较好的折中方法。
1.2 ORB特征点
ORB特征由关键点和描述子两部分组成，它的关键点称为“Oriented FAST”，是一种改进的FAST角点。它的描述子称为BRIEF。ORB特征的提取分为以下两个步骤：
1）FAST角点提取：找出图像中的“角点”。之前的FAST角点并不具有方向，在这里，ORB计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变特性。
2）BRIEF描述子：对前一步提取出特征点的周围图像区域进行描述。之前的BRIEF不具有方向不变性，这里ORB作了改进，利用了之前计算的方向信息。
1.3 FAST关键点
FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。他的思想是：如果一个像素与邻域的像素差别较大（过亮或者过暗），那么它更可能是角点。相比于其他的角点检测算法，FAST只需比较像素亮度的大小。
它的步骤如下以FAST12为例：

1. 在图像中选取像素p，假设它的亮度为Ip
   2）设置一个阈值T，比如Ip的20%。
   3）以像素p为中心，选取半径为3的圆上的16个像素点。
   4）假设圆上连续12个点的值大于Ip+T，或者小于lp-T，该像素点就可以认为是特征点。
   5）对每一个像素重复以上的操作。

但是，由于尺度不确定性与旋转不确定性的存在，例如，小毛驴的图片发生了偏转，或者拍照小毛驴的位置变远了。

如何解决？
针对尺度不一->建立图像金字塔

针对旋转->灰度质心法

1.4 BRIEF描述子
在提取Oriented FAST关键点后，我们对每个点计算其描述子。ORB使用改进的BRIEF特征描述。
BRIEF是一种二进制描述子，其描述响亮通常由很多个0和1组成，这里的0和1编码了关键点附近两个随机像素（比如p和q）的大小关系：如果p比q大，则取1，反之就取0。
如果我们取128对这样的点，那我们最后就会得到128维由0、1组成的向量。
BRIEF的优点：使用随机点的比较，速度非常快，而且由于使用二进制表达，存储起来也非常方便，适用于实时的图像匹配。
原始的BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而ORB在FAST特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转后的“steer BRIEF”特征使得ORB描述子具有较好的旋转不变性。
1.5 特征匹配
现在我们知道每一张图片的关键点与描述子，那我们如何进行图像间的匹配，从而估计相机的运动呢？

1）首先对两张图片分别计算Oriented FAST关键点
2）对每一个关键点画一个patch，本例子为圆
3）根据定义的描述子，计算每一个关键点的描述子
4）通过描述子的计算进行匹配

如何匹配？
最直白的方法就是通过暴力匹配，例如用第一张图片的第一个关键点的描述子与第二张图片所有关键点的描述子进行匹配。分别计算两个描述子的汉明距离，计算不同位数的个数，不同的越少越好。
但是利用暴力匹配运算量非常庞大，特别是想要匹配一帧和一个地图的时候。这不符合SLAM要求的实时性。
所以我们可以利用FLANN等策略来加速匹配。

https://blog.csdn.net/yangtrees/article/details/7482960

二、利用opencv提取ORB特征与手写ORB特征代码讲解
2.1利用opencv提取ORB特征

#include <iostream>
#include <opencv2/core/core.hpp> 
#include <opencv2/features2d/features2d.hpp>  //关于特征点提取检测等的头文件
#include <opencv2/highgui/highgui.hpp>  //关于图像的头文件
#include <chrono>

using namespace std;


int main(int argc, char ** argv){
    //判断输入的是否是三个参数
    if(argc != 3){
        cerr<<"usage: feature_extration img1 img2"<<endl;
        return -1;
    }
    //读取图像
    cv::Mat image_1 = cv::imread(argv[1],cv::IMREAD_COLOR);
    cv::Mat image_2 = cv::imread(argv[2],cv::IMREAD_COLOR);
    assert(image_1.data != nullptr && image_2.data != nullptr);
    
    //初始化
    std::vector<cv::KeyPoint> KeyPoint_1,KeyPoint_2;
    cv::Mat descriptors_1,descriptors_2;
    //创建ORB特征点与描述子提取器
    //this->
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    // 创建匹配计算类型
    //this->
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    
    //第一步：检测Orirnted FAST角点位置
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    detector->detect(image_1,KeyPoint_1);
    detector->detect(image_2,KeyPoint_2);
    
    //第二步：根据角点位置计算BRIEF描述子
    //this->
    descriptor->compute(image_1,KeyPoint_1,descriptors_1);
    descriptor->compute(image_2,KeyPoint_2,descriptors_2);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"extract ORB cost = "<<time_used.count()<<" seconds. "<<endl;
    
    cv::Mat outimg1;
    cv::drawKeypoints(image_1,KeyPoint_1,outimg1,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT );
    
    cv::imshow("ORB features", outimg1);
    
    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    //Class for matching keypoint descriptors.query descriptor index, train descriptor index, 
    //train image index, and distance between descriptors. 
    std::vector<cv::DMatch> matches;
    t1 = chrono::steady_clock::now();
    //this->
    matcher->match(descriptors_1,descriptors_2,matches);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"match ORB cost = " <<time_used.count()<< " seconds. " <<endl;
    
    //-- 第四步:匹配点对筛选
    // 计算最小距离和最大距离
    //返回一个 std::pair<ForwardIt,ForwardIt> （pair 对）
    auto min_max = std::minmax_element(matches.begin(),matches.end(),[](const cv::DMatch &m1, const cv::DMatch &m2) {return m1.distance < m2.distance;});
    
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;
    
    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);
    
    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    std::vector<cv::DMatch> good_matches;
    for(int i=0;i<descriptors_1.rows;i++){
        //this->
        if(matches[i].distance <= max(2*min_dist, 30.0)){
            good_matches.push_back(matches[i]);
        }
    }
    
    //--第五步，绘制匹配结果
    cv::Mat img_match;
    cv::Mat img_goodmatch;
    cv::drawMatches(image_1,KeyPoint_1,image_2,KeyPoint_2,matches,img_match);
    cv::drawMatches(image_1,KeyPoint_1,image_2,KeyPoint_2,good_matches,img_goodmatch);
    cv::imshow("all matches", img_match);
    cv::imshow("good matches", img_goodmatch);
    
    cv::waitKey(0);
    return 0;
}

2.2手写ORB特征

#include<iostream>
#include<chrono>
#include<opencv2/opencv.hpp>
#include<nmmintrin.h> 
#include<string>

using namespace std;

string first_file = "./1.png";
string second_file = "./2.png";

// 32 bit unsigned int, will have 8, 8x32=256
typedef vector<uint32_t> DescType ;

void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints,vector<DescType> &descriptors);

void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches);

int main(int argc,char **argv){
    //读取图片
    cv::Mat image1 = cv::imread(first_file,0);
    cv::Mat image2 = cv::imread(second_file,0);
    
    assert(image1.data != nullptr && image2.data != nullptr);
    
    
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //定义特征点和描述子向量
    vector<cv::KeyPoint> KeyPoints1;
    vector<cv::KeyPoint> KeyPoints2;
    vector<DescType> descriptors1;
    vector<DescType> descriptors2;
    //计算FAST特征点与描述子
    cv::FAST(image1, KeyPoints1, 40);
    ComputeORB(image1,KeyPoints1,descriptors1);
    cv::FAST(image2, KeyPoints2, 40);
    ComputeORB(image2,KeyPoints2,descriptors2);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
    
    //定义匹配关系
    vector<cv::DMatch> matches;
    t1 = chrono::steady_clock::now();
    //开始匹配
    BfMatch(descriptors1, descriptors2, matches);
    t2 = chrono::steady_clock::now();
    cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
    //匹配成功的数量
    cout << "matches: " << matches.size() << endl;
    
    cv::Mat image_show;
    cv::drawMatches(image1, KeyPoints1, image2, KeyPoints2, matches, image_show);
    cv::imshow("matches", image_show);
    cv::imwrite("matches.png", image_show);
    cv::waitKey(0);
    return 0;

}


// ORB pattern
//确定二进制描述子Rotated BRIEF中每一位上是0还是1时，两个像素点的选择；32*32图像块
int ORB_pattern[256 * 4] = {
  8, -3, 9, 5/*mean (0), correlation (0)*/,
  4, 2, 7, -12/*mean (1.12461e-05), correlation (0.0437584)*/,
  -11, 9, -8, 2/*mean (3.37382e-05), correlation (0.0617409)*/,
  7, -12, 12, -13/*mean (5.62303e-05), correlation (0.0636977)*/,
  2, -13, 2, 12/*mean (0.000134953), correlation (0.085099)*/,
  。。。。
};



void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints,vector<DescType> &descriptors){
    const int half_patch_size = 8;
    const int half_boundary = 16;
    int bad_points = 0;
    for (auto &kp: keypoints) {
        if (kp.pt.x < half_boundary || kp.pt.y < half_boundary ||
            kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary) {
            // outside
            bad_points++;
            descriptors.push_back({});
            continue;
        }

        float m01 = 0, m10 = 0;
        for (int dx = -half_patch_size; dx < half_patch_size; ++dx) {
            for (int dy = -half_patch_size; dy < half_patch_size; ++dy) {
                uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx);
                m10 += dx * pixel;
                m01 += dy * pixel;
            }
        }

        // angle should be arc tan(m01/m10);
        float m_sqrt = sqrt(m01 * m01 + m10 * m10) + 1e-18; // avoid divide by zero
        float sin_theta = m01 / m_sqrt;
        float cos_theta = m10 / m_sqrt;

        // compute the angle of this point
        DescType desc(8, 0);
        for (int i = 0; i < 8; i++) {
        uint32_t d = 0;
            for (int k = 0; k < 32; k++) {
                int idx_pq = i * 32 + k;
                cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
                cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);

                // rotate with theta
                cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y)+ kp.pt;
                cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y) + kp.pt;
                if (img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y, qq.x)) {
                    d |= 1 << k;
                }
            }
            desc[i] = d;
        }
        descriptors.push_back(desc);
    }

    cout << "bad/total: " << bad_points << "/" << keypoints.size() << endl;
}

void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches){
    const int d_max = 40;
    for(size_t i1 = 0;i1< desc1.size();++i1){
        if(desc1[i1].empty()) continue;
        cv::DMatch m{i1, 0, 256};
        for(size_t i2 =0;i2<desc2.size();++i2){
            if(desc2[i2].empty()) continue;
            int distance =0;
            for( int k=0;k<8;k++){
                //Emits the Streaming SIMD Extensions 4 (SSE4) instruction popcnt. This instruction calculates the number of bits of the parameter that are set to 1.
                //异或，相同为0，不同为1
                distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);
            }
            if(distance < d_max &&distance<m.distance){
                m.distance = distance;
                m.trainIdx = i2;
            }
        }
        if(m.distance < d_max){
            matches.push_back(m);
        }
    }
}

三、对极几何

https://blog.csdn.net/weixin_42427882/article/details/112436121

https://baike.baidu.com/item/%E5%B9%B3%E9%9D%A2%E6%96%B9%E7%A8%8B/9949549
Motion and structure from motion in a piecewise plannar environment.International Journal of Pattern Recognition and Artificial Intelligence, 1988

四、2d-2d恢复位姿代码

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>


using namespace std;
using namespace cv;

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    std::vector<KeyPoint> &keypoints_1,
    std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches);

void pose_estimation_2d2d(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat &R, Mat &t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv) {
    if (argc != 3) {
        cout << "usage: pose_estimation_2d2d img1 img2" << endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    //-- 估计两张图像间运动
    Mat R, t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

    //this->
    //-- 验证E=t^R*scale
    Mat t_x =
        (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
        t.at<double>(2, 0), 0, -t.at<double>(0, 0),
        -t.at<double>(1, 0), t.at<double>(0, 0), 0);

    cout << "t^R=" << endl << t_x * R << endl;

    //-- 验证对极约束
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    for (DMatch m: matches) {
        Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
        Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }
    return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++) {
        if (match[i].distance <= max(2 * min_dist, 30.0)) {
        matches.push_back(match[i]);
        }
    }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
    return Point2d
        (
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
    // 相机内参,TUM Freiburg2
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    //-- 把匹配点转换为vector<Point2f>的形式
    vector<Point2f> points1;
    vector<Point2f> points2;

    for (int i = 0; i < (int) matches.size(); i++) {
        points1.push_back(keypoints_1[matches[i].queryIdx].pt);
        points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    }

    //-- 计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = cv::findFundamentalMat(points1, points2, CV_FM_8POINT);
    cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

    //-- 计算本质矩阵
    Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
    double focal_length = 521;      //相机焦距, TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout << "essential_matrix is " << endl << essential_matrix << endl;

    //-- 计算单应矩阵
    //-- 但是本例中场景不是平面，单应矩阵意义不大
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    cout << "homography_matrix is " << endl << homography_matrix << endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    // 此函数仅在Opencv3中提供
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout << "R is " << endl << R << endl;
    cout << "t is " << endl << t << endl;

}

https://zhuanlan.zhihu.com/p/92221963

扩充内容
5.ORB-SLAM2的特征提取与匹配策略
特征提取：
Step 1 检查图像有效性。如果图像为空，那么就直接返回
Step 2 构建图像金字塔，把图像构建成8层，假如说我设置每张图片要提取1000个特征点，那每一层都是按照面积的百分比来分配要检测的特征点数量
Step 3 计算图像的特征点，并且将特征点进行均匀化。均匀的特征点可以提高位姿计算精度
例如，把第一层分成若干个小网格，对每一个网格用FAST算法计算角点，把计算出来的角点放到一个容器中等待分配
Step 4 对待分配的特征点利用四叉树均匀的分布到图像上
Step 5 对每一个特征点计算方向
Step 6 对图像进行高斯模糊，防止噪声的影响
Step 7 计算每一个旋转后特征点的描述子

特征匹配：
通过词袋，对关键帧的特征点进行跟踪

把世界上的每一个人看作一个特征点，假设某个犯罪分子是美国纽约州纽约市的一个人，现在国际刑警要去抓他们，我第一需要找的是美国，第二找的是纽约州，美国和纽约州相当于一个高维的特征。利用高维的特征进行匹配，这个人是不是要找的犯罪分子。
在图像的匹配中，通过训练词典，我们可以得到一个6层的字典树。现在有两幅图片，每一幅图片上面的特征点，都对应着这个树其中的一个word，现在把每一张图片中的第二层node下面的特征点都找出来，因为只有相同node下的特征点才可能有最小的距离，才可以匹配的上。
Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
Step 2：遍历KF中属于该node的特征点
Step 3：遍历F中属于该node的特征点，寻找最佳匹配点
Step 4：根据阈值 和 角度投票剔除误匹配
Step 4.1：第一关筛选：匹配距离必须小于设定阈值
Step 4.2：第二关筛选：最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
Step 4.3：计算匹配点旋转角度差所在的直方图，所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值。
Step 5：根据方向剔除误匹配的点