单应性矩阵Homography计算和优化

单应性矩阵，是用来描述两个平面之间的变换关系，是一个3x3的齐次矩阵。

图上的4个绿色的圈，两两可以对应，H可以表达第一张图变换到第二张图的转换关系。具体的表达式：

$a$表示尺度信息，$h_1-h_9$表示$H$矩阵，$H$矩阵有8个自由度，$h_9=1$,所以只需要4对点就可以计算出。

首先展开：

然后可以前两式比上第三式，这样可以把系数$a$约掉。并让等式右边等于0。

展开成矩阵的形式 $A_ih = 0 $的形式

把所有点对都考虑进来 $Ah=0$

求解方程组，可以利用DLT（Direct Linear Transform 直接线性变换）

具体的求解步骤就是利用最小二乘法，通过SVD 分解求的：

1. 对$A$进行SVD分解, A的大小是$2nX9$， $n$ 表示点对数

   $$A=UE{V}^T$$

2. $h$的值是最小的奇异值对应的V中的特征向量，也就是V的最后一列

3. 默认$h_9$的值不等于1，所以每个$h_i=h_i/h_9$

关于单应性矩阵的优化：

一、归一化

一般单应性矩阵的的点对的数值，可能会相差很大，这样会对结果的精度有影响，如果利用优化算法求解，结果收敛不会那么快，根据多视觉立体几何中的计算可以先对匹配点进行归一化操作，求出结果后，再进行一个逆变换成最终的结果。

具体的步骤：有对应点 $X_i$ 和 $X_i^{{}^{\prime }}$

1. 对$X_i$ 存在相似变换矩阵$T$，$T$只对点进行尺度和平移变换，${\hat{X}}_i=T{X}_i$ 关于T，先计算X点的平均值$cx$ 和 $cy$, 然后对X点去除中心，也就是平移到(0, 0),计算尺度因子$sx$和$sy$, 统计去除中心的X点到中心点（0， 0）的平均距离，$ stdx, stdy$， 那么尺度 $sx=stdx/\sqrt{2}$, $sy=stdy/\sqrt{2}$, 映射的最大距离是$\sqrt{2}$.
2. 同理，对$X_i^{{}^{\prime }}$求解 $T^{{}^{\prime }}$, ${\hat{X}}_i=T^{{}^{\prime }}{X}_i$
3. 对$X_i$ 和 $X_i^{{}^{\prime }}$,进行DLT变换，得$\hat{H}$
4. 对$\hat{H}$进行逆变换： $H=T^{{}^{\prime }-1}\hat{H}$T

二、优化算法

当有足够多的匹配点对，最小二乘法求解的结果，并不一定最小化他们之间误差。

D i s { X i , H X i ′ } Dis\{Xi, HX_{i}^{'}\} Dis{Xi,HXi′​}

关于优化算法，这里暂时不讨论了，常用的凸优化算法有，最小梯度下降法、高斯-牛顿算法、LM算法等。

实战阶段，引用了Eigen库，核心代码如下：

// 归一化
void normal ( MatrixXd& P, Matrix3d& T )
{

    double cx = P.col ( 0 ).mean();
    double cy = P.col ( 1 ).mean();

    P.array().col ( 0 ) -= cx;
    P.array().col ( 1 ) -= cy;

    double stdx = sqrt ( ( P.col ( 0 ).transpose() * P.col ( 0 ) ).mean() );
    double stdy = sqrt ( ( P.col ( 1 ).transpose() * P.col ( 1 ) ).mean() );
 

    double sqrt_2 = sqrt ( 2 );
    double scalex = sqrt_2 / stdx;
    double scaley = sqrt_2 / stdy;

    P.array().col(0) *= scalex;
    P.array().col(1) *= scalex;
    
    T << scalex, 0, -scalex*cx,
    0, scaley, -scaley*cy,
    0, 0, 1;
    
    
}

//DLT 计算 H矩阵
VectorXd solveHomographyDLT ( MatrixXd& srcNormal, MatrixXd& d
stNormal )
{

    int n = srcNormal.rows();
    // 2. DLT
    MatrixXd input ( 2*n, 9 );

    for ( int i=0; i<n; ++i ) {

        input ( 2*i, 0 ) = 0.;
        input ( 2*i, 1 ) = 0.;
        input ( 2*i, 2 ) = 0.;
        input ( 2*i, 3 ) = srcNormal ( i, 0 );
        input ( 2*i, 4 ) = srcNormal ( i, 1 );
        input ( 2*i, 5 ) = 1.;
        input ( 2*i, 6 ) = -srcNormal ( i, 0 ) * dstNormal ( i, 1 );
        input ( 2*i, 7 ) = -srcNormal ( i, 1 ) * dstNormal ( i, 1 );
        input ( 2*i, 8 ) = -dstNormal ( i, 1 );

        input ( 2*i+1, 0 ) = srcNormal ( i, 0 );
        input ( 2*i+1, 1 ) = srcNormal ( i, 1 );
        input ( 2*i+1, 2 ) = 1.;
        input ( 2*i+1, 3 ) = 0.;
        input ( 2*i+1, 4 ) = 0.;
        input ( 2*i+1, 5 ) = 0.;
        input ( 2*i+1, 6 ) = -srcNormal ( i, 0 ) * dstNormal ( i, 0 );
        input ( 2*i+1, 7 ) = -srcNormal ( i, 1 ) * dstNormal ( i, 0 );
        input ( 2*i+1, 8 ) = -dstNormal ( i, 0 );
    }

 // 3. SVD分解
    JacobiSVD<MatrixXd> svdSolver ( input, ComputeThinU | ComputeT
hinV );
    MatrixXd V = svdSolver.matrixV();
    return V.rightCols ( 1 );
}

// 主程序
void findHomography ( std::vector<Eigen::Vector2d>& srcPoints, std::v
ector<Eigen::Vector2d>& dstPoints, Eigen::Matrix3d& H, bool isNorma
l )
{

    assert ( srcPoints.size() == dstPoints.size() );
    int n = srcPoints.size();
    MatrixXd srcNormal ( n, 3 );
    MatrixXd dstNormal ( n, 3 );

    for ( int i=0; i<n; ++i ) {

        srcNormal ( i, 0 ) = srcPoints[i] ( 0 );
        srcNormal ( i, 1 ) = srcPoints[i] ( 1 );
        srcNormal ( i, 2 ) = 1.0;

        dstNormal ( i, 0 ) = dstPoints[i] ( 0 );
        dstNormal ( i, 1 ) = dstPoints[i] ( 1 );
        dstNormal ( i, 2 ) = 1.0;
    }

    // 1. 归一化
    Matrix3d srcT, dstT; // 归一化矩阵T
    if(isNormal) {
        normal ( srcNormal, srcT );
        normal ( dstNormal, dstT );
    }
    // 2. DLT
    VectorXd v = solveHomographyDLT(srcNormal, dstNormal);
    Matrix3d M ;
    M << v(0), v(1), v(2),
    v(3), v(4), v(5),
    v(6), v(7), v(8);

    // 3. 优化 TODO
    
    // 4. 反计算H
    if(isNormal) {
        H = dstT.inverse() * M * srcT;
        H.array() /= H(8);
    } else {
        H = M;
        H.array() /= H(8);
    }
}  
    

参考文献：

OpenCV: Basic concepts of the homography explained with code

Multiple View Geometry in Computer Vision

10.2 2D Alignment - DLT (cmu.edu)