slam十四讲-第七讲（1）对极几何-从本质矩阵、单应矩阵求解位姿_slam十四讲ppt 本质矩阵

通过对极几何的原理和矩阵分解技术，能够估计出两幅图像间的相对旋转和平移
在pose_estimation_2d2d.cpp代码中，通过特征匹配找到了两幅图像中对应的特征点，然后通过对极约束和基础矩阵/本质矩阵的计算，得到了相机的旋转矩阵R和平移向量t。

代码原理

1. 特征匹配 (find_feature_matches)

首先，使用ORB算法检测图像中的关键点，并为这些关键点生成描述符。然后，使用Hamming距离通过暴力匹配算法匹配两幅图像的描述符。匹配过程中，通过设置阈值过滤掉一些不好的匹配，以得到更准确的匹配点对。

2. 计算基础矩阵 (findFundamentalMat)

基础矩阵$F$描述了在无约束相机（即不知道内参）情况下，一对匹配点在两幅图像中的对极约束：

$$x^{\prime T}Fx=0$$

其中，$x$和$x^{\prime }$分别是第一幅和第二幅图像中的匹配点的齐次坐标。基础矩阵包含了相机的旋转和平移信息，但它不能直接用于恢复这些信息，因为它还包含了相机内参的影响。

3. 计算本质矩阵 (findEssentialMat)

本质矩阵$E$也描述了匹配点之间的对极约束，但它是在已知相机内参的情况下使用的：

$$x^{\prime T}Ex=0$$

与基础矩阵不同，本质矩阵仅由相机的旋转和平移决定，不包含内参的信息。这使得从本质矩阵中恢复出的旋转和平移更加精确。本质矩阵可以通过SVD分解进一步得到相机的旋转$R$和平移$t$。

4. 计算单应矩阵 (findHomography)

单应矩阵$H$描述了一幅图像到另一幅图像的透视变换，假设场景是平面的或者两幅图像之间仅发生旋转：

$$x^{\prime }=Hx$$

单应矩阵在处理平面场景或纯旋转的情况下非常有用，但它不能提供场景深度信息或是复杂场景中的三维结构信息。

5. 从本质矩阵恢复旋转和平移 (recoverPose)

最后一步使用函数recoverPose从本质矩阵中恢复出旋转$R$和平移$t$。这是通过对本质矩阵进行SVD分解并应用一些约束来实现的，以确保找到正确的解。

在提供的代码中，基础矩阵$F$、本质矩阵$E$和单应矩阵$H$的计算是通过OpenCV库中的特定函数完成的。这些函数使用图像中匹配点对的信息来估计相应的矩阵。下面是每个矩阵计算方法的详细说明：

代码解析

1. 计算基础矩阵$F$

基础矩阵是使用findFundamentalMat函数计算的，该函数的输入是两组对应的点（在这种情况下是points1和points2），这些点是从匹配的关键点中提取出来的像素坐标。cv::FM_8POINT方法被用作计算方法，它是一种经典的算法，基于八点算法求解基础矩阵。

Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, cv::FM_8POINT);
1
2

2. 计算本质矩阵$E$

本质矩阵是使用findEssentialMat函数计算的，该函数需要两组匹配点（同样是points1和points2），相机焦距和光心（主点）的位置。这里使用TUM数据集的标定值。findEssentialMat函数内部使用五点算法来求解本质矩阵。

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);
1
2
3
4

3. 计算单应矩阵$H$

单应矩阵是使用findHomography函数计算的，输入是相同的两组匹配点。此处使用的是RANSAC算法来鲁棒地估计单应矩阵，它可以有效地处理异常值（即错误匹配）。

Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
1
2

findHomography函数中的RANSAC参数指定了使用RANSAC算法进行鲁棒估计，3是RANSAC算法的阈值，用于决定何时将一个点视为内点。

总结

这三个矩阵$F,E,H$的计算都依赖于从两幅图像中提取并匹配的特征点。基础矩阵$F$和本质矩阵$E$用于估计相机的运动（即旋转$R$和平移$t$），而单应矩阵$H$在处理平面场景或估计纯旋转时特别有用。在这段代码中，通过对这些矩阵的计算和分解，可以从两幅图像中的2D-2D匹配点对恢复出相对于相机位姿的信息。

书中代码

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2 
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], IMREAD_COLOR );
    Mat img_2 = imread ( argv[2], IMREAD_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="<<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 = 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;

    //-- 从本质矩阵中恢复旋转和平移信息.
    recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
    
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174