齐次坐标系统和变换矩阵

齐次坐标系统，旋转矩阵，平移向量，变换矩阵

clc;
clear;
close all;

%wcs: world coordinate system
wcs = eye(4);
wcs(1:3,1:3)=wcs(1:3,1:3)*200;

%欧拉角
roll = pi/2;
pitch = pi/3;
yaw = pi/4;

angle_zyx = [yaw pitch roll];
rotationMatrix=eul2rotm(angle_zyx,'ZYX');
translation = [100,200,-1200]';

%ncs_1: new coordinate system
ncs_1 = eye(4);
ncs_1(1:3,1:3) = rotationMatrix*wcs(1:3,1:3);
ncs_1(1:3,4) = wcs(1:3,4)+translation;


transform = eye(4,4);
transform(1:3,1:3) = rotationMatrix;
transform(1:3,4) = translation;
ncs_2 = transform*wcs;

assert(max(max(ncs_1-ncs_2))<1e-10);



axis_limit = 1500;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-axis_limit,500];


trplot(wcs,'frame',  'R1','color','r',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
hold on;
plot3(wcs(1,4),wcs(2,4),wcs(3,4),'ko')
hold on;
trplot(ncs_2,'frame',  'R2','color','g',  'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
plot3(ncs_2(1,4),ncs_2(2,4),ncs_2(3,4),'ko')