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

基于https://blog.youkuaiyun.com/wxc_1998/article/details/118853843
的修改/完善（无需初试估值，直接最小二乘迭代求解转换参数）

本质是因为，求坐标系转换参数是一个非线性的最小二乘优化问题，所以需要进行迭代求解

二维坐标系转换

包含平移，缩放，旋转

convert.m:

%% wys 2023/5/30
% 二维坐标转换  4参数计算 最小二乘优化
clc
clear
close all

%% 读取数据
xoy = load('imgcoord.txt'); % 读取坐标
XOY_C = load('showcoord.txt'); % XOY_C数据，变换后的坐标系

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

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

%% 最小二乘法优化  需要迭代
% dx = init_x;
% dy = init_y;
% s = scale; 
% xita = init_xita; % 参数赋初值

dx = 0;% 不使用估值
dy = 0;
s = 1; 
xita = 0; % 参数赋初值

for k = 1:1000
    B=[1,0,cos(xita)*xoy(1,1)-sin(xita)*xoy(1,2),s*(-sin(xita)*xoy(1,1)-cos(xita)*xoy(1,2));
           0,1, sin(xita)*xoy(1,1)+cos(xita)*xoy(1,2),s*(cos(xita)*xoy(1,1)-sin(xita)*xoy(1,2))];
    rotation = [cos(xita), -sin(xita); sin(xita), cos(xita)];
    xoy_trans = s*rotation*[xoy(1,1);xoy(1,2)] + [dx; dy]; %计算出的真值
    L = [XOY_C(1,1); XOY_C(1,2)] - xoy_trans;
    for i = 2:length(xoy)
       B = [B;
           1,0,cos(xita)*xoy(i,1)-sin(xita)*xoy(i,2),s*(-sin(xita)*xoy(i,1)-cos(xita)*xoy(i,2));
           0,1, sin(xita)*xoy(i,1)+cos(xita)*xoy(i,2),s*(cos(xita)*xoy(i,1)-sin(xita)*xoy(i,2))];
       rotation = [cos(xita), -sin(xita); sin(xita), cos(xita)];
       xoy_trans = s*rotation*[xoy(i,1);xoy(i,2)] + [dx; dy];
       
       tL = [XOY_C(i,1); XOY_C(i,2)] - xoy_trans;
       L = [L;tL];  
    end
    
    BB = (B'*B)\B'; % (B'*B)\B' 等同于 inv(B'*B)* B'
    para = BB * L;
    
    
    if (abs(para(1)) < 0.00001 && abs(para(2)) < 0.00001 && abs(para(3)) < 0.00001 && abs(para(4)) < 0.00001)
       fprintf('第 %d 次循环,达到收敛。\n', k);
       break;
    else
       dx = dx + para(1);
       dy = dy + para(2);
       s = s + para(3);
       %s=s;
       xita = xita + para(4); % 改进参数
       if(xita>2*pi)
           xita = xita-2*pi;
       elseif(xita<0)
           xita = xita+2*pi;
       end
       fprintf('第 %d 次循环。\n', k);
    end
    
    
end

rotation = [cos(xita), -sin(xita); sin(xita), cos(xita)];

disp(rotation);
s = s
dx=dx
dy=dy
xita = xita

xoy_aft = [xoy(:,1),xoy(:,2)]* rotation'* s  + [dx, dy] % 使用计算的4个参数进行变换

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

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

init_cal.m:

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

    %% 计算尺度缩放
    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;
   
    %% 计算旋转角度
    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; % 旋转角度(nishizhen)
    % while(xita<0)
    %     xita = xita+2*pi;
    %     if(xita>0)
    %         break;
    %     end
    % end
    
    %% 计算dxdy
    r = [cos(xita),-sin(xita);sin(xita),cos(xita)]
    dx =  XOY_C_p1(1) - xoy_p1*[cos(xita);-sin(xita)]*scale;
    dy =  XOY_C_p1(2) - xoy_p1*[sin(xita);cos(xita)]*scale;
    % [dx;dy] = XOY_C_p1 - xoy_p1*r*scale; 
    
    %% output
    
    trans_dx = dx; % x平移
    trans_dy = dy; % y平移
    scale = scale; % 尺度
end

azimuth.m:

function [outputArg] = azimuth(dx,dy)
    %AZIMUTH  wys 
    %   方位角计算（没有考虑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

坐标逆时针旋转角度θ为正
R=[cos,-sin;sin,cos]

注释之后再补