二维坐标系空间变换（详细解读，附MATLAB代码）

二维坐标系空间变换

假如存在任意两个二维坐标系，如下图所示：
在这里插入图片描述

目的：将xoy坐标系经过处理变换到XOY坐标系。

经过分析可知：对于二维平面上的变换需要 x y 方向上两个平移参数dx，dy，以及一个绕着垂直于xy平面的旋转参数θ，此外，还存在一个缩放尺度因子m（可选）。
求解出以上四个参数就能够实现任意两个二维坐标系之间的相互转换，转换公式如下。
在这里插入图片描述

下面分两种方法求解以上四个参数：

1 直接法

前提：已知两对公共点分别在两个坐标系下的坐标，如上图中待转换坐标系中的o,p 和在参考坐标系中的O,P。

求解过程：
1）对于平移参数，简单认为第一对公共点（如o与O）的差值就是所有点的平移量，即 O 点在两个坐标系下的坐标值直接相减得到dx，dy。
在这里插入图片描述
2）对于旋转参数，认为op连线在xoy坐标系与XOY坐标系下的方位角差值为旋转角θ。

首先计算线段op在两个坐标系下的向量表示：

基于上述向量，计算方位角：

注意：方位角A和α计算时要区分不同象限，判断△x,△y的正负。这里简单附上代码，就不过多解释了。
azimuth.m

function [outputArg] = azimuth(dx,dy)
%AZIMUTH  xiaochen 2021/07/17 
%   方位角计算（没有考虑dx,dy为0的情况）
%   通过dx 与 dy，计算方位角  判断属于哪个象限
if(dy >0 && dx > 0) % 第一象限 锐角 0-90°，直接求解
    a_orb = atan(dy / dx);
elseif(dy >0 && dx < 0) % 第二象限 钝角 90-180°，180°- XX
    a_orb = pi - atan(dy / abs(dx));
elseif(dy <0 && dx < 0) % 第三象限 180-270° 180°+ XX
    a_orb = pi + atan(dy / dx);
elseif(dy <0 && dx > 0) % 第四象限 270-360° 360°- XX
    a_orb = 2*pi - atan(abs(dy) / dx);
end
outputArg = a_orb;
end

3）对于尺度参数，认为op连线在xoy坐标系与XOY坐标系下的长度的比值即为对应的缩放参数。

长度计算：
在这里插入图片描述
参数m计算：

至此，我们就通过直接法计算出了二维坐标系转换所有的4个转换参数。

代码如下：（azimuth函数在上方）

%% xiaochen 2021/07/17
% 二维坐标转换  3参数计算 直接参数法，无最小二乘优化
clc
clear
close all

%% 任选两对公共点
xoy = load('xoy_Cor.txt'); % 读取坐标，xoy_Cor 待转换坐标系
XOY_C = load('XOY_C_Cor.txt'); % XOY_C数据，参考坐标系

%% 计算平移量
xoy_p1 = [xoy(1,1), xoy(1,2)]; % xoy 点1
xoy_p2 = [xoy(150,1), xoy(150,2)]; % xoy 点2

XOY_C_p1 = [XOY_C(1,1), XOY_C(1,2)]; % XOY_C 点1
XOY_C_p2 = [XOY_C(150,1), XOY_C(150,2)]; % XOY_C 点2

%% 计算平移量
dx =  XOY_C_p1(1) - xoy_p1(1);
dy =  XOY_C_p1(2) - xoy_p1(2);

%% 计算旋转角度
xoy_dy = xoy_p2(2)-xoy_p1(2); xoy_dx = xoy_p2(1)-xoy_p1(1);
XOY_C_dy = XOY_C_p2(2)-XOY_C_p1(2); XOY_C_dx = XOY_C_p2(1)-XOY_C_p1(1);

% xoy方位角计算 判断
a_xoy = azimuth(xoy_dx, xoy_dy);
% XOY_C方位角计算 判断
a_XOY_C = azimuth(XOY_C_dx, XOY_C_dy);

% a_xoy = atan(xoy_dy / xoy_dx);
% a_XOY_C = atan(XOY_C_dy / XOY_C_dx);
xita =  a_XOY_C - a_xoy; % 旋转角度

rotation = [cos(xita), sin(xita); -sin(xita), cos(xita)]; % 旋转矩阵

xoy_aft = [xoy(:,1),xoy(:,2)]* rotation  + [dx, dy]; % 使用计算的三个参数进行变换
xoy_aft = [xoy_aft, xoy(:,3)]; % 与最后一列高程进行组合

dlmwrite('xoy_Coor.txt',xoy_aft,'delimiter','\t','precision',10); % 写出数据文件

%% 查看效果
figure;
plot(XOY_C(:,1),XOY_C(:,2),'.-');
hold on;
plot(xoy(:,1),xoy(:,2),'.-');
hold on;
plot(xoy_aft(:,1),xoy_aft(:,2),'.-');
% legend('XOY_C', 'xoy', 'xoyaft');

fprintf('translation: [%f, %f], rotation: %f° \n', dx, dy, rad2deg(xita));
disp('the rotation matrix is:');
disp(rotation);
disp('finish!');


%% 两点之间距离，计算尺度时使用，这里没有进行使用
s_xoy = sqrt(power((xoy_p1(1)-xoy_p2(1)),2) + power((xoy_p1(2)-xoy_p2(2)),2));
s_XOY_C = sqrt(power((XOY_C_p1(1)-XOY_C_p2(1)),2) + power((XOY_C_p1(2)-XOY_C_p2(2)),2));
scale = (s_XOY_C - s_xoy)/s_xoy;

2 最小二乘优化法

