VSLAM（4）：特征点提取与对极几何

聪明的笨小子

已于 2023-07-18 19:55:29 修改

阅读量580

点赞数

分类专栏：视觉SLAM14讲文章标签：计算机视觉人工智能

于 2023-06-26 05:23:04 首次发布

本文链接：https://blog.csdn.net/zhangzhixin1998/article/details/131346514

版权

视觉SLAM14讲专栏收录该内容

10 篇文章 2 订阅

订阅专栏

本节总结图像特征点定义，如何提取特征点以及对极几何的原理，并给出SLAM14讲书中的代码实例。

一. 特征点提取

特征点的定义是一张图中比较有代表性的点，这些点会在相机少量运动后保持不变。所以利用特征点的这些特性，可以根据前后帧估计出相机运动的位姿变化，

1.1 特征点

至今为止特征没有万能和精确的定义。特征的精确定义往往由问题或者应用类型决定。特征是一个数字图像中“有趣”的部分，它是许多计算机图像分析算法的起点。因此一个算法是否成功往往由它使用和定义的特征决定。因此特征检测最重要的一个特性是“可重复性”：同一场景的不同图像所提取的特征应该是相同的。

在视觉SLAM问题中，我们希望图像特征在相机运动后保持稳定，这样就可以将特征点的位移作为视觉里程计的位移量。常见的特征点包括角点，边缘点和区块等，但是这些传统的特征通常会存在一些问题，例如一个从远处看是角点的点，靠近后很可能就不再是角点了。针对这些问题，CV领域设计了许多更加稳定的局部特征，例如SIFT，ORB等。这些特征点有如下特点：

可重复性（Respeatability）：相同的“区域”可以在不同的图像中找到；
可区别性（Distinctiveness）：不同的“区域”有不同的表达；
高效率（Eiffciency）：同一图像中，特征点的数量应远小于像素的数量；
本地性（Locality）：特征仅与一小片图像区域有关。

一个特征点由关键点（Key Poit）和描述子（Describer）组成。关键点就是特征提取出来的点，即特征点在图像中的坐标，一些类型的特征点还具有大小和方向（例如ORB）。而描述子则是认为设计的用来描述特征点特有性质的向量。外观相似的特征应该有相似的描述子。因此，两个特征点的描述子在空间上相近时，就可以认为他们是同样的特征点。

1.2 ORB特征

ORB特征全称是“Oriented FAST”，就是有方向的FAST角点。提取ORB的步骤为：

FAST角点提取：找出图像中的“角点”。相较于与原版的FAST，ORB中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变性；
BRIEF描述子：对前一步提取出特征点的周围图像区域进行描述。

关于ORB的具体计算方法可以看参考2，因为ORB已经集成在Opencv库中，直接调用就可以，并且这部分的知识主要涉及CV的，就不再展开来写了。

这里使用coco数据中的一张图片，将经过resize和旋转的图片和原图片进行ORB特征匹配，效果如下：

可以看到，对于同一张图，ORB特征匹配可以很好的匹配到途中相同的区域。

二. 2D-2D对极几何

2.1 对极约束

在得到特征匹配的点对之后，我们希望通过特征点之间的点对运动关系推算出相机的运动，这里引出2D-2D的对极几何，如下图所示：

$O_R$ 和 $O_L$ 分别是相机运动前后的中心点， $X_L$ 和 $X_R$ 则是两个匹配好的特征点对， $X$ 是这两个点对对应到3D空间的实际点。我们定义 $X$ ， $O_R$ 和 $O_L$ 构成的平面称为极平面（Epipolar plane）， $e_L$ 和 $e_R$ 分别是极平面和左右两个图像平面的交点，称为极点（Epipoles） $O_R$ $O_L$ 称为极线（Epipolar Line）。 $X_L$ 和 $X_R$ 默认是匹配正确的，那么他们对应到3D空间中就应该是一个点，也就是 $X$ 。但实际场景中因为各种误差，可能导致对应的并不是同一个点。

设 $X$ 在空间的位置为：

$X_L^Ts_1^TK^{-T}t^\wedge RK^{-1}s_2X_R$ $X_L^Ts_1K^{-T}t^\wedge RK^{-1}s_2X_R =0$ $X = [X,Y,Z]^T$

定义两帧之间的运动为 $R$ 和 $t$ ，那么个根据相机模型内外参数，就有：

