多视图几何--2单应矩阵-2.0从0-1理解并计算单应矩阵

1 什么是单应矩阵

单应矩阵（Homography Matrix）是一个描述两个平面之间点的映射关系的矩阵。在计算机视觉中，它常用于描述同一场景在不同视角下的图像之间的几何变换关系。单应矩阵即射影变换的数学形式。

2 单应矩阵与相机内外参关系的一般形式

由于一般情况下，我们已知相机的外参（即世界坐标系到相机坐标系的变换关系），因此我们不假设平面的坐标系与世界坐标系对齐。假设平面在相机 $C_1$ 下的法向量为 $\mathbf{n}$，平面到相机 $C_1$ 的距离为 $d$，并且对于平面上的点 $P_{C_1}$，满足以下关系：

$${\mathbf{n}}^T{P}_{C_1}+d=0$$

其中 ${\mathbf{n}}^T{P}_{C_1}$ 表示点到法向量的投影。

假设相机 $C_1$ 的外参为 $T_{C_{1}W}=[R_1|{\mathbf{t}}_1]$，相机 $C_2$ 的外参为 $T_{C_{2}W}=[R_2|{\mathbf{t}}_2]$。根据变换矩阵的性质，可以得到：

$$T_{W{C}_1}=T_{C_{1}W}^{-1}={\left[\begin{array}{cc}{R}_1& {\mathbf{t}}_1\\ 0^T& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}{R}_1^{-1}& -R_1^{-1}{\mathbf{t}}_1\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(2-5)}$$

因此，相机 1 到相机 2 的变换矩阵为：

$$T_{C_2{C}_1}=T_{C_{2}W}{T}_{W{C}_1}=\left[\begin{array}{cc}{R}_2& {\mathbf{t}}_2\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_1^{-1}& -R_1^{-1}{\mathbf{t}}_1\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{R}_2{R}_1^{-1}& -R_2{R}_1^{-1}{\mathbf{t}}_1+{\mathbf{t}}_2\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{21}& {\mathbf{t}}_{21}\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(2-6)}$$

根据相机投影关系，有：

$$\begin{array}{rl}& {\mathbf{p}}_1=\frac{1}{Z_{C_1}}{K}_1{P}_1\\ & {\mathbf{p}}_2=\frac{1}{Z_{C_2}}{K}_2{P}_2\end{array}\text{\hspace{1em}(2-7)}$$

$$P_1=Z_{C_1}{K}_1^{-1}{\mathbf{p}}_1\text{\hspace{1em}(2-8)}$$

又 $P_1,P_2$ 为点 $P_W$ 在两个相机坐标系下的坐标，这两个点又可以通过下面的变换联系：

$$P_2=R_{21}{P}_1+{\mathbf{t}}_{21}\text{\hspace{1em}(2-9)}$$

联合（2-7）到（2-9），可以得出：

$${\mathbf{p}}_2=\frac{1}{Z_{C_2}}{K}_2(R_{21}{Z}_{C_1}{K}_1^{-1}{\mathbf{p}}_1+{\mathbf{t}}_{21})\text{\hspace{1em}(2-10)}$$

对于平面，假设在相机 $C_1$ 坐标系下，对于平面上的点 $P_1$，满足：

$$\frac{{\mathbf{n}}^T{P}_1}{-d}=1\text{\hspace{1em}(2-11)}$$

将（2-11）带入公式（2-10）中的 ${\mathbf{t}}_{21}$ 可以得到：

$$\begin{array}{rl}{\mathbf{t}}_{21}& ={\mathbf{t}}_{21}\frac{{\mathbf{n}}^T{P}_1}{-d}\\ & =\frac{{\mathbf{t}}_{21}{\mathbf{n}}^T}{-d}{P}_1\\ & \stackrel{2-8}{=}-\frac{{\mathbf{t}}_{21}{\mathbf{n}}^T}{d}{Z}_{C_1}{K}_1^{-1}{\mathbf{p}}_1\end{array}\text{\hspace{1em}(2-12)}$$

将（2-12）代入公式（2-10）中，可以得到：

