齐次变换矩阵 左乘 原齐次变换矩阵

 

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是为了使得坐标轴变长，便于作图时候显示 


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


%% 变换（一）
% 动态参考坐标系DRF00，由变换（一）得到
% (0) DRF00平移回到和W完全重合。只有平移，不旋转
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
% (4) 把DRF00平移到，变换（一）结束时候的位置，得到DRF01。只有平移，不旋转
% 效果等价于，DRF00固定自己的坐标原点，沿着W的XYZ轴旋转
roll_DRF00toDRF01 = pi/2;
pitch_DRF00toDRF01 = 0;
yaw_DRF00toDRF01 = 0;
EA_zyx_DRF00toDRF01 = [yaw_DRF00toDRF01 pitch_DRF00toDRF01 roll_DRF00toDRF01];
R_DRF00toDRF01 = eul2rotm(EA_zyx_DRF00toDRF01,'ZYX');
HTfM_WtoDRF01 = HTfM_WtoDRF00;
HTfM_WtoDRF01(1:3,1:3) = R_DRF00toDRF01*HTfM_WtoDRF01(1:3,1:3);
DRF01 = HTfM_WtoDRF01*W;

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

figure(1)
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;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')
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')


%% 变换（二）
% 动态参考坐标系DRF00，由变换（一）得到。对DRF00做一下操作，得到DRF02
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转
% DRF00在旋转的过程中，其坐标原点在世界坐标系下的位置也会随之旋转。
roll_DRF00toDRF02 = pi/10;
pitch_DRF00toDRF02 = 0;
yaw_DRF00toDRF02 = 0;
EA_zyx_DRF00toDRF02 = [yaw_DRF00toDRF02 pitch_DRF00toDRF02 roll_DRF00toDRF02];
Tl_DRF00toDRF02 = [0,0,0]';
HTfM_DRF00toDRF02= genHTfM_useEularAngle(EA_zyx_DRF00toDRF02,Tl_DRF00toDRF02);
HTfM_WtoDRF02 = HTfM_DRF00toDRF02*HTfM_WtoDRF00;
DRF02 = HTfM_WtoDRF02*W;

figure(2)
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;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')

hold on;
trplot(DRF02,'frame',  'R02','color','m',  '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),'mo')

%% 变换（三）
% 动态参考坐标系DRF00，由变换（一）得到。对DRF00做一下操作，得到DRF03
%（1）DRF00相对于W的坐标原点，在原有平移的基础上，沿着W的XYZ方向平移，得到DRF03
roll_DRF00toDRF03 = 0;
pitch_DRF00toDRF03 = 0;
yaw_DRF00toDRF03 = 0;
EA_zyx_DRF00toDRF03 = [yaw_DRF00toDRF03 pitch_DRF00toDRF03 roll_DRF00toDRF03];
Tl_DRF00toDRF03 = [0,240,0]';
HTfM_DRF00toDRF03= genHTfM_useEularAngle(EA_zyx_DRF00toDRF03,Tl_DRF00toDRF03);
HTfM_WtoDRF03 = HTfM_DRF00toDRF03*HTfM_WtoDRF00;
DRF03 = HTfM_WtoDRF03*W;

figure(3)
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;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')

hold on;
trplot(DRF03, 'frame', 'R03', 'color', 'k', '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),'ko')

function HTfM= genHTfM_useRotM(rotationMatrix,translation)
% HTfM homogeneous transform matrix

HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;

end

function HTfM= genHTfM_useEularAngle(eularAngle_zyx,translation)
% HTfM homogeneous transform matrix

rotationMatrix=eul2rotm(eularAngle_zyx,'ZYX');
HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;

end

​​​​​​​