slambook2+ch7+pose_estimated_2d2d代码理解

算法流程

• 寻找关键点，计算描述子，寻找匹配点

• 利用对极几何计算本质矩阵，基础矩阵，单应矩阵

• 8点法求解本质矩阵（基础矩阵）

• 大于8点时可采用最小二乘法、随机采样一致性（RANSAC）

• 存在误匹配时倾向使用RANSAC

• 利用SVD(奇异值分解)求得旋转矩阵和平移矩阵

• 数值法、分析法分解单应矩阵求得R、t

• 验证本质矩阵，对极约束

• 相机内参数矩阵、焦距、光心已知

• 因为对极约束的尺度特性，真实的地图通常与计算出的相差一个非0常数倍

• 因为本质矩阵为平移矩阵叉乘旋转矩阵求得，当相机没有做平移运动时，对极几何无法应用

• 分析对极约束结果可知，算法精度不高

代码

//
// Created by wcm on 2020/6/6.
//
#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( ){
   
    //--读取图像
    Mat img_1=imread("/home/automobile/wcm/slambook2/ch7/1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2=imread("/home/automobile/wcm/slambook2/ch7/2.png", CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Cannot 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;

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

    //--验证E=t^R*scale
    //首先声明一个3*3的空矩阵，然后根据t求取t的反对称矩阵t^
    //at<double>(i,j)函数表示像素点的位置坐标
    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>