$$\begin{array}{rl}{\mathbf{p}}_2& =\frac{1}{Z_{C_2}}{K}_2\left(R_{21}-\frac{{\mathbf{t}}_{21}{\mathbf{n}}^T}{d}\right)Z_{C_1}{K}_1^{-1}{\mathbf{p}}_1\\ & =H_{21}{\mathbf{p}}_1\end{array}\text{\hspace{1em}(2-13)}$$

因此，单应矩阵的表达形式为：

$$H_{21}=\frac{Z_{C_1}}{Z_{C_2}}{K}_2\left(R_{21}-\frac{{\mathbf{t}}_{21}{\mathbf{n}}^T}{d}\right)K_1^{-1}\text{\hspace{1em}(2-14)}$$

如果将 $R_{21}$ 与 $t_{21}$ 的展开形式（公式 2-6），则可以得到：

$$H_{21}=\frac{Z_{C_1}}{Z_{C_2}}{K}_2\left(R_2{R}_1^{-1}-\frac{\left(-R_2{R}_1^{-1}{\mathbf{t}}_1+{\mathbf{t}}_2\right){\mathbf{n}}^T}{d}\right)K_1^{-1}\text{\hspace{1em}(2-15)}$$

3理论求解方法

2d射影变换定义
平面射影变换是关于齐次 3 维矢量的一种线性变换,并可用一个非奇异 $3\times 3$ 矩阵 H 表示为:

$$\left[\begin{array}{l}{x}_1^{\prime }\\ x_2^{\prime }\\ x_3^{\prime }\end{array}\right]=\left[\begin{array}{lll}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\\ x_3\end{array}\right]\text{\hspace{1em}(1)}$$

或更简洁地表示为 ${\mathbf{x}}^{\prime }=\mathrm{Hx}$.
注意：此方程中的矩阵 H 乘以任意一个非零比例因子不会使射影变换改变. 换句话说 H 是一个齐次矩阵，与点的齐次表示一样，有意义的仅仅是矩阵元素的比率。在 H 的九个元素中有八个独立比率,因此一个射影变换有八个自由度。
将x看成向量，则$x^{\prime }和Hx$同一方向，则其叉乘$x^{\prime }\times Hx=0$.
如果将矩阵 H 的第 $j$ 行记为 ${\mathbf{h}}^{jT}$(行向量), 那么由矩阵乘法法则可得：

$$\mathrm{H}{\mathbf{x}}_i=\left(\begin{array}{l}{\mathbf{h}}^{1T}{\mathbf{x}}_i\\ {\mathbf{h}}^{2T}{\mathbf{x}}_i\\ {\mathbf{h}}^{3T}{\mathbf{x}}_i\end{array}\right)\text{\hspace{1em}(2)}$$

记 ${\mathbf{x}}_i^{\prime }={\left(x_i^{\prime },y_i^{\prime },w_i^{\prime }\right)}^{\mathrm{T}}$, 则叉积定义可以显式地写成：

$${\mathbf{x}}_i^{\prime }\times \mathrm{H}{\mathbf{x}}_i=\left(\begin{array}{c}{y}_i^{\prime }{\mathbf{h}}^{3T}{\mathbf{x}}_i-w_i^{\prime }{\mathbf{h}}^{2T}{\mathbf{x}}_i\\ w_i^{\prime }{\mathbf{h}}^{1T}{\mathbf{x}}_i-x_i^{\prime }{\mathbf{h}}^{3T}{\mathbf{x}}_i\\ x_i^{\prime }{\mathbf{h}}^{2T}{\mathbf{x}}_i-y_i^{\prime }{\mathbf{h}}^{1T}{\mathbf{x}}_i\end{array}\right).\text{\hspace{1em}(3)}$$

因为对 $j=1,2,3,{\mathbf{h}}^{jT}{\mathbf{x}}_i={\mathbf{x}}_i^T{\mathbf{h}}^j$ 皆成立(其结果是一个常数，因此这样是成立的),这就给出关于 H 元素的三个方程,并可以写成下列形式

