三维七参数转换模型
三维仿射变换的一般形式
[ X Y Z ] = [ Δ X Δ Y Δ Z ] + ( 1 + m ) [ 1 θ z − θ y − θ z 1 θ x θ y − θ x 1 ] [ X 0 Y 0 Z 0 ] \begin{bmatrix} {X}\\ {Y}\\ {Z} \end{bmatrix}=\begin{bmatrix} {\Delta X}\\ {\Delta Y}\\ {\Delta Z} \end{bmatrix}+(1+m)\begin{bmatrix} {1}&{\theta_z}&{-\theta_y} \\ {-\theta_z}&{1}&{\theta_x} \\ {\theta_y}&{-\theta_x}&{1} \end{bmatrix} \begin{bmatrix} {X_0}\\ {Y_0}\\ {Z_0} \end{bmatrix} ⎣⎡XYZ⎦⎤=⎣⎡ΔXΔYΔZ⎦⎤+(1+m)⎣⎡1−θzθyθz1−θx−θyθx1⎦⎤⎣⎡X0Y0Z0⎦⎤
变换方程
[ X Y Z ] = [ 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 ] [ Δ X Δ Y Δ Z ( 1 + m ) θ x ( 1 + m ) θ y ( 1 + m ) θ z ( 1 + m ) ] \begin{bmatrix} {X }\\ {Y }\\ {Z } \end{bmatrix} = \begin{bmatrix} {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{bmatrix} \begin{bmatrix} {\Delta X}\\ {\Delta Y}\\ {\Delta Z}\\ {(1+m)\theta_x}\\ {(1+m)\theta_y}\\ {(1+m)\theta_z}\\ {(1+m)} \end{bmatrix} ⎣⎡XYZ⎦⎤=⎣⎡1000100010(Z0−Y0−Z00X0Y0−X00X0Y0Z0⎦⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡ΔXΔYΔZ(1+m)θx(1+m)θy(1+m)θz(1+m)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
间接平差求解
B
=
[
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
]
B=\begin{bmatrix} {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{bmatrix}
B=⎣⎡1000100010Z0−Y0−Z00X0Y0−X00X0Y0Z0⎦⎤
l
=
[
x
d
e
s
t
−
x
s
r
c
y
d
e
s
t
−
y
s
r
c
.
.
.
.
.
.
.
.
.
]
l=\begin{bmatrix} x_{dest}-x_{src}\\ y_{dest}-y_{src} \\ ... \\ ... \\ ... \end{bmatrix}\\
l=⎣⎢⎢⎢⎢⎡xdest−xsrcydest−ysrc.........⎦⎥⎥⎥⎥⎤
求解的参数:
X
=
[
Δ
X
Δ
Y
Δ
Z
(
1
+
m
)
θ
x
(
1
+
m
)
θ
y
(
1
+
m
)
θ
z
(
1
+
m
)
]
X=\begin{bmatrix} {\Delta X}\\ {\Delta Y}\\ {\Delta Z}\\ {(1+m)\theta_x}\\ {(1+m)\theta_y}\\ {(1+m)\theta_z}\\ {(1+m)} \end{bmatrix}
X=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡ΔXΔYΔZ(1+m)θx(1+m)θy(1+m)θz(1+m)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
最小二乘原理:
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
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));
}