第7讲视觉里程计2 --- 求解相机的运动

上一篇博客中学习了特征提取和匹配的概念，并且调用OpenCV库实现了ORB特征的提取和匹配。

找到了匹配点后，我们希望能够根据匹配的点对来估计相机的运动。由于相机的原理不同，情况就变得有点复杂了：

当相机为单目的时候，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，那么问题就是根据两组3D点估计运动。该问题通常用ICP来解决。
如果有3D点及其在相机的投影位置，也能估计相机的运动。该问题通过PnP来求解。

1、对极约束

下面先来看一下两个相邻帧之间的匹配点有什么关系？

以上图为例，我们设从 $I_{1}$ 到 $I_{2}$ 的运动为 $R,\vec{t}$ ，两个相机的中心分别为 $O_{1},O_{2}$ 。

现在，假设 $I_{1}$ 中有一个特征点 $p_{1}$ ，它在 $I_{2}$ 中对应的点为 $p_{2}$

（我们知道这是通过特征匹配得到的，如果匹配结果正确的话，就可以认为 $p_{1},p_{2}$ 是同一个空间点在两个成像平面上投影）

现在，我们假设匹配结果是正确的，然后就可以开始下面的数学推导了：

在 $I_{1}$ 的坐标系下，设点 $P$ 的空间位置为 $P=[X,Y,Z]^{T}$ ，

根据针孔相机模型可以知道，两个像素点 $p_{1},p_{2}$ 的像素位置满足：

$z_{1}p_{1}=KP$ ， $z_{2}p_{2}=K(RP+t)$

因为使用的是齐次坐标，所以在上式的左边乘以任意一个非零常数也是成立的，则有：

$p_{1}=KP$ ， $p_{2}=K(RP+t)$

现在取两个像素点在归一化平面上的坐标： $x_{1}=K^{-1}p_{1}$ ， $x_{2}=K^{-1}p_{2}$ ，代入上式中得：

$x_{2}=Rx_{1}+t$

然后两边同时左乘 $t^{\Lambda }$ ，注意： $t^{\Lambda }t=t\times t=0$ ，得：

$t^{\Lambda }x_{2}=t^{\Lambda }Rx_{1}$

然后，两式同时左乘 $x_{2}^{T}$ ，得到：

$x_{2}^{T}t^{\Lambda }x_{2}=x_{2}^{T}t^{\Lambda }Rx_{1}$

因为 $t^{\Lambda }x_{2}=t\times x_{2}$ 是一个和 $t,x_{2}$ 都垂直的向量，所以： $x_{2}^{T}t^{\Lambda }x_{2}$ =0，则有：

$x_{2}^{T}t^{\Lambda }Rx_{1}=0$

将 $x_{1},x_2$ 的值重新代入得到：

${\color{Red} p_{2}^{T}K^{-T}t^{\Lambda }RK^{-1}p_{1}=0}$

上式就称之为对极约束，它的几何意义是 $O_1,O_2,P$ 共面。

我们把上式的中间两个部分记为两个矩阵：基础矩阵（Fundamental matrix）F、本质矩阵（Essential matrix）E

${\color{Red} E=t^{\Lambda }R}$ ， ${\color{Red} F=K^{-T}t^{\Lambda }RK^{-1}=K^{-T}EK^{-1}}$

于是对极约束进一步简化为下式：

有了上面的基础之后，相机的位姿估计问题就可以分解为下面两步：

（1）根据相邻帧配对点的像素位置求出E或者F

（2）根据E或者F求出R, t

注：由对极几何的表达式也可以知道利用对极几何仅根据配对的像素点位置就可以求相机运动，记为 2D-2D 问题（瞎说的）

2、本质矩阵E

上面说了，根据 E 或者 F 都可以求出R, t，E 和 F 之间相差的就是相机内参K，而相机的内参矩阵在SLAM问题中一般是已知的，所以我们经常使用形式更加简单的本质矩阵E 来求解相机的运动。

根据定义，本质矩阵 $E=t^{\Lambda}R$ ，它是一个3*3的矩阵，内有9个未知数。另一方面，由于旋转和平移各有3个自由度，故 $E=t^{\Lambda}R$ 一共有6个自由度，但由于尺度等价性， $E$ 实际上只有5个自由度。 $E$ 具有5个自由度的事实，表明我们最少可以用5对点来求解 $E$ 。但是 $E$ 的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计 $E$ ---------这就是经典的八点法。（ps: 我对这段话不是很理解）

接下来的问题就是如何根据估计到的本质矩阵 $E$ ，来分解出R和t了。这个过程是通过奇异值分解得到的。

3、单应矩阵H

单应矩阵H，描述了两个平面之间的映射关系。若场景中的特征点都落在同一个平面上（比如墙、地面），则可以通过单应性来进行运动估计。这种情况在无人携带的俯视星际或者扫地机携带的顶视相机中比较常见。

下面从数学的角度说明一下什么是单应矩阵：

考虑在图像 $I_{1},I_{2}$ 上有一对匹配好的特征点 $p_{1},p_{2}$ ，这些特征点落在平面 $P$ 上，设这个平面的方程为：

$n^{T}P+d=0$

整理一下，就是：

$-\frac{n^{T}P}{d}=1$

又因为： $p_{2}=K(RP+t)$ ，可以得到：