$$\left[\begin{array}{ccc}{0}^T& -w_i^{\prime }{\mathbf{x}}_i^T& y_i^{\prime }{\mathbf{x}}_i^T\\ w_i^{\prime }{\mathbf{x}}_i^T& 0^T& -x_i^{\prime }{\mathbf{x}}_i^T\\ -y_i^{\prime }{\mathbf{x}}_i^T& x_i^{\prime }{\mathbf{x}}_i^T& 0^T\end{array}\right]\left[\begin{array}{l}{\mathbf{h}}^1\\ {\mathbf{h}}^2\\ {\mathbf{h}}^3\end{array}\right]=0.\text{\hspace{1em}(4)}$$

这些方程都有 $A_i\mathbf{h}=0$ 的形式, 其中 $A_i$ 是 $3\times 9$ 的矩阵, $\mathbf{h}$ 是由矩阵 $H$ 的元素组成的 9 维矢量，由于上述方程组中第一个等式乘以$x_i$加上第二个等式乘以$y_i$,再除以$-w_i$可以得到第三个等式，因此上述方程组是线性相关的，其秩为2.因此舍弃第三个方程得到线性无关的方程组：
$$\left[\begin{array}{ccc}{0}^T& -w_i^{\prime }{\mathbf{x}}_i^T& y_i^{\prime }{\mathbf{x}}_i^T\\ w_i^{\prime }{\mathbf{x}}_i^T& 0^T& -x_i^{\prime }{\mathbf{x}}_i^T\end{array}\right]\left[\begin{array}{l}{\mathbf{h}}^1\\ {\mathbf{h}}^2\\ {\mathbf{h}}^3\end{array}\right]=0.\text{\hspace{1em}(4)}$$

$$\mathrm{H}=\left[\begin{array}{lll}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right],$$
$$\mathbf{h}=\left(\begin{array}{l}{\mathbf{h}}^1\\ {\mathbf{h}}^2\\ {\mathbf{h}}^3\end{array}\right)=\left(\begin{array}{l}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\\ h_9\end{array}\right)$$

其中 $h_i$ 是 $\mathbf{h}$ 的第 $i$ 个元素. 将等式(5)展开得：
$$\left[\begin{array}{ccccccccc}0& 0& 0& -w_i^{\prime }{x}_i& -w_i^{\prime }{y}_i& -w_i^{\prime }{w}_i& y_i^{\prime }{x}_i& y_i^{\prime }{y}_i& y_i^{\prime }{w}_i\\ w_i^{\prime }{x}_i& w_i^{\prime }{y}_i& w_i^{\prime }{w}_i& 0& 0& 0& -x_i^{\prime }{x}_i& -x_i^{\prime }{y}_i& -x_i^{\prime }{w}_i\end{array}\right]\left[\begin{array}{l}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\\ h_9\end{array}\right]=0.\text{\hspace{1em}(6)}$$
即$A\mathbf{h}=0$。每个匹配对提供两个方程，单应矩阵一共八个线性无关的元素，因此最少需要四个匹配对才能求解出单应矩阵。
齐次解(Ax=0):
观察等式(6)其实是一个线性方程组求解问题，当有4个匹配点对，即8个方程时，此时是适定方程组即未知量的秩等于系数矩阵的秩，此时可以利用线性方程组高斯消去法、求逆解法或者迭代解法求解单应矩阵元素(参考索尔的《数值分析》)。如果匹配点对超对4个，即线性矩阵秩大于未知量的秩，此时选择最小二乘法则求解齐次线性超定方程组，这里其实一个通用的解法，使用svd求解，系数矩阵$A$的最小特征值对应的特征向量为单应矩阵元素。
非齐次解(Ax=b):
既然单应矩阵有8个线性无关的元素，我们不妨假设其中一个元素为常数，例如假设最后一个元素$h_9=1$，等式(6)变为:
$$\left[\begin{array}{cccccccc}0& 0& 0& -w_i^{\prime }{x}_i& -w_i^{\prime }{y}_i& -w_i^{\prime }{w}_i& y_i^{\prime }{x}_i& y_i^{\prime }{y}_i\\ w_i^{\prime }{x}_i& w_i^{\prime }{y}_i& w_i^{\prime }{w}_i& 0& 0& 0& -x_i^{\prime }{x}_i& -x_i^{\prime }{y}_i\end{array}\right]\left[\begin{array}{l}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right]=\left[\begin{array}{l}-y_i^{\prime }{w}_i\\ x_i^{\prime }{w}_i\end{array}\right].\text{\hspace{1em}(7)}$$
这里就是slam14讲公式(7.20)的方程组。使用最小二乘求解非齐次方程组即可。使用非齐次形式存在一个问题，即假设的元素如果为0等式7是不成立的，因此这种方法存在一个缺陷，不推荐这种计算方法。
将上述求解单应矩阵的过程称为DLT(direct linear transform).