对于多公共点（已知多于两对公共点分别在两个坐标系下的坐标）的坐标变换，需利用最小二乘原理，将模型参数一起作为未知参数列立误差方程进行解算。

转换公式仍与上述方法相同：
在这里插入图片描述
写成线性公式的形式：

每一个（x，y）可由上式计算得（X’，Y’），将该点在XOY坐标系下的（X，Y）当做观测值，则有：
在这里插入图片描述
以dx、dy、m、θ为未知数，将以上两式线性化展开，保留一次项，可得：

上式写成矩阵形式：

求解参数 X：

编程实现过程：
1）未知数近似值的求解，可选取任意两点，采用上述直接法求得；
2）B矩阵元素的计算，采用1）求得的近似值，代入线性化后的误差方程式计算系数项，每对公共点可列两个误差方程式，可计算得B矩阵中的两行，如下图；
在这里插入图片描述
3）L矩阵元素的计算，采用1）求得的近似值，将（x，y）计算出（X’，Y），用公共点坐标减去计算坐标即得L矩阵，每对公共点可计算得L矩阵中的两个元素；

在这里插入图片描述
4）如果未知数的初值不够准确，则需要迭代计算，即将解出的参数X

加到对应的初值上，形成新的初值，重复上述迭代计算，直到参数X的改正数接近于0，或一个可以接受的阈值，结束迭代。

代码如下：（init_cal函数如下）（azimuth函数在上方）

function [trans_dx,trans_dy, xita] = init_cal(xoy_p1,xoy_p2,XOY_C_p1,XOY_C_p2)
%INIT_CAL xiaochen 2021/07/17
%   两点法计算初值
%   先通过两点法计算转换参数，为后续进行优化提供初值

dx =  XOY_C_p1(1) - xoy_p1(1);
dy =  XOY_C_p1(2) - xoy_p1(2);

%% 计算旋转角度
xoy_dy = xoy_p2(2)-xoy_p1(2); xoy_dx = xoy_p2(1)-xoy_p1(1);
XOY_C_dy = XOY_C_p2(2)-XOY_C_p1(2); XOY_C_dx = XOY_C_p2(1)-XOY_C_p1(1);

% xoy方位角计算 判断
a_xoy = azimuth(xoy_dx, xoy_dy);
% XOY_C方位角计算 判断
a_XOY_C = azimuth(XOY_C_dx, XOY_C_dy);

xita =  a_XOY_C - a_xoy; % 旋转角度
trans_dx = dx; % x平移
trans_dy = dy; % y平移
end

最小二乘法求解脚本：

%% xiaochen 2021/07/17
% 二维坐标转换  3参数计算 最小二乘优化
clc
clear
close all

%% 读取数据
xoy = load('xoy_Cor.txt'); % 读取坐标，xoy_Cor.txt为翻转180度后的数据
XOY_C = load('XOY_C_Cor.txt'); % XOY_C数据，参考坐标系

xoy_p1 = [xoy(100,1), xoy(100,2)]; % xoy 点1
xoy_p2 = [xoy(150,1), xoy(150,2)]; % xoy 点2
XOY_C_p1 = [XOY_C(100,1), XOY_C(100,2)]; % XOY_C 点1
XOY_C_p2 = [XOY_C(150,1), XOY_C(150,2)]; % XOY_C 点2

[init_x, init_y, init_xita] = init_cal(xoy_p1,xoy_p2,XOY_C_p1,XOY_C_p2); % 直接法计算参数初值

%% 最小二乘法优化 
xita = init_xita;
dx = init_x;
dy = init_y;
m = 0; % 参数赋初值

for i = 1:length(xoy)
   B = [1,0,cos(xita)*xoy(i,1)+sin(xita)*xoy(i,2),(1+m)*(-sin(xita)*xoy(i,1)+cos(xita)*xoy(i,2));...
       0,1, -sin(xita)*xoy(i,1)+cos(xita)*xoy(i,2),(1+m)*(-cos(xita)*xoy(i,1)-sin(xita)*xoy(i,2))];
   rotation = [cos(xita), sin(xita); -sin(xita), cos(xita)];
   xoy_trans = (1+m)*rotation*[xoy(i,1);xoy(i,2)] + [dx; dy];
   
   L = [XOY_C(i,1); XOY_C(i,2)] - xoy_trans;
   BB = (B'*B)\B'; % (B'*B)\B' 等同于 inv(B'*B)* B'
   para = BB * L;
   if (abs(para(1)) < 0.01 && abs(para(2)) < 0.01 && abs(para(3)) < 0.01 && abs(para(4)) < 0.01)
       fprintf('第 %d 次循环,达到收敛。\n', i);
       break;
   else
       dx = dx + para(1);
       dy = dy + para(2);
       m = m + para(3);
       xita = xita + para(4); % 改进参数
   end
   fprintf('第 %d 次循环。\n', i);
end

%% 转换数据并输出
fprintf('translation: [%f, %f], rotation: %f° \n', dx, dy, rad2deg(xita));
disp('the rotation matrix is:');
disp(rotation);

xoy_aft = [xoy(:,1),xoy(:,2)]* rotation  + [dx, dy]; % 使用计算的三个参数进行变换,没有使用尺度参数m
xoy_aft = [xoy_aft, xoy(:,3)]; % 与最后一列高程进行组合

dlmwrite('xoy_Opti.txt',xoy_aft,'delimiter','\t','precision',10); % 写出数据文件
%% 查看效果
figure;
plot(XOY_C(:,1),XOY_C(:,2),'.-');
hold on;
plot(xoy(:,1),xoy(:,2),'.-');
hold on;
plot(xoy_aft(:,1),xoy_aft(:,2),'.-');