《SLAM十四讲》7.4 对极约束求解相机运动（代码详解）

最新推荐文章于 2021-12-20 15:23:54 发布

sxxblogs

最新推荐文章于 2021-12-20 15:23:54 发布

阅读量1.2k

点赞数 5

分类专栏： SLAM学习文章标签：计算机视觉 slam

本文链接：https://blog.csdn.net/sunnyblogs/article/details/107687377

版权

本文详细解读了《SLAM十四讲》中关于对极约束的代码实现，从相机内参矩阵的设定到使用ORB特征匹配，通过findFundamentalMat计算基础矩阵F和findEssentialMat求解本质矩阵E，进一步分解得到旋转矩阵R和平移向量t。博客以实际相机参数为例，解释了Point2f类型的必要性和数据填充过程。

摘要由CSDN通过智能技术生成

先上完整代码，稍后进行详细解释
完整代码包括上一章讲到的ORB特征检测与匹配部分，在这里ORB部分的内容封装到了函数find_feature_matches（）中。
关于该代码的介绍《十四讲》中的原话是：“上一节实践部分的程序提供了特征匹配，而这次我们就使用匹配好的特征点来计算 E,F 和 H，进而分解 E 得到 R,t。整个程序使用 OpenCV 提供的算法进行求解。我们把上一节的特征提取封装成函数，以供后面使用。”

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

using namespace std;
using namespace cv;

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

//两张图像得到关键点（keypoints）和描述子之间匹配关系（matches）
void find_feature_matches(
        const Mat& img_1, const Mat& img_2,
        vector<KeyPoint>& keypoints_1,
        vector<KeyPoint>& keypoints_2,
        vector<DMatch>& matches
        );
//从特征点(keypoints)和匹配关系（matches）求解位姿(R,t)
void pose_estimation_2d2d(
        const vector<KeyPoint> keypoints_1,
        const vector<KeyPoint> keypoints_2,
        const vector<DMatch> matches,
        Mat& R, Mat& t
        );

int main(int argc, char **argv) {
   
    if(argc != 3){
   
        cout << "usage: feature_extraction 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);

    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);

    //--验证 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="