《视觉SLAM十四讲》单应矩阵与cv::decomposeHomographyMat函数源码浅析

Charming Sun

已于 2022-04-26 12:40:34 修改

阅读量4.5k

点赞数 20

分类专栏：读书笔记 OpenCV 文章标签： OpenCV

于 2020-04-03 13:36:11 首次发布

本文链接：https://blog.csdn.net/CharmingSun/article/details/105291224

版权

读书笔记同时被 2 个专栏收录

3 篇文章 0 订阅

订阅专栏

OpenCV

3 篇文章 0 订阅

订阅专栏

参考文献：
[1] 视觉SLAM十四讲：从理论到实践/高翔等著.—北京：电子工业出版社，2017.3
[2] 计算机视觉中的多视图几何：原书第2版/（澳）理查德 · 哈特利（Richard Hartley），（英）安德鲁 · 西塞曼（Andrew Zisserman）著；韦穗，章权兵译.—北京：机械工业出版社，2019.8
[3] Ezio Malis, Manuel Vargas. Deeper understanding of the homography decomposition for vision-based control. [Research Report] RR-6303, INRIA. 2007, pp.90. ffinria-00174036v3

一、前言

由于《视觉SLAM十四讲》P146“7.3.3 单应矩阵”这一节中对方程 $\textbf{n}^{T}\textbf{P}+d=0$ ……(7.16)的由来缺乏说明，以及《计算机视觉中的多视图集合》P253的结论13.1与例13.2表述不够清晰，所以本文的第二节我将用更加详细的过程来推导单应矩阵。

因为《视觉SLAM十四讲》中没有提供从单应矩阵解析相机旋转与平移信息的示例代码，所以本文的第三节我将剖析cv::decomposeHomographyMat函数的具体实现，算法的推导过程在参考文献3中有详细说明。

有些人发现使用单应矩阵进行运动估计的结果和使用对极约束、PnP、ICP得出的结果相差甚远，其实这是因为忽略了单应矩阵只适用于两幅图像中所有匹配的特征点几乎处于同一平面上的前提条件，所以导致使用单应矩阵进行运动估计得出的结果往往是错误的。所以本文的第四、五节，我将增加从单应矩阵中使用cv::decomposeHomographyMat函数恢复旋转和平移信息的代码，并且配合两组不同的图片进行正反对比。使用单应矩阵还有一些其它的限制条件，本文没有阐述。

二、单应矩阵的推导

《视觉SLAM十四讲》中的推导过程：
设特征点所在平面的平面方程为 $A x + B y + C z + D = 0$ ，取第一幅视图的相机中心为世界原点
将方程两边同时除以 $\sqrt{A^{2}+B^{2}+C^{2}}$ 得 $\frac{A}{\sqrt{A^{2}+B^{2}+C^{2}}}x+\frac{B}{\sqrt{A^{2}+B^{2}+C^{2}}}y+\frac{C}{\sqrt{A^{2}+B^{2}+C^{2}}}z+\frac{D}{\sqrt{A^{2}+B^{2}+C^{2}}}=0$
由点到平面的距离公式可得原点到平面的距离为 $d=\frac{\left|Ax_{1}+By_{1}+Cz_{1}+D\right|}{\sqrt{A^{2}+B^{2}+C^{2}}}=\frac{\left|D\right|}{\sqrt{A^{2}+B^{2}+C^{2}}}$
因为原点为相机中心，又有 $(\frac{A}{\sqrt{A^{2}+B^{2}+C^{2}}},\frac{B}{\sqrt{A^{2}+B^{2}+C^{2}}},\frac{C}{\sqrt{A^{2}+B^{2}+C^{2}}})^{T}$ 为平面的单位法向量 $\textbf{n}$
所以可得 $\textbf{n}^{T} \cdot \left[\begin{matrix}x \\ y \\ z\end{matrix}\right]+d=\textbf{n}^{T}\textbf{P}+d=0$ ， $\textbf{P}$ 为平面内特征点的坐标，进一步可得 $-\frac{\textbf{n}^{T}\textbf{P}}{d}=1$
$\textbf{p}_{1}$ 、 $\textbf{p}_{2}$ 分别为平面内特征点 $\textbf{P}$ 在第一幅视图中和第二幅视图中的像素坐标，由针孔相机模型可得：
$\textbf{p}_{2}=\textbf{K}(\textbf{RP}+\textbf{t})=\textbf{K}(\textbf{RP}+\textbf{t} \cdot (-\frac{\textbf{n}^{T}\textbf{P}}{d}))=\textbf{K}(\textbf{R}-\frac{\textbf{tn}^{T}}{d})\textbf{P}=\textbf{K}(\textbf{R}-\frac{\textbf{tn}^{T}}{d})\textbf{K}^{-1}\textbf{p}_{1}=\textbf{Hp}_{1}$
《计算机视觉中的多视图几何》中的推导过程：
设第一幅视图的投影矩阵（将世界坐标转换为归一化相机坐标）为 $\textbf{P}=\left[\begin{array}{c|c}\textbf{I} & \textbf{0}\end{array}\right]$ ，设第二幅视图的投影矩阵为 $\textbf{P}^{\prime}=\left[\begin{array}{c|c}\textbf{A} & \textbf{a}\end{array}\right]$ ，取第一幅视图的相机中心为世界原点。
设 $\textbf{v}=\textbf{n}/d$ ，设特征点所在平面的坐标为 $\boldsymbol{\pi}=\left(\textbf{v}^{T},1\right)^{T}$ ，则特征点 $\textbf{X}$ 满足方程 $\boldsymbol{\pi}^{T}\textbf{X}=0$ ，解方程可得 $\textbf{X}=\left(\textbf{x}^{T},-\textbf{v}^{T}\textbf{x}\right)^{T}$
设 $\textbf{x}$ 为特征点 $\textbf{X}$ 在第一幅图像上的投影，设 $\textbf{x}^{\prime}$ 为特征点 $\textbf{X}$ 在第二幅图像上的投影，则对于 $\textbf{x}^{\prime}$ 有如下等式：
$\textbf{x}^{\prime}=\textbf{P}^{\prime}\textbf{X}=\left[\begin{array}{c|c}\textbf{A} & \textbf{a}\end{array}\right]\textbf{X}=\textbf{Ax}-\textbf{av}^{T}\textbf{x}=\left(\textbf{A}-\textbf{av}^{T}\right)\textbf{x}$
所以关于摄像机 $\textbf{P}=\left[\begin{array}{c|c}\textbf{I} & \textbf{0}\end{array}\right]$ ， $\textbf{P}^{\prime}=\left[\begin{array}{c|c}\textbf{R} & \textbf{t}\end{array}\right]$ 的单应是 $\textbf{H}=\textbf{R}-\frac{\textbf{tn}^{T}}{d}$
将两幅图像的归一化相机坐标乘以两幅图像的相机内参 $\textbf{K}$ 和 $\textbf{K}^{\prime}$ 转换为像素坐标，得到相机 $\textbf{P}_{E}=\textbf{K}\left[\begin{array}{c|c}\textbf{I} & \textbf{0}\end{array}\right]$ ，和相机 $\textbf{P}^{\prime}_{E}=\textbf{K}^{\prime}\left[\begin{array}{c|c}\textbf{R} & \textbf{t}\end{array}\right]$ 及其诱导单应为：
$\textbf{H}=\textbf{K}^{\prime}(\textbf{R}-\frac{\textbf{tn}^{T}}{d})\textbf{K}^{-1}$
其中 $\textbf{K}$ 和 $\textbf{K}^{\prime}$ 分别为第一幅视图的相机内参和第二幅视图的相机内参（在使用双目相机的情况下），若两幅视图来自同一个相机则 $\textbf{K}=\textbf{K}^{\prime}$