4提高精度和稳健性的做法

4.1归一化

归一化的目的主要是降低数据噪声的影响，提高单应矩阵的精度，此外归一化具有相似不变性，即经过归一化求得的单应矩阵与原数据求解的结果是完全等价的，可以看一下我自己的理解.
这里有两种方式：
ORB slam方法:
$$\begin{array}{rl}& \mu =\frac{1}{N}\sum _1^{N}x\\ & \sigma =\sum _1^N\frac{|x-\mu |}{N}\\ & x_{\text{norm }}=\frac{x-\mu }{\sigma }\end{array}$$
这里不清楚是否具有相似不变性(未经过证明)。
MVG方法：
归一化$x$:
计算一个只包括位移和缩放的相似变换$T$,将点$x$变到新的点集$x$,使得点$x$的形心位于原点$(0,0)$,并且它们到原点的平均距离是$\sqrt{2}$.

4.2RanSAC

将ransac应用于单应矩阵求解可以降低噪点和错误点对对单应矩阵的影响。这里不再介绍ransac方法，具体参考维基百科，在MVG中如下：

5完整的计算过程

将第2节中的方法与第1节的理论结合，得到完整的单应矩阵计算方法：

特别地说明：针对ransac的结果，再利用LM算法最小化双向重投影误差，这里好像有点问题：即LM一般是用来求解非线性最小二乘问题的。重投影误差是最小二乘问题，而双向重投影误差并不是一个最小二乘问题，因此是无法用LM最小化双向重投影误差的，我翻阅了opencv利用LM求解单应矩阵的代码，发现opencv使用的也仅是重投影误差(opencv\calib3d\src\fundam.cpp)。

6 codes by c plus plus

三个函数构成

