C++手写八点法计算基础矩阵F筛选匹配特征点

starvapour

已于 2022-02-10 13:39:34 修改

阅读量1.2k

点赞数 2

分类专栏： C++/slam学习笔记手写算法实现文章标签：矩阵计算机视觉 opencv

于 2022-01-11 19:57:40 首次发布

本文链接：https://blog.csdn.net/starvapour/article/details/122440199

版权

C++/slam学习笔记同时被 2 个专栏收录

13 篇文章 6 订阅

订阅专栏

手写算法实现

2 篇文章 0 订阅

订阅专栏

C++手写八点法计算基础矩阵F筛选匹配特征点

1. 数据
2. 理论
3. 代码实现

SLAM学习笔记，基于已有的图像与特征点，手写八点法，归一化坐标并计算出基础矩阵F，然后用基础矩阵筛选出错误的匹配点。

1. 数据

两张前后帧的jpg图片，两个记录着特征点坐标的txt文件。
txt文件内容格式：

194.240067 201.709167 
296.351013 387.698456 
19.343060 192.708969 
292.529175 353.677307 
143.711227 189.554550 
...

每一行都是当前图片上一个特征点的x和y坐标。

2. 理论

目标：通过相似变换矩阵T将坐标(x, y)转换到均值为0，方差为1的归一化坐标上，再求出归一化基础矩阵F’，最后得到适用于非归一化坐标的基础矩阵F。

归一化需要使用相似变换矩阵T：
$\left[ \begin{matrix} S & 0 & t_x \\ 0 & S & t_y \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ 1 \end{matrix} \right]= \left[ \begin{matrix} x' \\ y' \\ 1 \end{matrix} \right]\tag{相似变换矩阵T}$
将 $(x, y)$ 映射到 $(x^{'}, y^{'})$ 上，可得：
$x' = Sx + t_x$
$y' = Sy + t_y$
因为目标是方差为1，所以目标为 $\frac{1}{n}∑\sqrt{x'^2+y'^2} = \sqrt{2}$

