slambook2+ch7+pose_estimation_2d2d+估计多张图像之间的位姿

算法

1. 计算第一张图和第二张图的关键点并匹配
2. 以第一张图的相机坐标为世界坐标，计算第二张图相对第一张图的旋转矩阵、平移矩阵
3. 不断更新第一张图，在进行第二次计算时，以第二张图为第一张图，以第二张图的相机坐标系为世界坐标系

代码

//
// Created by automobile on 2020/7/19.
//
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#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(
        Mat &img_1,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(){
   
    //--读取图像
   // cv::String path = "/home/automobile/wcm/database/1080img" ;
    cv::String path = "/home/automobile/wcm/database/img_data" ;
    std::vector<cv::String> filenames;
    //cv::glob()读取文件夹中数据
    cv::glob(path, filenames);

    if (filenames.size() == 0){
   
        cout << "NO image files [png]" << endl;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    //--寻找关键点，计算描述子，匹配关键点
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    Mat img_1, img_2;
    Mat R, t;

    Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1,