如何根据4组点对求解单应矩阵(C++版)

引言

最近一个项目用到了单应矩阵的求解，虽然Eigen和Opencv都有相应的现成接口，但出于某些原因不允许依赖过多的第三方库，所以只好自己搞个。好在大佬们已经有过很多现成的代码了，这是只是搬运一下。
具体整理后的代码已上传我的github
https://github.com/PotterSu/FindHomography/

在此感谢大佬们的工作
https://scm_mos.gitlab.io/vision/homography-matrix/
本文的程序主要参考下面的Git,只是把其中求解部分剥离出来并做了简单的修改
https://github.com/roymacdonald/ofxGLWarper

求解过程

1.首先，求解单应矩阵需要四组对应点对，我们先设定四组对应点

//原始四点坐标
    cv::Point2f srcPoint1(0.0f, 0.0f);
    cv::Point2f srcPoint2(0.0f, 1.0f);
    cv::Point2f srcPoint3(1.0f, 0.0f);
    cv::Point2f srcPoint4(1.0f, 1.0f);
    //目标四点坐标
    cv::Point2f dstPoint1(0.0f, 0.0f);
    cv::Point2f dstPoint2(0.0f, 1.0f);
    cv::Point2f dstPoint3(1.0f, 0.0f);
    cv::Point2f dstPoint4(2.0f, 2.0f);

    std::vector<cv::Point2f> src;
    src.push_back(srcPoint1);
    src.push_back(srcPoint2);
    src.push_back(srcPoint3);
    src.push_back(srcPoint4);
    std::vector<cv::Point2f> dst;
    dst.push_back(dstPoint1);
    dst.push_back(dstPoint2);
    dst.push_back(dstPoint3);
    dst.push_back(dstPoint4);

2.接下来根据这四组对应点来构建8组单应矩阵方程，这里具体的推导过程可以参考上面的博客，讲述的比较清楚。
如下所示，根据4组对应点对可以得到8行9列的矩阵，每行代表一个方程关系，前8列为原始坐标构建的系数矩阵，最后一列为目标点的坐标，具体对应代码

float P[8][9]=
   {
   	{-src[0].x, -src[0].y, -1,   0,   0,  0, src[0].x * dst[0].x, src[0].y * dst[0].x, -dst[0].x }, // h11
   	{  0,   0,  0, -src[0].x, -src[0].y, -1, src[0].x * dst[0].y, src[0].y * dst[0].y, -dst[0].y }, // h12

   	{-src[1].x, -src[1].y, -1,   0,   0,  0, src[1].x * dst[1].x, src[1].y * dst[1].x, -dst[1].x }, // h13
   	{  0,   0,  0, -src[1].x, -src[1].y, -1, src[1].x * dst[1].y, src[1].y * dst[1].y, -dst[1].y }, // h21

   	{-src[2].x, -src[2].y, -1,   0,   0,  0, src[2].x * dst[2].x, src[2].y * dst[2].x, -dst[2].x }, // h22
   	{  0,   0,  0, -src[2].x, -src[2].y, -1, src[2].x * dst[2].y, src[2].y * dst[2].y, -dst[2].y }, // h23

   	{-src[3].x, -src[3].y, -1,   0,   0,  0, src[3].x * dst[3].x, src[3].y * dst[3].x, -dst[3].x }, // h31
   	{  0,   0,  0, -src[3].x, -src[3].y, -1, src[3].x * dst[3].y, src[3].y * dst[3].y, -dst[3].y }, // h32
   };

3.最后利用高斯消元法来求解上面的矩阵数组，这样可以得到方程的各个系数(即单应矩阵)，这部分相对于原作者的写法没有改动。

static void gaussian_elimination(float *input, int n)
{
   float * A = input;
   int i = 0;
   int j = 0;
   //m = 8 rows, n = 9 cols
   int m = n-1;
   while (i < m && j < n)
   {
   	// Find pivot in column j, starting in row i:
   	int maxi = i;
   	for(int k = i+1; k < m; k++)
       {
           //选取第j列最大的数，并记录行
   		if(fabs(A[k * n + j]) > fabs(A[maxi * n + j]))
           {
   			maxi = k;
   		}
   	}
   	if (A[maxi * n + j] != 0)
       {
   		//swap rows i and maxi, but do not change the value of i
   		if(i != maxi)
   			for(int k = 0; k < n; k++)
               {
   				float aux = A[i * n + k];
   				A[i * n + k] = A[maxi * n + k];
   				A[maxi * n + k] = aux;
   			}
   		//Now A[i,j] will contain the old value of A[maxi,j].
   		//divide each entry in row i by A[i,j]
           //将主行归一化
   		float A_ij = A[i * n + j];
   		for(int k = 0; k < n; k++)
           {
   			A[i * n + k] /= A_ij;
   		}
   		//Now A[i,j] will have the value 1.
           //主行*A[u,j]，再用A[u,j]-该数即可消除
   		for(int u = i+1; u< m; u++)
           {
   			//subtract A[u,j] * row i from row u
   			float A_uj = A[u * n + j];
   			for(int k = 0; k <n; k++)
               {
   				A[u * n + k] -= A_uj * A[i * n + k];
   			}
   			//Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
   		}
   		i++;
   	}
   	j++;
   }

   //back substitution
   //最后一位不用管，其他各行用最后一个数-前面各列数*已求的未知数
   for(int i = m-2; i >= 0; i--)
   {
   	for(int j = i+1; j < n-1; j++)
       {
   		A[i * n + m] -= A[i * n + j] * A[j * n + m];
   	}
   }
}

4.最后展示了跟OpenCV的findHomography求解的结果对比情况，会有一定误差不过基本准确。