已知 $x^{'} = T x$ ，因此 $x'^T = x^TT^T$
因此，对极约束 $x^T_2$ F $x_1$ 变为
$x'^T_2$ $F^{'}$ $x'_1$
$=x^T_2$ $T^T_2$ $F^{'}$ $T_1$ $x_1$
可得：
$T^T_2 F' T_1$
解得将F’转换为F的公式。

已知 $x' = Sx + t_x$
要使得均值为0，即：
$\frac{1}{n}∑x'_i = 0$
$\frac{1}{n}∑(Sx_i + t_x) = 0$
$\frac{S}{n}∑x_i + t_x = 0$
$S\overline{x} + t_x = 0$
$t_x = -S\overline{x}$
同理可得： $t_y = -S\overline{y}$

要使得方差为1，则 $\frac{1}{n}∑\sqrt{x'^2+y'^2} = \sqrt{2}$
代入上方结论可得：
$\frac{1}{n}∑\sqrt{(Sx + t_x)^2+(Sy + t_y)^2} = \sqrt{2}$
$\frac{1}{n}∑\sqrt{(Sx - S\overline{x})^2+(Sy-S\overline{y})^2} = \sqrt{2}$
$S\frac{1}{n}∑\sqrt{(x - \overline{x})^2+(y-\overline{y})^2} = \sqrt{2}$
$\frac{\sqrt{2}}{\frac{1}{n}∑\sqrt{(x - \overline{x})^2+(y-\overline{y})^2}}$

可得： $S=\sqrt{2}/方差$
求出S后，代入 $t_x = -S\overline{x}$ ， $t_y = -S\overline{y}$ 即可得到 $t_x, t_y$

注：每张图片对应一个单独的归一化变换矩阵T！

因此，算法执行步骤如下：
1、输入1张图中的8个点
2、遍历点，求出x与y的均值
3、再次遍历，用均值求出该图所有点的方差
4、用 $S=\sqrt{2}/方差$ 求得S，代入S求得 $t_x, t_y$
5、重复步骤，获得两张图的变换矩阵 $T_1，T_2$
6、利用变换矩阵对原始坐标进行归一化，解矩阵方程计算 $F^{'}$
7、代入 $F^{'}$ 求解得到 $F$
8、用基础矩阵 $F$ 筛选正确与错误的匹配点

3. 代码实现

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include "math.h"

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

using namespace std;
using namespace cv;

int main(int argc, char **argv)
{
    // -----------------------------------------------------------从磁盘读取文件-----------------------------------------------------------
    // 包含两张前后帧的图片文件，和两个记录着特征点坐标的txt文件
    // 因为在build文件中编译，所以要向上级目录读取文件
    string save_path = "../";
    // 从磁盘上读图片
    cv::Mat cur_img = cv::imread(save_path + "cur_img.jpg", 0);
    cv::Mat forw_img = cv::imread(save_path + "forw_img.jpg", 0);

    // 从磁盘上读取特征点，并以vector<cv::KeyPoint>的形式储存
    string temp;
    int pos;
    float x;
    float y;
    // 读取第1张特征点
    ifstream cur_pts_file(save_path + "cur_pts.txt");
    // 以vector<cv::KeyPoint >的形式储存
    vector<cv::KeyPoint> cur_pts;
    if (!cur_pts_file.is_open())
    {
        cout << "未成功打开文件cur_pts.txt" << endl;
    }
    while (getline(cur_pts_file, temp))
    {
        if (temp.length() != 0)
        {
            // 获得字符串
            // 找到用于分割坐标的空格位置
            pos = temp.find(" ");
            // x坐标
            x = stof(temp.substr(0, pos));
            // y坐标
            y = stof(temp.substr(pos + 1, temp.length() - pos - 1));
            // 将<cv::Point2f >转换为<cv::KeyPoint >
            KeyPoint temp_keypoint;
            temp_keypoint.pt = Point2f(x, y);
            // 将<cv::KeyPoint >放入vector
            cur_pts.push_back(temp_keypoint);
        }
    }
    cur_pts_file.close();

    // 读取第2张特征点
    ifstream forw_pts_file(save_path + "forw_pts.txt");
    vector<cv::KeyPoint> forw_pts;
    if (!forw_pts_file.is_open())
    {
        cout << "未成功打开文件forw_pts.txt" << endl;
    }
    while (getline(forw_pts_file, temp))
    {
        if (temp.length() != 0)
        {
            // 获得字符串
            // 找到用于分割坐标的空格位置
            pos = temp.find(" ");
            // x坐标
            x = stof(temp.substr(0, pos));
            // y坐标
            y = stof(temp.substr(pos + 1, temp.length() - pos - 1));
            // 将<cv::Point2f >转换为<cv::KeyPoint >
            KeyPoint temp_keypoint;
            temp_keypoint.pt = Point2f(x, y);
            // 将<cv::KeyPoint >放入vector
            forw_pts.push_back(temp_keypoint);
        }
    }
    forw_pts_file.close();

    // // 打印行数进行验证是否都加载正常
    // cout << cur_pts.size() << endl;
    // cout << forw_pts.size() << endl;

    // 创建特征点之间的匹配
    vector<DMatch> matches;
    BFMatcher bfMatcher(NORM_L2);

    //计算特征点描述子，特征向量提取
    Mat dst1, dst2;
    // 使用ORB算法提取特征
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // 计算描述子
    descriptor->compute(cur_img, cur_pts, dst1);
    descriptor->compute(forw_img, forw_pts, dst2);
    // 进行描述子的匹配
    bfMatcher.match(dst1, dst2, matches);
    // 创建用于输出的图像
    cv::Mat out_image;
    // 绘制输出图像
    drawMatches(cur_img, cur_pts, forw_img, forw_pts, matches, out_image);
    // 显示输出图像
    imshow("全部匹配的连线图像", out_image);
    waitKey(0);

    // 截取前8个特征点匹配对，绘制图像
    vector<DMatch> matches_8(matches.begin(), matches.begin() + 8);

    // 创建用于输出前8个特征点的图像
    cv::Mat out_image_8;
    // 用于输出的特征点
    vector<cv::KeyPoint> cur_pts_8;
    vector<cv::KeyPoint> forw_pts_8;
    // 提取出需要的8对点，放入新的vector<cv::KeyPoint >
    for (auto p = matches_8.begin(); p != matches_8.end(); p++)
    {
        // 根据vector<DMatch>中记载的索引取出vector<cv::KeyPoint >中对应的关键点
        // queryIdx对应查询图像的特征描述子索引
        // trainIdx对应训练(模板)图像的特征描述子索引
        cur_pts_8.push_back(cur_pts.at(p->queryIdx));
        forw_pts_8.push_back(forw_pts.at(p->trainIdx));
    }

    // 绘制前8对点的输出图像
    drawMatches(cur_img, cur_pts_8, forw_img, forw_pts_8, matches_8, out_image_8);
    // 显示只有8对匹配点的输出图像
    imshow("8点连线图像", out_image_8);
    waitKey(0);
    // 依靠人工方式肉眼辨认确认这8个点都匹配正确
    // 然后将这8对特征点作为八点法的运算数据

    // 用于以vector<cv::Point2f>形式储存这八对点
    vector<cv::Point2f> cur_pts_8_pointf;
    vector<cv::Point2f> forw_pts_8_pointf;

    // 遍历需要的八对点
    for (auto p = matches_8.begin(); p != matches_8.end(); p++)
    {
        // 根据vector<DMatch>中记载的索引取出vector<cv::KeyPoint >中对应的关键点
        // CV_PROP_RW int queryIdx; // query descriptor index - 此匹配对应的查询图像的特征描述子索引
        // 一行
        cur_pts_8_pointf.push_back(cur_pts.at(p->trainIdx).pt);
        forw_pts_8_pointf.push_back(forw_pts.at(p->trainIdx).pt);
    }

    // -----------------------------------------------------------方法1：直接使用opencv库函数-----------------------------------------------------------
    // 使用opencv库函数的八点法完成计算基础矩阵
    Mat f_matrix_from_cv = findFundamentalMat(cur_pts_8_pointf, forw_pts_8_pointf, CV_FM_8POINT);
    cout << "findFundamentalMat" << endl << f_matrix_from_cv << endl;

    // -----------------------------------------------------------方法2：手写归一化并使用SVD分解-----------------------------------------------------------
    // 与opencv库函数使用的是相同的算法

    // 对八对点分别进行归一化
    // 每张图都使用一个变换矩阵T用于将x与y归一化到均值为1的状态
    // 用于储存输出的归一化点
    std::vector<cv::Point2f> cur_pts_8_pointf_norm = cur_pts_8_pointf;
    std::vector<cv::Point2f> forw_pts_8_pointf_norm = forw_pts_8_pointf;

    // 第1张图
    // 第一步得到所有特征点的均值
    float mean_x = 0;
    float mean_y = 0;
    for (int i = 0; i < 8; i++)
    {
        mean_x += cur_pts_8_pointf.at(i).x;
        mean_y += cur_pts_8_pointf.at(i).y;
    }
    mean_x /= 8;
    mean_y /= 8;

    //第二步求方差sum( sqrt( (x-mean(x))^2 + (y-mean(y))^2 ) ) / n
    float variance = 0;
    for (int i = 0; i < 8; i++)
    {
        variance += sqrt((cur_pts_8_pointf.at(i).x - mean_x) * (cur_pts_8_pointf.at(i).x - mean_x) +
                         (cur_pts_8_pointf.at(i).y - mean_y) * (cur_pts_8_pointf.at(i).y - mean_y));
    }
    variance /= 8;

    // 根据方差计算S
    float S = sqrt(2) / variance;
    // 计算出tx与ty
    float tx = -S * mean_x;
    float ty = -S * mean_y;

    // 得到变换矩阵T1
    Matrix<float, 3, 3> T1;
    T1 << S, 0, tx, 0, S, ty, 0, 0, 1;

    // 输出归一化点
    for (int i = 0; i < 8; i++)
    {
        cur_pts_8_pointf_norm.at(i).x = S * cur_pts_8_pointf.at(i).x + tx;
        cur_pts_8_pointf_norm.at(i).y = S * cur_pts_8_pointf.at(i).y + ty;
    }

    // 第2张图，重复上述步骤
    // 第一步得到所有特征点的均值
    mean_x = 0;
    mean_y = 0;
    for (int i = 0; i < 8; i++)
    {
        mean_x += forw_pts_8_pointf.at(i).x;
        mean_y += forw_pts_8_pointf.at(i).y;
    }
    mean_x /= 8;
    mean_y /= 8;

    //第二步求方差sum( sqrt( (x-mean(x))^2 + (y-mean(y))^2 ) ) / n
    variance = 0;
    for (int i = 0; i < 8; i++)
    {
        variance +=
            sqrt((forw_pts_8_pointf.at(i).x - mean_x) * (forw_pts_8_pointf.at(i).x - mean_x) +
                 (forw_pts_8_pointf.at(i).y - mean_y) * (forw_pts_8_pointf.at(i).y - mean_y));
    }
    variance /= 8;

    // 根据方差计算S
    S = sqrt(2) / variance;
    // 计算出tx与ty
    tx = -S * mean_x;
    ty = -S * mean_y;

    // 得到变换矩阵T2
    Matrix<float, 3, 3> T2;
    T2 << S, 0, tx, 0, S, ty, 0, 0, 1;

    // 输出归一化点(S*x + tx, S*y+ ty)
    for (int i = 0; i < 8; i++)
    {
        forw_pts_8_pointf_norm.at(i).x = S * forw_pts_8_pointf.at(i).x + tx;
        forw_pts_8_pointf_norm.at(i).y = S * forw_pts_8_pointf.at(i).y + ty;
    }

    // 打印两幅图像的归一化转换矩阵与归一化坐标
    cout << "T1" << endl << T1 << endl << endl;
    cout << "T2" << endl << T2 << endl << endl;

    // 对于求解矩阵Ax=0，首先构建系数矩阵A
    Mat matrix_a(8, 9, CV_32FC1);
    Vec<float, 9> params;

    auto p = matches_8.begin();
    float x1, y1, x2, y2;

    // 为矩阵A赋值
    for (int i = 0; i < 8; i++)
    {
        x1 = cur_pts_8_pointf_norm.at(i).x;
        y1 = cur_pts_8_pointf_norm.at(i).y;
        x2 = forw_pts_8_pointf_norm.at(i).x;
        y2 = forw_pts_8_pointf_norm.at(i).y;
        // 为A的其中一行赋值
        params[0] = x2 * x1;
        params[1] = x2 * y1;
        params[2] = x2;
        params[3] = y2 * x1;
        params[4] = y2 * y1;
        params[5] = y2;
        params[6] = x1;
        params[7] = y1;
        params[8] = 1.0;

        matrix_a.at<Vec<float, 9>>(i) = params;
        p++;
    }
    // 构建矩阵A完成
    // 打印系数矩阵A
    cout << "\nmatrix A:\n" << matrix_a << endl << endl;

    // 求归一化的基础矩阵F'
    Matrix<float, 8, 9> matrix_a_eigen;
    // 将A转换为Eigen矩阵
    cv2eigen(matrix_a, matrix_a_eigen);
    // SVD分解
    // A=USV^T
    JacobiSVD<Eigen::MatrixXf> svd(matrix_a_eigen, ComputeThinU | ComputeThinV);
    auto V = svd.matrixV();
    auto U = svd.matrixU();
    // 计算S中所有的奇异值
    auto A = svd.singularValues();
    cout << "V" << V << endl << endl;
    cout << "A" << A << endl << endl;

    // 使用V矩阵的最后一列，即最小奇异值对应的奇异向量作为F'
    Eigen::Matrix<float, 9, 1> v_col = V.col(7);
    // 按照顺序将它reshape成3x3矩阵
    Map<Matrix3f> f_matrix_from_svd(v_col.data(), 3, 3);
    f_matrix_from_svd = f_matrix_from_svd.transpose().eval();

    cout << endl << "F'" << endl << f_matrix_from_svd << endl << endl;

    // 将F'还原为F
    // F=T2^T * F' * T1
    f_matrix_from_svd = T2.transpose() * f_matrix_from_svd.eval() * T1;

    // 打印基础矩阵F
    cout << endl << "F" << endl << f_matrix_from_svd << endl << endl;

    // -----------------------------------------------------------构造可视化图像验证-----------------------------------------------------------
    // 使用基础矩阵F筛选出错误的匹配点
    Matrix3f f_matrix_eigen;

    // 使用方法1：opencv库函数得到的基础矩阵
    // cv2eigen(f_matrix_from_cv, f_matrix_eigen);

    // 使用方法2：手写八点法得到的基础矩阵
    f_matrix_eigen = f_matrix_from_svd;

    // 记录匹配错误的点，并且绘制可视化图像
    std::vector<cv::DMatch> wrong_matches;
    std::vector<cv::KeyPoint> wrong_kpoints1;
    std::vector<cv::KeyPoint> wrong_kpoints2;
    cv::Mat wrong_image;

    cout << "error match:" << endl;

    int index = 0;

    // 利用基础矩阵F筛选匹配
    for (auto p = matches.begin() + 8; p != matches.end(); p++)
    {
        // 计算每一对特征点代入基础矩阵得到的值
        Eigen::Vector3f v1;
        v1 << cur_pts.at(p->queryIdx).pt.x, cur_pts.at(p->queryIdx).pt.y, 1.0;
        Eigen::Vector3f v2;
        v2 << forw_pts.at(p->trainIdx).pt.x, forw_pts.at(p->trainIdx).pt.y, 1.0;

        auto match_value = v2.transpose() * (f_matrix_eigen * v1);

        // 如果匹配误差超出阈值1，视为错误匹配
        if (match_value > 1)
        {
            cout << "match value: " << setw(12) << match_value << "             ";
            cout << "point1：(" << setw(10) << cur_pts.at(p->queryIdx).pt.x << setw(10)
                 << cur_pts.at(p->queryIdx).pt.y << ")   ";
            cout << "point2：(" << setw(10) << forw_pts.at(p->trainIdx).pt.x << setw(10)
                 << forw_pts.at(p->trainIdx).pt.y << ")   " << endl;
            // 记录错误点匹配用于可视化显示
            wrong_kpoints1.push_back(cur_pts.at(p->queryIdx));
            wrong_kpoints2.push_back(forw_pts.at(p->trainIdx));
            p->queryIdx = index;
            p->trainIdx = index;
            index += 1;
            wrong_matches.push_back(*p);
        }
    }

    // 绘制错误匹配可视化图像
    cv::Mat error_img;
    // 绘制输出图像
    drawMatches(cur_img, wrong_kpoints1, forw_img, wrong_kpoints2, wrong_matches, error_img);
    // 显示错误匹配点的输出图像
    imshow("错误连线图像", error_img);
    waitKey(0);

    // 人工肉眼确认筛选出的匹配的确是错误的匹配点

    return 0;
}