calibration of sth

clc;
clear;
close all;


% rng(0);
% rand(1)*2*pi

%% W: world coordinate system
% 尺寸4X4，齐次矩阵，齐次坐标系
W = eye(4);
W(1:3,1:3)=W(1:3,1:3)*200; % 乘以200是为了使得坐标轴变长，便于作图时候显示


%% 变换（零）
% 动态参考坐标系DRF00，初始状态和世界坐标系完全重合
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
%（4）相对于W的坐标原点，沿着W的XYZ方向平移
roll_W_WtoDRF00 = pi/2; % Z
yaw_W_WtoDRF00 = 0;     % Y
pitch_W_WtoDRF00 = 0;   % X
EAZYX_W_WtoDRF00 = [roll_W_WtoDRF00 yaw_W_WtoDRF00 pitch_W_WtoDRF00 ];
TlXYZ_W_WtoDRF00 = [100, 100, -1000]';
HTfM_W_WtoDRF00 = genHTfM_useEularAngle(EAZYX_W_WtoDRF00,TlXYZ_W_WtoDRF00);
DRF00 = HTfM_W_WtoDRF00*W;

%% 变换（一）
% 动态参考坐标系DRF01，初始状态和DRF00坐标系完全重合
%（1）相对于DRF00的坐标原点，沿着DRF00的X方向平移
roll_W_DRF00toDRF01 = 0;    % Z
yaw_W_DRF00toDRF01 = 0;     % Y
pitch_W_DRF00toDRF01 = 0;   % X
EAZYX_W_DRF00toDRF01 = [roll_W_DRF00toDRF01 yaw_W_DRF00toDRF01 pitch_W_DRF00toDRF01 ];
TlXYZ_W_DRF00toDRF01 = [500,0,0]';
HTfM_W_DRF00toDRF01 = genHTfM_useEularAngle(EAZYX_W_DRF00toDRF01,TlXYZ_W_DRF00toDRF01);


% 右乘（先相对于W变换，然后再做W到DRF00的变换）
HTfM_DRF00_DRF00toDRF01 = HTfM_W_WtoDRF00*HTfM_W_DRF00toDRF01
DRF01 = HTfM_DRF00_DRF00toDRF01*W;

%% 变换（二）
% 动态参考坐标系DRF02，初始状态和DRF01坐标系完全重合
%（1）DRF02绕着DRF00的Y轴旋转
roll_W_DRF01toDRF02 = 0;       % Z
yaw_W_DRF01toDRF02 = -pi/4;     % Y
pitch_W_DRF01toDRF02 = 0;      % X
EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
TlXYZ_W_DRF01toDRF02 = [0,0,0]';
HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

HTfM_DRF01_DRF01toDRF02 =   HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01% 右乘
DRF02 = HTfM_DRF01_DRF01toDRF02*W;

%% 变换（三）
% 动态参考坐标系DRF03，初始状态和DRF02坐标系完全重合
%（1）DRF02绕着DRF00的Z轴旋转
roll_W_DRF02toDRF03 = pi/2;        % Z
yaw_W_DRF02toDRF03 = 0;     % Y
pitch_W_DRF02toDRF03 = 0;       % X
EAZYX_W_DRF02toDRF03 = [roll_W_DRF02toDRF03 yaw_W_DRF02toDRF03 pitch_W_DRF02toDRF03];
TlXYZ_W_DRF02toDRF03 = [0,0,0]';
HTfM_W_DRF02toDRF03 = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF03,TlXYZ_W_DRF02toDRF03);

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

HTfM_DRF00_DRF02toDRF03 =   HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF01_DRF01toDRF02% 右乘
DRF03 = HTfM_DRF00_DRF02toDRF03*W;



%%
% 画图
axis_limit = 1200;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-1500,300];

figure(6)
title("figure(6)");
trplot(W,'frame',  'RW','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);

hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame',  'R00','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);

% hold on;
% trplot(DRF01, 'frame', 'R01', 'color', 'b', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF01(1,4),DRF01(2,4),DRF01(3,4),'bo')

% hold on;
% trplot(DRF02, 'frame', 'R02', 'color', 'k', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF02(1,4),DRF02(2,4),DRF02(3,4),'ko')
% 
% hold on;
% trplot(DRF03, 'frame', 'RDRF03', 'color', 'm', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF03(1,4),DRF03(2,4),DRF03(3,4),'mo')


%% 变换（四）
% 动态参考坐标系DRF0X，初始状态和DRF02坐标系完全重合
%（1）DRF0X绕着DRF00的Z轴旋转
for i=-pi/2:pi/10:-pi/10

    %% 变换（二）
    % 动态参考坐标系DRF02，初始状态和DRF01坐标系完全重合
    %（1）DRF02绕着DRF00的Y轴旋转
    roll_W_DRF01toDRF02 = 0;       % Z
    yaw_W_DRF01toDRF02 = i;     % Y
    pitch_W_DRF01toDRF02 = 0;      % X
    EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
    TlXYZ_W_DRF01toDRF02 = [0,0,0]';
    HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

    EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

    HTfM_DRF01_DRF01toDRF02 =   HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01% 右乘
    DRF02 = HTfM_DRF01_DRF01toDRF02*W;


    for j=0:pi/8:2*pi
        %% 变换（三）
        % 动态参考坐标系DRF03，初始状态和DRF02坐标系完全重合
        %（1）DRF02绕着DRF00的Z轴旋转

        roll_W_DRF02toDRF0X = j;        % Z
        yaw_W_DRF02toDRF0X = 0;         % Y
        pitch_W_DRF02toDRF0X = 0;       % X
        EAZYX_W_DRF02toDRF0X = [roll_W_DRF02toDRF0X yaw_W_DRF02toDRF0X pitch_W_DRF02toDRF0X];
        TlXYZ_W_DRF02toDRF0X = [0,0,0]';
        HTfM_W_DRF02toDRF0X = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF0X,TlXYZ_W_DRF02toDRF0X);

        EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

        HTfM_DRF00_DRF02toDRF0X =   HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF0X*inv(HTfM_W_WtoDRF00)*HTfM_DRF01_DRF01toDRF02% 右乘
        DRF0X = HTfM_DRF00_DRF02toDRF0X*W;

        hold on;
        trplot(DRF0X, 'frame', 'RDRF0X', 'color', 'm', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
        hold on;
        plot3(DRF0X(1,4),DRF0X(2,4),DRF0X(3,4),'mo')
    end




end