$s_1X_L=KX , s_2X_R = K(RX+t)$

$K$ 为相机内参矩阵， $s_1s_2$ 分别是两帧图像中单个像素大小。现在定义两个像素点的归一化平面上的坐标分别为：

$x_1 = K^{-1}s_1X_L$

$x_2 = K^{-1}s_2X_R$

那么结合上面的式子就有:

$x_2 = Rx_1+t$

两边同时乘 $t^\wedge$ ，左边相当于是 $x_2$ 和 $t$ 的外积，结果会是一个即垂直于 $x_2$ 又垂直于 $t$ 的向量，而右边 $t$ 与本身做外积就等于0，那么就有：

$t^\wedge x_2 = t^\wedge R x_1$

两边再同时左乘 $x_2^T$ ，因为左边的是 $x_2$ 和 $t$ 的外积，再与 $x_2^T$ 作内积时，结果是零：

$x_2^T t ^\wedge R x_1 = 0$

重新代入 $x_1 x_2$ 的定义式就有：

$X_L^T s_1^T K^{-T} t^\wedge R K^{-1} s_2 X_R =0$

上面的两个式子就成为对极约束，是一个关于 $R, t$ 与两帧像素坐标的关系等式。两个式子的中间部分可以记作两个矩阵：基础矩阵（Fundamental Matrix）F和本质矩阵（Essential Matrix）E，可以进一步简化式子：

$E = t^\wedge R, F = K^{-T}EK^{-1}, x_2^TEx_1 =X_L^T s_1^T Fs_2 X_R=0$

这样有了两个像素点与相机位姿转换矩阵R，t之间的约束条件，就可以估计相机位姿：

根据配对点的像素位置，求出E或则F；
根据E或者F，求出R，t。

2.2 本质矩阵

根据上面的定义，本质矩阵 $E = t^\wedge R$ ，是个3*3的矩阵，九个未知数。本质矩阵有如下的性质：

本质矩阵由对极约束定义，而对极约束的一侧为0，所以对E乘以任何常数，对极约束依然满足。这个叫做E在不同尺度下是等价的；
根据 $E = t^\wedge R$ ，其中 $t$ 的秩为2， $R$ 的秩为3，所以可以推算出E的秩为2，他的奇异值必然是 $[\sigma_1, \sigma_2, 0]^T$ 的形式。这个叫做本质矩阵的内在性质；
由于旋转和平移各有三个自由度，加起来是六个自由度，但由于尺度等价性，其实是有五个自由度。

详细的推导可以看参考4博主给出的推算过程。

通过本质矩阵的性质后，我们需要通过对极约束推算出旋转和平移矩阵是什么样的。因为尺度不变性的原因，不需要设9个未知数来求解，使用八个未知数就可以求解出R和t。这就是八点法（Eight-point-algorithm）。

假设两个点： $x_1 = [u_1 v_1 1]^T, x_2 = [u_2 v_2 1]^T,$ 是同一个点在不同相机角度下的坐标，那么将两个点的坐标代入对极约束的公式就有：

$\begin{pmatrix} u_1 & v_1 & 1 \end{pmatrix} \begin{pmatrix} e_1 & e_2 & e_3\\ e_4 & e_5 & e_6\\ e_7 & e_8 & e_9 \end{pmatrix} \begin{pmatrix} u_2\\v_2 \\ 1 \end{pmatrix} = 0$

把E展开，写成向量的形式：

$e = [e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9]^T$

那么对极约束的公式在展开后，就可以转化为以下形式：

$[u_1u_2,u_1v_2,u_1,v_1u_2,v_1v_2,v_1,u_2,v_2,1] . e= 0$

当我们有多个点对的时候，上式同样满足，如果我们放入八个点，这时的就构成了一个关于E的线性方程组，当系数矩阵满秩时（为8），那么E就可以解出来。

在得出E后，我们需要求通过E求解旋转和平移矩阵，这个过程通过奇异值分解得出（SVD）：

$E = U\Sigma V^T$

但是解出的E可能会不满足E的内在性质，他的奇异值不一定会是两个非零值和一个零的结果，可能是三个非零值。通常的做法是对求出的E进行SVD分解后，得到 $\Sigma = diag(\sigma_1, \sigma_2, \sigma_3)$ ，假设满足 $\sigma_1 \geq \sigma_2 \geq \sigma_3$ ，那么定义新的E为：