//
#include <iostream>
#include<random>
#include<opencv2/calib3d.hpp>
#include<opencv2/core.hpp>
#include<opencv2/stitching.hpp>
#include<opencv2/features2d.hpp>
#include<opencv2/highgui.hpp>
#include <opencv2/flann.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>#include<Eigen/Core>
#include<Eigen/SVD>
#include<Eigen/QR>
#include<Eigen/Dense>//归一化操作
void normalizePoints2(const std::vector<Eigen::Vector3d>& origin_pts, std::vector<Eigen::Vector3d>& normalized_pts, Eigen::Matrix3d &T)
{if (origin_pts.empty()){return;}//std::cout << "归一化\n";//如果包含无穷远点怎么处理//计算形心(质心)Eigen::Vector3d center_pt;center_pt << 0.0, 0.0, 0.0;double sum_x = 0.0, sum_y = 0.0;for (size_t i = 0; i < origin_pts.size(); i++){sum_x += origin_pts[i](0) ;sum_y += origin_pts[i](1) ;}center_pt(0) = sum_x / double(origin_pts.size());center_pt(1) = sum_y / double(origin_pts.size());center_pt(2) = 1.0;double scale = 0.0,value=0.0;for (size_t i = 0; i < origin_pts.size(); i++){value += std::sqrt(std::pow(origin_pts[i](0) - center_pt(0), 2.0) + std::pow(origin_pts[i](1) - center_pt(1), 2.0));}value /= double(origin_pts.size());scale = sqrt(2.0) / value;double tx = -1.0 * scale * center_pt(0);double ty = -1.0 * scale * center_pt(1);//相似变换矩阵T << scale, 0.0, tx,0.0, scale, ty,0.0, 0.0, 1.0;normalized_pts.resize(origin_pts.size());for (size_t i = 0; i < origin_pts.size(); i++){normalized_pts[i] = T * origin_pts[i];//T.inverse()}//std::cout << T << std::endl;//验证double mean_x = 0.0, mean_y = 0.0, dist = 0.0;for (size_t i = 0; i < normalized_pts.size(); i++){mean_x += normalized_pts[i].x();mean_y += normalized_pts[i].y();dist += std::pow((normalized_pts[i].x() * normalized_pts[i].x() + normalized_pts[i].y() * normalized_pts[i].y()), 0.5);}mean_x /= double(normalized_pts.size());mean_y /= double(normalized_pts.size());dist /= double(normalized_pts.size());//	std::cout << "归一化结果：" << mean_x << " " << mean_y << " " << dist << std::endl;}//利用svd直接求解H矩阵
void svdComputeH(const std::vector<Eigen::Vector3d>&src_pts,const std::vector<Eigen::Vector3d>&trg_pts, Eigen::Matrix3d&result)
{const int rows_num = 2 * src_pts.size();Eigen::MatrixXd A=Eigen::MatrixXd::Zero(rows_num,9);//std::cout << "A0" << A << std::endl;for (size_t i = 0; i < src_pts.size(); i++){//i行A(2*i, 0) = 0.0; A(2 * i, 1) = 0.0; A(2 * i, 2) = 0.0;A(2 * i, 3) = -1.0*src_pts[i].x(); A(2 * i, 4) = -1.0 * src_pts[i].y();A(2 * i, 5) = -1.0 ;A(2 * i, 6) =trg_pts[i].y()* src_pts[i].x(); A(2 * i, 7) = trg_pts[i].y() * src_pts[i].y(); A(2 * i, 8) = trg_pts[i].y() ;//i+1行A(2 * i + 1, 0) = src_pts[i].x();A(2 * i + 1, 1) = src_pts[i].y();A(2 * i + 1, 2) = 1.0;A(2 * i + 1, 3) = 0.0; A(2 * i + 1, 4) = 0.0; A(2 * i + 1, 5) = 0.0;A(2 * i + 1, 6) =-1.0* trg_pts[i].x() * src_pts[i].x(); A(2 * i + 1, 7) = -1.0 * trg_pts[i].x()*src_pts[i].y(); A(2 * i + 1, 8) = -1.0 * trg_pts[i].x();}
//	std::cout << "A" << A << std::endl;Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::MatrixXd V = svd.matrixV();Eigen::MatrixXd D = svd.singularValues();//寻找最小奇异值对应的特征向量double max_num = double(INT32_MAX);Eigen::MatrixXd H;//for (size_t i = 0; i < D.rows(); i++)//{//	if (D(i, 0) < max_num)//	{//		max_num = D(i, 0);//		H = V.col(i);//	}//}H = V.col(8);/*std::cout << "D" << D << std::endl;std::cout << "v" << V << std::endl;std::cout << H << std::endl;*/result << H(0,0), H(1, 0), H(2, 0),H(3, 0), H(4, 0), H(5, 0),H(6, 0), H(7, 0), H(8, 0);//std::cout << result << std::endl;
}//利用ransac求解H矩阵
void ransacComputeH(std::vector<Eigen::Vector3d>& src_pts, std::vector<Eigen::Vector3d>& trg_pts, Eigen::Matrix3d& result)
{int point_size = src_pts.size();double dist_t = 10;//像素int max_inlier_num_T = 0.8* point_size;int max_iter_num = 30;int iter_count = 0;//每次迭代的结果对应的索引std::vector<std::vector<int> >inlier_x_indices{};std::vector<std::vector<int> >inlier_y_indices{};std::vector<int> inlier_indices{};srand(time(NULL));while(true){//inlier_indices.clear();++iter_count;//随机抽选一定数量点，理论上四个点就可以std::vector<int>random_indices;std::vector< Eigen::Vector3d> random_x, random_y;for (size_t i = 0; i <8; i++){int random_num = rand() % point_size;//随机选择一个点//	std::cout << "random_num" << random_num << std::endl;random_indices.push_back(random_num);random_x.push_back(src_pts[random_num]);random_y.push_back(trg_pts[random_num]);}//归一化std::vector< Eigen::Vector3d> normalized_random_x, normalized_random_y;Eigen::Matrix3d Tx, Ty;normalizePoints2(random_x, normalized_random_x, Tx);normalizePoints2(random_y, normalized_random_y, Ty);Eigen::Matrix3d normalized_random_result;svdComputeH(normalized_random_x, normalized_random_y, normalized_random_result);//逆变换为原始的HEigen::Matrix3d random_result=Ty.inverse()*normalized_random_result*Tx;std::cout << iter_count<<" " << random_result<< std::endl;//计算残差（距离）int inlier_num = 0;std::vector<int> inlier_indices_tmp{};for (size_t i = 0; i < point_size; i++){Eigen::Vector3d src_x= src_pts[i];Eigen::Vector3d computed_trg_pt = random_result * src_x;//利用计算结果得到的理论值double computed_trg_pt_x = computed_trg_pt(0) / computed_trg_pt(2);double computed_trg_pt_y = computed_trg_pt(1) / computed_trg_pt(2);double computed_dist = std::pow(std::pow(computed_trg_pt_x - trg_pts[i](0), 2) + std::pow(computed_trg_pt_y - trg_pts[i](1), 2), 0.5);//inlierif (computed_dist<dist_t){++inlier_num;inlier_indices_tmp.push_back(i);}}//寻找最大数量的内点集合（最大一致集）if (inlier_indices.size()<inlier_indices_tmp.size()){inlier_indices = inlier_indices_tmp;}if (iter_count >= max_iter_num || inlier_num > max_inlier_num_T){break;}}//选择最大内点集重新估计参数std::vector< Eigen::Vector3d> inlier_x, inlier_y;for (size_t i = 0; i < inlier_indices.size(); i++){inlier_x.push_back(src_pts[inlier_indices[i]]);inlier_y.push_back(trg_pts[inlier_indices[i]]);}std::vector< Eigen::Vector3d> normalized_inlier_x, normalized_inlier_y;Eigen::Matrix3d Tx, Ty, normalized_inlier_result;normalizePoints2(inlier_x, normalized_inlier_x, Tx);normalizePoints2(inlier_y, normalized_inlier_y, Ty);svdComputeH(normalized_inlier_x, normalized_inlier_y, normalized_inlier_result);result = Ty.inverse() * normalized_inlier_result * Tx;
}//图像拼接验证H矩阵的正确性
bool featureDetecteAndMatch2(cv::Mat& img1, cv::Mat& img2,int H_method=0)
{using namespace cv;using namespace std;Mat g1(img1, Rect(0, 0, img1.cols, img1.rows));  // init roiMat g2(img2, Rect(0, 0, img2.cols, img2.rows));cvtColor(g1, g1, COLOR_BGR2GRAY);cvtColor(g2, g2, COLOR_BGR2GRAY);vector<cv::KeyPoint> keypoints_roi, keypoints_img;  /* keypoints found using SIFT */Mat descriptor_roi, descriptor_img;                           /* Descriptors for SIFT */vector<cv::DMatch> matches, good_matches;//cv::Ptr<cv::SIFT> sift = cv::SIFT::create();//Ptr<cv::ORB>orb_feature = ORB::create(1000);//Ptr<cv::BRISK>  brisk_feature = BRISK::create(1000);Ptr<cv::AKAZE>  akaze_feature = AKAZE::create();int i, dist = 80;akaze_feature->detectAndCompute(g1, cv::Mat(), keypoints_roi, descriptor_roi);      /* get keypoints of ROI image */akaze_feature->detectAndCompute(g2, cv::Mat(), keypoints_img, descriptor_img);         /* get keypoints of the image *///FlannBasedMatcher matcher;                                   /* FLANN based matcher to match keypoints *///matcher.match(descriptor_roi, descriptor_img, matches);  //实现描述符之间的匹配Ptr<DescriptorMatcher> matche = DescriptorMatcher::create("BruteForce-Hamming");matche->match(descriptor_roi, descriptor_img, matches);double max_dist = 0; double min_dist = 5000;//-- Quick calculation of max and min distances between keypointsfor (int i = 0; i < descriptor_roi.rows; i++){double dist = matches[i].distance;if (dist < min_dist) min_dist = dist;if (dist > max_dist) max_dist = dist;}// 特征点筛选for (i = 0; i < descriptor_roi.rows; i++){if (matches[i].distance < 5 * min_dist){good_matches.push_back(matches[i]);}}printf("%ld no. of matched keypoints in right image\n", good_matches.size());/* Draw matched keypoints */Mat img_matches;//绘制匹配drawMatches(img1, keypoints_roi, img2, keypoints_img,good_matches, img_matches, Scalar::all(-1),Scalar::all(-1), vector<char>(),DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);//cv::namedWindow("matches_image", cv::WINDOW_NORMAL);imshow("matches_image", img_matches);//waitKey(0);vector<Point2f> keypoints1, keypoints2;for (i = 0; i < good_matches.size(); i++){keypoints1.push_back(keypoints_img[good_matches[i].trainIdx].pt);keypoints2.push_back(keypoints_roi[good_matches[i].queryIdx].pt);}//计算单应矩阵(射影变换矩阵)Mat H=cv::Mat_<double>(3,3); Mat H2=cv::Mat_<double>(3, 3);//利用opencv自带的计算单应if (H_method==0){H= findHomography(keypoints1, keypoints2, RANSAC);H2= findHomography(keypoints2, keypoints1, RANSAC);}//利用自己写的计算单应else if (H_method == 1){std::vector<Eigen::Vector3d> src_pts, trg_pts;src_pts.resize(keypoints1.size());trg_pts.resize(keypoints2.size());for (size_t i = 0; i < keypoints1.size(); i++){src_pts[i](0) = keypoints1[i].x;src_pts[i](1) = keypoints1[i].y;src_pts[i](2) = 1.0;trg_pts[i](0) = keypoints2[i].x;trg_pts[i](1) = keypoints2[i].y;trg_pts[i](2) = 1.0;}Eigen::Matrix3d H_result, H_result2;ransacComputeH(src_pts, trg_pts, H_result);ransacComputeH(trg_pts ,src_pts, H_result2);for (size_t i = 0; i < 3; ++i){for(size_t j = 0; j < 3; ++j){H.at<double>(i, j) = H_result(i, j)*(1.0/ H_result(2, 2)); //*(1.0 / H_result(2, 2)):把最后一个元素变为1，和opencv保持一致H2.at<double>(i, j) = H_result2(i, j) * (1.0 / H_result2(2, 2));}}}std::cout << "H:\n" << H << std::endl;Mat stitchedImage;  //定义射影变换后的图像(也是拼接结果图像)Mat stitchedImage2;  //定义射影变换后的图像(也是拼接结果图像)int mRows = img2.rows;if (img1.rows > img2.rows){mRows = img1.rows;}int count = 0;for (int i = 0; i < keypoints2.size(); i++){if (keypoints2[i].x >= img2.cols / 2)count++;}//判断匹配点位置来决定图片是左还是右if (count / float(keypoints2.size()) >= 0.5)  //待拼接img2图像在右边{cout << "img1 should be left" << endl;vector<Point2f>corners(4);vector<Point2f>corners2(4);corners[0] = Point(0, 0);corners[1] = Point(0, img2.rows);corners[2] = Point(img2.cols, img2.rows);corners[3] = Point(img2.cols, 0);stitchedImage = Mat::zeros(img2.cols + img1.cols, mRows, CV_8UC3);warpPerspective(img2, stitchedImage, H, Size(img2.cols + img1.cols, mRows));perspectiveTransform(corners, corners2, H);/*circle(stitchedImage, corners2[0], 5, Scalar(0, 255, 0), 2, 8);circle(stitchedImage, corners2[1], 5, Scalar(0, 255, 255), 2, 8);circle(stitchedImage, corners2[2], 5, Scalar(0, 255, 0), 2, 8);circle(stitchedImage, corners2[3], 5, Scalar(0, 255, 0), 2, 8); */cout << corners2[0].x << ", " << corners2[0].y << endl;cout << corners2[1].x << ", " << corners2[1].y << endl;imshow("temp", stitchedImage);//imwrite("temp.jpg", stitchedImage);Mat half(stitchedImage, Rect(0, 0, img1.cols, img1.rows));img1.copyTo(half);imshow("result", stitchedImage);}else  //待拼接图像img2在左边{cout << "img2 should be left" << endl;stitchedImage = Mat::zeros(img2.cols + img1.cols, mRows, CV_8UC3);warpPerspective(img1, stitchedImage, H2, Size(img1.cols + img2.cols, mRows));imshow("temp", stitchedImage);//计算射影变换后的四个端点vector<Point2f>corners(4);vector<Point2f>corners2(4);corners[0] = Point(0, 0);corners[1] = Point(0, img1.rows);corners[2] = Point(img1.cols, img1.rows);corners[3] = Point(img1.cols, 0);perspectiveTransform(corners, corners2, H2);  //射影变换对应端点/*circle(stitchedImage, corners2[0], 5, Scalar(0, 255, 0), 2, 8);circle(stitchedImage, corners2[1], 5, Scalar(0, 255, 255), 2, 8);circle(stitchedImage, corners2[2], 5, Scalar(0, 255, 0), 2, 8);circle(stitchedImage, corners2[3], 5, Scalar(0, 255, 0), 2, 8); */cout << corners2[0].x << ", " << corners2[0].y << endl;cout << corners2[1].x << ", " << corners2[1].y << endl;Mat half(stitchedImage, Rect(0, 0, img2.cols, img2.rows));img2.copyTo(half);imshow("result", stitchedImage);}imwrite("result2.bmp", stitchedImage);return true;
}
#include<fstream>
int main()
{//1 比较利用上述代码计算的H矩阵与opencv计算的H矩阵差异//读取文件std::vector<cv::Point2d>src_cv_pts, trg_cv_pts;std::vector<Eigen::Vector3d>src_points;std::ifstream fin1("./pts1.txt");while (true){if (fin1.eof()){break;}Eigen::Vector3d pt;double x, y;fin1 >> x >> y;pt(0) = x;pt(1) = y;pt(2) = 1.0;src_points.push_back(pt);cv::Point2d pp;pp.x = x;pp.y = y;src_cv_pts.push_back(pp);}std::vector<Eigen::Vector3d>trg_points, normalized_points;std::ifstream fin2("./pts2.txt");while (true){if (fin2.eof()){break;}Eigen::Vector3d pt;double x, y;fin2 >> x >> y;pt(0) = x;pt(1) = y;pt(2) = 1.0;trg_points.push_back(pt);cv::Point2d pp;pp.x = x;pp.y = y;trg_cv_pts.push_back(pp);}std::cout << "point size:" << src_points.size() << " " << trg_points.size() << std::endl;Eigen::Matrix3d H0,H00;svdComputeH(src_points, trg_points, H0);double scale0 = 1.0 / H0(2, 2);for (size_t i = 0; i < 3; i++){for (size_t j = 0; j < 3; j++){H00(i, j) = H0(i, j) * scale0;}}std::cout << "h0:\n" << H0 << std::endl;std::cout << "h00:\n" << H00 << std::endl;Eigen::Matrix3d HH;ransacComputeH(src_points, trg_points, HH);std::cout << "result:\n" << HH << std::endl;Eigen::Matrix3d hhh;double scale = 1.0 / HH(2, 2);for (size_t i = 0; i < 3; i++){for (size_t j = 0; j < 3; j++){hhh(i, j) = HH(i, j) * scale;}}std::cout << "result2:\n" << hhh << std::endl;cv::Mat opencv_H = cv::findHomography(src_cv_pts, trg_cv_pts, cv::RANSAC);std::cout << "opencv result:" << opencv_H << std::endl;//2利用图像拼接检查H矩阵计算的效果std::cout << "image stitching\n:";//cv::Mat img1 = cv::imread("7.jpg");cv::Mat img2 = cv::imread("8.jpg");featureDetecteAndMatch2(img1, img2,1);std::cout << "Hello World!\n";
}

原始图像：

图像拼接效果

参考

1
2
3
[4] slam14讲-单应矩阵
[5] 计算机视觉中多视图几何