二维仿射变换与间接平差四参数计算

二维四参数转换模型

仿射变换的一般形式

[ x d e s t y d e s t ] = [ Δ x Δ y ] + ( 1 + m ) [ cos ⁡ α − sin ⁡ α sin ⁡ α cos ⁡ α ] [ x s r c y s r c ] \begin{bmatrix} x_{dest}\\ y_{dest} \end{bmatrix} =\begin{bmatrix} \Delta x\\ \Delta y \end{bmatrix}+ (1+m)\begin{bmatrix} \cos\alpha & -\sin\alpha \\ \sin\alpha & \cos\alpha \end{bmatrix} \begin{bmatrix} x_{src}\\ y_{src} \end{bmatrix} [xdest​ydest​​]=[ΔxΔy​]+(1+m)[cosαsinα​−sinαcosα​][xsrc​ysrc​​]

式中：

x s r c , y s r c ：原坐标系下平面直角坐标系，单位米 x d e s t , y d e s t ：目标坐标系下平面直角坐标系，单位米 Δ x , Δ y ：平移参数，单位米 α ：旋转参数，单位米 m ：尺度参数，无量纲 \begin{aligned} x_{src},y_{src} & \text{：原坐标系下平面直角坐标系，单位米} \hspace{100cm} \\ x_{dest},y_{dest} & \text{：目标坐标系下平面直角坐标系，单位米} \\ \Delta x ,\Delta y &\text{：平移参数，单位米} \\ \alpha &\text{：旋转参数，单位米} \\ m &\text{：尺度参数，无量纲} \end{aligned} xsrc​,ysrc​xdest​,ydest​Δx,Δyαm​：原坐标系下平面直角坐标系，单位米：目标坐标系下平面直角坐标系，单位米：平移参数，单位米：旋转参数，单位米：尺度参数，无量纲​

求解：平移、旋转和尺度四个参数。

• 步骤一 变换

  a = ( 1 + m ) cos ⁡ α b = ( 1 + m ) sin ⁡ α a=(1+m)\cos\alpha \\ b=(1+m)\sin\alpha a=(1+m)cosαb=(1+m)sinα

  原公式变为：

  [ x d e s t y d e s t ] = [ Δ x Δ y ] + [ a − b b a ] [ x s r c y s r c ] \begin{bmatrix} x_{dest}\\ y_{dest} \end{bmatrix} =\begin{bmatrix} \Delta x\\ \Delta y \end{bmatrix}+ \begin{bmatrix} a & -b \\ b & a \end{bmatrix} \begin{bmatrix} x_{src}\\ y_{src} \end{bmatrix} [xdest​ydest​​]=[ΔxΔy​]+[ab​−ba​][xsrc​ysrc​​]

• 步骤二 矩阵变换成线性回归方程

[ x d e s t y d e s t ] = [ 1 0 x s r c − y s r c 0 1 y s r c x s r c ] [ Δ x Δ y a b ] \begin{bmatrix} x_{dest}\\ y_{dest} \end{bmatrix} =\begin{bmatrix} 1 & 0 & x_{src} & -y_{src}\\ 0 & 1 & y_{src} & x_{src} \end{bmatrix} \begin{bmatrix} \Delta x\\ \Delta y\\ a\\ b \end{bmatrix} [xdest​ydest​​]=[10​01​xsrc​ysrc​​−ysrc​xsrc​​]⎣⎢⎢⎡​ΔxΔyab​⎦⎥⎥⎤​

即：

L = [ 1 0 x s r c − y s r c 0 1 y s r c x s r c ] [ Δ x Δ y a b ] + [ 0 0 ] L=\begin{bmatrix} 1 & 0 & x_{src} & -y_{src} \\ 0 & 1 & y_{src} & x_{src} \end{bmatrix} \begin{bmatrix} \Delta x\\ \Delta y\\ a\\ b \end{bmatrix}+ \begin{bmatrix} 0 \\ 0 \end{bmatrix} L=[10​01​xsrc​ysrc​​−ysrc​xsrc​​]⎣⎢⎢⎡​ΔxΔyab​⎦⎥⎥⎤​+[00​]

令：

B = [ 1 0 x s r c − y s r c 0 1 y s r c x s r c ] X = [ Δ x Δ y a b ] d = [ 0 0 ] B=\begin{bmatrix} 1 & 0 & x_{src}& -y_{src} \\ 0 & 1 & y_{src} & x_{src} \end{bmatrix}\\ X=\begin{bmatrix} \Delta x\\ \Delta y\\ a\\ b \end{bmatrix}\\ d=\begin{bmatrix} 0\\ 0 \end{bmatrix} B=[10​01​xsrc​ysrc​​−ysrc​xsrc​​]X=⎣⎢⎢⎡​ΔxΔyab​⎦⎥⎥⎤​d=[00​]

得：

L = B X + d L=BX+d L=BX+d

参数

X

估计：

X = X 0 + x X=X^0+x X=X0+x