$E = U diag(\frac{\sigma_1+\sigma_2}{2}, \frac{\sigma_1+\sigma_2}{2},0)V^T$

根据E的内在性质可知，他存在两个非零特征值，即 $\Sigma = diag(\sigma_1, \sigma_2, 0)$ 。在SVD分解中，对于任何一个E，存在两个可能的t和R与之对应:

$t_1^ \wedge = UR_Z(\frac{\pi}{2})\Sigma U^T\ \ \ , \ \ \ R_1 = UR_Z^T(\frac{\pi}{2})V^T$

$t_2^ \wedge = UR_Z(-\frac{\pi}{2})\Sigma U^T \ \ , \ \ \ R_2 = UR_Z^T(-\frac{\pi}{2})V^T$

其中 $R_Z(\frac{\pi}{2})$ 表示沿Z轴旋转90度得到的旋转矩阵。同时由于本质矩阵的尺度不变性，E和-E是等价的，所以R和t的解存在四个。

参考5有详细的介绍如何从本质矩阵恢复出相机的状态转移矩阵。虽然有四个解，但是只有一个解中，观测在两个相机中才有正方向的深度。因此，把四个解代入，求出观测点的深度，查看是否满足两个相机坐标下的深度都为正。

2.3 单应矩阵

单应矩阵（Homography）描述两个平面之间的映射关系。假设一个3d空间点 $P$ 位于一个平面上，这个平面满足下面方程：

$n^TP+d =0$

其中 $n$ 代表平面的法向量， $d$ 为相机到平面的距离。整理得：

$-\frac{-n^TP}{d} = 1$

假设这时候有一对匹配好的点 $p_1$ 和 $p_2$ 在这个平面上，那么根据下面的式：

$p_1=KP , \ \ \ \ \ p_2 = K(RP+t)$

就可以得到：

$p_2 = K(RP+t) \ \ \ = K(RP+t(-\frac{n^T P}{d})) \\ =K(R-\frac{tn^T}{d})P = k(R-\frac{tn^T}{d})K^{-1}p_1$

方程坐标是 $p_2$ ，右边是一个关于 $p_1$ 的方程，中间部分的方程就叫做单应矩阵，记作 $H$ 。

$p_2 = Hp_1$

H的内部包含旋转，平移和平面的参数。所以我们可以通过一对匹配点的坐标表示出计算出H，在从H推导出旋转和平移矩阵：

$\begin{pmatrix} u_2\\v_2 \\ 1 \end{pmatrix} = \begin{pmatrix} h_1&h_2&h_3\\h_4&h_5&h_6 \\ h_7&h_8&h_9 \end{pmatrix} \begin{pmatrix} u_1\\v_1 \\ 1 \end{pmatrix}$

展开得

$u_2 = \frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9}$

$v_1 = \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}$

因为转换的是齐次坐标，H具有尺度不变性，一般通过尺度变换使得 $h_9$ 为1。代入后整理得：

$h_1u_1+h_2v_1+h_3-h_7u_1u_2-h_8v_1u_2=u_2$

$h_4u_1+h_5v_1+h_6-h_7u_1v_2-h_8v_1v_2 = v_2$

可以看出使用一对匹配点可以得到两个约束，于是自由度为8的但应矩阵就可以通过4对匹配点得出。在解出H矩阵后，通过分解得到R和t。分解的方法包括数值法和解析法。结果同样会是返回四组旋转矩阵和平移向量，可以通过先验知识来决策出正确的R和t。

2.4 代码实例

下面是14讲中关于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 );
    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;
    
}

关于对极约束的讨论：

对极约束是描述同一空间点在两个不同相机坐标系下存在的对应关系，根据对极几何的条件，需要至少八个点来计算相机的位姿变换；
对极约束是建立在三角测量的关系基础上，所以只有当相机变换矩阵的平移量不为零的情况下才满足约束关系，纯旋转是无法使用对极约束的；
即使当平移存在的情况下，三角测量同样存在不确定性，这个不确定性是跟平移量大小成比例关系，当平移量较小时，不确定性较大；反之亦然。
增大三角化的精度，一种办法时特稿特征点的提取精度，但同时导致计算成本增大；另一种办法是使得平移量增大，但这同样会导致图像的外观变化明显，导致特征提取和匹配出错。