二维四参数转换模型
仿射变换的一般形式
[ 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⎦⎤