三、cv::decomposeHomographyMat函数源码浅析

源文件路径：opencv-3.1.0/modules/calib3d/src/homography_decomp.cpp
函数调用流程：

其中值得注意的是HomographyDecompInria::decompose函数中最后面的如下代码：

    //Ra, ta, na
    findRmatFrom_tstar_n(ta_star, na, v, Ra);
    ta = Ra * ta_star;

    camMotions[0].R = Ra;
    camMotions[0].t = ta;
    camMotions[0].n = na;

    //Ra, -ta, -na
    camMotions[1].R = Ra;
    camMotions[1].t = -ta;
    camMotions[1].n = -na;

    //Rb, tb, nb
    findRmatFrom_tstar_n(tb_star, nb, v, Rb);
    tb = Rb * tb_star;

    camMotions[2].R = Rb;
    camMotions[2].t = tb;
    camMotions[2].n = nb;

    //Rb, -tb, -nb
    camMotions[3].R = Rb;
    camMotions[3].t = -tb;
    camMotions[3].n = -nb;

以上代码说明cv::decomposeHomographyMat函数会返回四组结果，我们可以根据深度为正值排除两组结果（假设物体在相机前方），我们还可以假设特征点所在平面的单位法向量已知，这样又可以排除一组结果，或者通过其它先验信息来判断。由于平面有两个方向相反的单位法向量，也因此 $\textbf{H}=\textbf{K}(\textbf{R}-\frac{\textbf{tn}^{T}}{d})\textbf{K}^{-1}$ 或者 $\textbf{H}=\textbf{K}(\textbf{R}+\frac{\textbf{tn}^{T}}{d})\textbf{K}^{-1}$ 都是对的。

四、实验代码

下面附上我更改过的slambook/ch7/pose_estimation_2d2d.cpp，在第168行到第178行增加了由单应矩阵恢复相机的旋转与平移信息并且打印的代码：

#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], 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="<<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;

    //-- 从单应矩阵中恢复旋转和平移信息.
    vector<Mat> R_h, t_h, n;
    int solutions = decomposeHomographyMat(homography_matrix, K, R_h, t_h, n);
    cout << "========Homography========" << endl;
    for(int i = 0; i < solutions; ++i) {
        cout << "======== " << i << " ========" << endl;
        cout << "rotation" << i << " = " << endl;
        cout << R_h.at(i) << endl;
        cout << "translation" << i << " = " << endl;
        cout << t_h.at(i) << 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.png和2.png，匹配到的特征点不在同一平面上，通过单应矩阵计算相机运动将得出的不准确的结果
第二组图片为a.png和b.png，匹配到的所有特征点几乎在同一平面上，通过单应矩阵计算相机运动将得出较为准确的结果

Charming Sun

关注

20
点赞
踩
42

收藏

觉得还不错? 一键收藏
8
评论
《视觉SLAM十四讲》单应矩阵与cv::decomposeHomographyMat函数源码浅析

参考文献：[1] 视觉SLAM十四讲：从理论到实践/高翔等著.—北京：电子工业出版社，2017.3[2] 计算机视觉中的多视图几何：原书第2版/（澳）理查德 · 哈特利（Richard Hartley），（英）安德鲁 · 西塞曼（Andrew Zisserman）著；韦穗，章权兵译.—北京：机械工业出版社，2019.8[3] Ezio Malis, Manuel Vargas. Deeper...
复制链接

扫一扫