$p_{2}=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}$

令 $H=K(R-\frac{tn^{T}}{d})K^{-1}$ ，于是 $p_{2}=Hp_{1}$ （H就是所谓的单应矩阵）

自由度为8的单应矩阵可以通过4对匹配特征点算出（这些特征点不能有三点共线的情况），然后通过H分解得到R，t。

计算出两幅图像之间的单应矩阵H之后，可以有如下用途：

（1）根据H分解出相机的运动R、t

（2）应用 $p_{2}=Hp_{1}$ 这个关系可以将一个视图中的所有点变换到另一个图的视角下的图片。

实践部分：下面看一下如何使用OpenCV计算基础矩阵、本质矩阵、单应矩阵以及分解R、T和利用单应矩阵变换图像的视角。

estimation.hpp:

/*
在这里，要接触到OpenCV的两个重要的模块：
（1）features2d模块：进行特征提取和匹配要用到的模块
（2）calib3d模块：这是一个之前没有接触过的模块，它是OpenCV实现的一个相机校准和姿态估计模块
*/

#ifndef ESTIMATION_H
#define ESTIMATION_H

#include "feature_extract_match.hpp"

//opencv
#include <opencv2/calib3d/calib3d.hpp>

//估计相机的运动：返回R、t
void pose_estimation_2d2d(vector<KeyPoint> key_points_1, vector<KeyPoint> key_points_2, vector<DMatch> matches, Mat &R, Mat &t)
{
    //内参矩阵
    double fx, fy, cx, cy;
    Mat K = Mat::eye(3, 3, CV_64FC1);   //Mat::eye()返回一个指定大小和类型的恒定矩阵
    fx = 520.9;
    fy = 521.0;
    cx = 325.1;
    cy = 249.7;
    K.at<double>(0, 0) = fx;
    K.at<double>(0, 2) = cx;
    K.at<double>(1, 1) = fy;
    K.at<double>(1, 2) = cy;
    cout << "internal reference matrix = " << endl;
    cout << K << endl;

    //将所有的KeyPoint转化为Point2f
    vector<Point2f> points_1;
    vector<Point2f> points_2;
    for(int i = 0; i < matches.size(); i++)
    {
        points_1.push_back(key_points_1[matches[i].queryIdx].pt);   //pt属性是KeyPoint的坐标
        points_2.push_back(key_points_2[matches[i].trainIdx].pt);
    }

    //计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points_1, points_2, CV_FM_8POINT);  //最后一个参数CV_FM_8POINT表示使用8点法计算基础矩阵
    cout << "fundamental_matrix = " << endl;
    cout << fundamental_matrix << endl;

    //直接调用findEssentialMat()函数来求本质矩阵，findEssentialMat()有两种原型，本次要调用的为下面这一种
    //findEssentialMat(InputArray points1, InputArray points2, double focal = 1.0, Point2d pp = Point2d(0, 0), int method = RANSAC, double prob = 0.999, double threshold = 1.0, OutputArray mask = noArray());
    double focal = 521;
    Point2d pp(325.1, 249.7);
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points_1, points_2, focal, pp);
    cout << "essential_matrix = " << endl;
    cout << essential_matrix << endl;

    //从本质矩阵恢复R、t
    //recoverPose(InputArray E, InputArray points1, InputArray points2, OutputArray R, OutputArray t, double focal = 1.0, Point2d pp = Point2d(0, 0), InputOutputArray = noArray());
    recoverPose(essential_matrix, points_1, points_2, R, t, focal, pp);
}

//计算单应矩阵
void homograph_estimation(vector<KeyPoint> key_points_1, vector<KeyPoint> key_points_2, vector<DMatch> matches, Mat &H)
{
    //将所有的KeyPoint转换为Point2f
    vector<Point2f> points_1, points_2;
    for(int i = 0; i < matches.size(); i++)
    {
        points_1.push_back(key_points_1[matches[i].queryIdx].pt);
        points_2.push_back(key_points_2[matches[i].trainIdx].pt);
    }

    //计算单应矩阵
    H = findHomography(points_1, points_2, RANSAC);
    cout << "H = " << endl;
    cout << H <<endl;
}

#endif

main.hpp:

#include "estimation.hpp"
#include "feature_extract_match.hpp"

//opencv，warpPerspective()在这个头文件中
#include "opencv2/imgproc.hpp"

int main(int argc, char ** argv)
{
    Mat img1, img2;
    img1 = imread("../datas/1.png");
    img2 = imread("../datas/2.png");

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;
    feature_extract_match(img1, img2, key_points_1, key_points_2, matches);

    cout << "一共找到了： " << matches.size() << "个匹配点" << endl;

    // Mat R, t;
    // pose_estimation_2d2d(key_points_1, key_points_2, matches, R, t);
    // cout << "R = " << endl;
    // cout << R << endl;
    // cout << "t = " << endl;
    // cout << t << endl;

    Mat H;
    Mat img_warp_perspective;
    homograph_estimation(key_points_1, key_points_2, matches, H);
    warpPerspective(img1, img_warp_perspective, H, img2.size());
    imshow("img1", img1);
    imshow("img2", img2);
    imshow("img_warp_perspective", img_warp_perspective);
    waitKey(0);

    return 0;
}