二维四参数转换模型
仿射变换的一般形式
[
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}
[xdestydest]=[ΔxΔy]+(1+m)[cosαsinα−sinαcosα][xsrcysrc]
式中:
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,ysrcxdest,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} [xdestydest]=[ΔxΔy]+[ab−ba][xsrcysrc] -
步骤二 矩阵变换成线性回归方程
[
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}
[xdestydest]=[1001xsrcysrc−ysrcxsrc]⎣⎢⎢⎡Δ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=[1001xsrcysrc−ysrcxsrc]⎣⎢⎢⎡Δ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=[1001xsrcysrc−ysrcxsrc]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−xsrcydest−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
[vxvy]=[1001xsrcysrc−ysrcxsrc]⎣⎢⎢⎡Δ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} ⎣⎡x2y21⎦⎤=⎣⎡(1+m)cosα(1+m)sinα0−(1+m)sinα(1+m)cosα0ΔxΔy1⎦⎤⎣⎡x1y11⎦⎤