旋转矩阵注意点

用R=Rz*Ry*Rx 旋转矩阵去乘以原始坐标轴正交基向量，代表将各个向量依次沿 原始坐标系的x,y,z轴旋转相应角度（使用右手定则旋转）。

下图的例子中，依次进行了z轴旋转180°，沿y轴90°，x轴90°。

clc
clear all
close all
%%
e_x = [1 0 0]'; % front
e_y = [0 1 0]'; % left
e_z = [0 0 1]'; % up



alpha = 3.1493750236320928e+00;     % Rotation around vertical axis (yaw)
zeta = 1.5794758471340726e+00;      % rotation around horizontal axis (pitch)
kappa = 1.5742435495914964e+00;     % rotation around view axis (roll)
t_x = 4.6151672058497878e-01;
t_y = 5.9808916994333639e-01;
t_z = -1.2381792951442079e-01;

%%
origin = [0 0 0]';
origin_cam = [t_x t_y t_z]';

R_alpha = [
    cos(alpha)  -sin(alpha) 0;
    sin(alpha)  cos(alpha)  0;
    0           0           1];

R_zeta = [
    cos(zeta)   0   sin(zeta);
    0           1   0;
    -sin(zeta)  0   cos(zeta)];

R_kappa = [
    1           0     0;
    0   cos(kappa) -sin(kappa)
    0   sin(kappa)  cos(kappa)];

 R = R_kappa * R_zeta * R_alpha;

e_x_cam = R_alpha * e_x + origin_cam;
e_y_cam = R_alpha * e_y + origin_cam;
e_z_cam = R_alpha * e_z + origin_cam;

% PunktA_inPlatform = [0, 0, 3];
% PunktA_inCamera = R_alpha*PunktA_inPlatform'+origin_cam;


%% PLOTTING
figure
hold on
axis equal

%plot plattform system
x1 = [origin'; e_x'];
x2 = [origin'; e_y'];
x3 = [origin'; e_z'];

plot3(x1(:,1), x1(:,2), x1(:,3), 'Color', 'r');
plot3(x2(:,1), x2(:,2), x2(:,3), 'Color', 'g');
plot3(x3(:,1), x3(:,2), x3(:,3), 'Color', 'b');
text(origin(1), origin(2), origin(3), 'Platform');
text(e_x(1), e_x(2), e_x(3), 'x');
text(e_y(1), e_y(2), e_y(3), 'y');
text(e_z(1), e_z(2), e_z(3), 'z');

% plot camera system
x1_cam = [origin_cam'; e_x_cam'];
x2_cam = [origin_cam'; e_y_cam'];
x3_cam = [origin_cam'; e_z_cam'];

plot3(x1_cam(:,1), x1_cam(:,2), x1_cam(:,3), 'Color', 'r');
plot3(x2_cam(:,1), x2_cam(:,2), x2_cam(:,3), 'Color', 'g');
plot3(x3_cam(:,1), x3_cam(:,2), x3_cam(:,3), 'Color', 'b');
text(origin_cam(1), origin_cam(2), origin_cam(3), 'Camera');
text(e_x_cam(1), e_x_cam(2), e_x_cam(3), 'x');
text(e_y_cam(1), e_y_cam(2), e_y_cam(3), 'y');
text(e_z_cam(1), e_z_cam(2), e_z_cam(3), 'z');
% xlim([-3, 3]);
% ylim([-3, 3]);
% zlim([-3, 3]);



e_x_cam = R_zeta * ( R_alpha * e_x) + origin_cam;
e_y_cam = R_zeta *(R_alpha* e_y) + origin_cam;
e_z_cam = R_zeta *(R_alpha * e_z) + origin_cam;



%% PLOTTING
figure
hold on
axis equal

%plot plattform system
x1 = [origin'; e_x'];
x2 = [origin'; e_y'];
x3 = [origin'; e_z'];

plot3(x1(:,1), x1(:,2), x1(:,3), 'Color', 'r');
plot3(x2(:,1), x2(:,2), x2(:,3), 'Color', 'g');
plot3(x3(:,1), x3(:,2), x3(:,3), 'Color', 'b');
text(origin(1), origin(2), origin(3), 'Platform');
text(e_x(1), e_x(2), e_x(3), 'x');
text(e_y(1), e_y(2), e_y(3), 'y');
text(e_z(1), e_z(2), e_z(3), 'z');

% plot camera system
x1_cam = [origin_cam'; e_x_cam'];
x2_cam = [origin_cam'; e_y_cam'];
x3_cam = [origin_cam'; e_z_cam'];

plot3(x1_cam(:,1), x1_cam(:,2), x1_cam(:,3), 'Color', 'r');
plot3(x2_cam(:,1), x2_cam(:,2), x2_cam(:,3), 'Color', 'g');
plot3(x3_cam(:,1), x3_cam(:,2), x3_cam(:,3), 'Color', 'b');
text(origin_cam(1), origin_cam(2), origin_cam(3), 'Camera');
text(e_x_cam(1), e_x_cam(2), e_x_cam(3), 'x');
text(e_y_cam(1), e_y_cam(2), e_y_cam(3), 'y');
text(e_z_cam(1), e_z_cam(2), e_z_cam(3), 'z');

e_x_cam = R_kappa *(R_zeta * ( R_alpha * e_x)) + origin_cam;
e_y_cam = R_kappa *(R_zeta *(R_alpha* e_y)) + origin_cam;
e_z_cam = R_kappa *(R_zeta *(R_alpha * e_z)) + origin_cam;



%% PLOTTING
figure
hold on
axis equal

%plot plattform system
x1 = [origin'; e_x'];
x2 = [origin'; e_y'];
x3 = [origin'; e_z'];

plot3(x1(:,1), x1(:,2), x1(:,3), 'Color', 'r');
plot3(x2(:,1), x2(:,2), x2(:,3), 'Color', 'g');
plot3(x3(:,1), x3(:,2), x3(:,3), 'Color', 'b');
text(origin(1), origin(2), origin(3), 'Platform');
text(e_x(1), e_x(2), e_x(3), 'x');
text(e_y(1), e_y(2), e_y(3), 'y');
text(e_z(1), e_z(2), e_z(3), 'z');

% plot camera system
x1_cam = [origin_cam'; e_x_cam'];
x2_cam = [origin_cam'; e_y_cam'];
x3_cam = [origin_cam'; e_z_cam'];

plot3(x1_cam(:,1), x1_cam(:,2), x1_cam(:,3), 'Color', 'r');
plot3(x2_cam(:,1), x2_cam(:,2), x2_cam(:,3), 'Color', 'g');
plot3(x3_cam(:,1), x3_cam(:,2), x3_cam(:,3), 'Color', 'b');
text(origin_cam(1), origin_cam(2), origin_cam(3), 'Camera');
text(e_x_cam(1), e_x_cam(2), e_x_cam(3), 'x');
text(e_y_cam(1), e_y_cam(2), e_y_cam(3), 'y');
text(e_z_cam(1), e_z_cam(2), e_z_cam(3), 'z');