三维仿射变换与七参数计算

三维七参数转换模型

三维仿射变换的一般形式

$$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right]+(1+m)\left[\begin{array}{ccc}1& {\theta }_z& -{\theta }_y\\ -{\theta }_z& 1& {\theta }_x\\ {\theta }_y& -{\theta }_x& 1\end{array}\right]\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]$$

变换方程

$$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{ccccccc}1& 0& 0& 0& -Z_0& Y_0& X_0\\ 0& 1& 0& (Z_0& 0& -X_0& Y_0\\ 0& 0& 1& -Y_0& X_0& 0& Z_0\end{array}\right]\left[\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\\ (1+m){\theta }_x\\ (1+m){\theta }_y\\ (1+m){\theta }_z\\ (1+m)\end{array}\right]$$

间接平差求解

$$B=\left[\begin{array}{ccccccc}1& 0& 0& 0& -Z_0& Y_0& X_0\\ 0& 1& 0& Z_0& 0& -X_0& Y_0\\ 0& 0& 1& -Y_0& X_0& 0& Z_0\end{array}\right]$$
$$l=\left[\begin{array}{c}{x}_{dest}-x_{src}\\ y_{dest}-y_{src}\\ ...\\ ...\\ ...\end{array}\right]$$
求解的参数：
$$X=\left[\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\\ (1+m){\theta }_x\\ (1+m){\theta }_y\\ (1+m){\theta }_z\\ (1+m)\end{array}\right]$$

最小二乘原理：
$$V^{T}PV=min$$
得：
$$\begin{gathered} W=B^{T}Pl \\ X=(B^{T}PB{)}^{-1}W \\ {\sigma }^2=V^{T}PV \\ \sigma =\sqrt{\frac{V^{T}PV}{n-t}} \end{gathered}$$

Eigen3代码实现

#define SEC_TO_RAD 4.84813681109535993589914102357e-6

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()<3)
        return;
    int  n=srcData.count()*3;

    MatrixXd B = MatrixXd::Zero(n, 7);
    MatrixXd l = MatrixXd::Zero(n, 1);

    for (int i = 0; i < n; i++)
    {
        if (i % 3 == 0)
        {
            B(i, 0) = 1;
            B(i, 1) = 0;
            B(i, 2) = 0;
            B(i, 3) = 0;
            B(i, 4) = -srcData[i / 3].Z;
            B(i, 5) = srcData[i / 3].Y;
            B(i, 6) = srcData[i / 3].X;
            l(i, 0) = destData[i / 3].X - srcData[i / 3].X;
        }
        else if (i % 3 == 1)
        {
            B(i, 0) = 0;
            B(i, 1) = 1;
            B(i, 2) = 0;
            B(i, 3) = srcData[i / 3].Z;
            B(i, 4) = 0;
            B(i, 5) = -srcData[i / 3].X;
            B(i, 6) = srcData[i / 3].Y;
            l(i, 0) = destData[i / 3].Y - srcData[i / 3].Y;
        }
        else
        {
            B(i, 0) = 0;
            B(i, 1) = 0;
            B(i, 2) = 1;
            B(i, 3) = -srcData[i / 3].Y;
            B(i, 4) = srcData[i / 3].X;
            B(i, 5) = 0;
            B(i, 6) = srcData[i / 3].Z;
            l(i, 0) = destData[i / 3].Z - srcData[i / 3].Z;
        }
    }
    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;
    //转化为 proj4 + towgs参数
    _dx=Para(0, 0);
    _dy=Para(1, 0);
    _dz=Para(2, 0);
    
    _rotationX= -Para(3, 0)/SEC_TO_RAD;
    _rotationY= -Para(4, 0)/SEC_TO_RAD;
    _rotationZ= -Para(5, 0)/SEC_TO_RAD;

    _scale=Para(6, 0)*1000000.0;
    //误差
    _sigma2=sqrt(sigma2(0,0)/(n-7));

}