代入上式，推导误差方程如下：

L + V = B X 0 + d + B x V = B x + ( B X 0 + d − L ) = B x − ( L − L 0 ) L+V=BX^0+d+Bx\\ V=Bx+(BX^0+d-L)=Bx-(L-L^0) L+V=BX0+d+BxV=Bx+(BX0+d−L)=Bx−(L−L0)

l = L − L 0 = [ x d e s t − x s r c y d e s t − y s r c ] V = B x − l l=L-L^0= \begin{bmatrix} x_{dest}-x_{src}\\ y_{dest}-y_{src} \end{bmatrix}\\ V=Bx-l l=L−L0=[xdest​−xsrc​ydest​−ysrc​​]V=Bx−l

误差方程，即：

[ v x v y ] = [ 1 0 x s r c − y s r c 0 1 y s r c x s r c ] [ Δ x Δ y a b ] − l \begin{bmatrix} v_x\\ v_y \end{bmatrix} =\begin{bmatrix} 1 & 0 & x_{src} & -y_{src} \\ 0 & 1 & y_{src} & x_{src} \end{bmatrix} \begin{bmatrix} \Delta x\\ \Delta y\\ a\\ b \end{bmatrix} -l [vx​vy​​]=[10​01​xsrc​ysrc​​−ysrc​xsrc​​]⎣⎢⎢⎡​ΔxΔyab​⎦⎥⎥⎤​−l

• 步骤三 间接平差

最小二乘原理：

V T P V = m i n V^TPV=min VTPV=min

得：

W = B T P l X = ( B T P B ) − 1 W σ 2 = V T P V σ = V T P V n − t W=B^TPl\\ X=(B^TPB)^{-1}W \\ \sigma ^2 =V^TPV\\ \sigma =\sqrt{\frac{V^TPV}{n-t}} W=BTPlX=(BTPB)−1Wσ2=VTPVσ=n−tVTPV​

​

• 步骤四 计算参数

  α = tan ⁡ − 1 ( b a ) m = a cos ⁡ α − 1 \alpha=\tan^{-1}(\frac{b}{a})\\ m=\frac{a}{\cos\alpha}-1 α=tan−1(ab​)m=cosαa​−1

实现代码 Eigen3

struct Data {
    double X;
    double Y;
    double Z;
};


void calculate(const QList<Data> &srcData, const QList<Data> &destData)
{
    if(srcData.count()!=destData.count())
        return;
    if(srcData.count()<2)
        return;
    //方程的格数即，条件数，t=n-4
    //3个点时，t=3*2-4=2
    //2个点时，t=2*2-4=0
    //因此，至少3个点
    int n=srcData.count()*2;
    
    MatrixXd B = MatrixXd::Zero(n,4);
    MatrixXd l = MatrixXd::Zero(n, 1);

    //矩阵初始化
    for (int i = 0; i < n; i++)
    {
        if (i % 2 == 0)
        {
            B(i, 0) = 1;
            B(i, 1) = 0;
            B(i, 2) = srcData[i / 2].X;
            B(i, 3) = -srcData[i / 2].Y;
            l(i, 0) = destData[i / 2].X - srcData[i / 2].X;
            
        }
        else
        {
            B(i, 0) = 0;
            B(i, 1) = 1;
            B(i, 2) = srcData[i / 2].Y;
            B(i, 3) = srcData[i / 2].X;
            l(i, 0) = destData[i / 2].Y - srcData[i / 2].Y;
           
        }
    }
    MatrixXd BTB = B.transpose()*B;
    MatrixXd W = B.transpose()*l;
    MatrixXd Para = BTB.inverse()*W;
    MatrixXd V = B * Para - l;
    MatrixXd sigma2 = V.transpose()*V;

    _sigma2=sqrt(sigma2(0,0)/(srcData.count()-4));

    _dx= Para(0, 0);
    _dy = Para(1, 0);
    
    double u=1.0,v=0;

    u += Para(2, 0);
    v += Para(3, 0);

    _rotation=atan(v/u);
    _scale=u/cos(_rotation);

}
           

拓展- 仿射变换齐次坐标形式

[ x 2 y 2 1 ] = [ ( 1 + m ) c o s α − ( 1 + m ) s i n α Δ x ( 1 + m ) s i n α ( 1 + m ) c o s α Δ y 0 0 1 ] [ x 1 y 1 1 ] \begin{bmatrix} x_2\\ y_2 \\ 1 \end{bmatrix} = \begin{bmatrix} (1+m)cos\alpha & -(1+m)sin\alpha & \Delta x \\ (1+m)sin\alpha & (1+m)cos\alpha & \Delta y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_1\\ y_1\\ 1 \end{bmatrix} ⎣⎡​x2​y2​1​⎦⎤​=⎣⎡​(1+m)cosα(1+m)sinα0​−(1+m)sinα(1+m)cosα0​ΔxΔy1​⎦⎤​⎣⎡​x1​y1​1​⎦⎤​