代码片段 | Matlab三维图显示[ R T 0 1] 的最佳方法

news2024/11/26 10:50:39

% 输入N组RT矩阵
N = 4;
R = zeros(3, 3, N);
T = zeros(3, N);
R(:,:,1) = [-0.902608 0.250129 0.350335 ; 
            0.314198 0.939127 0.138996 ;
            -0.294242 0.235533 -0.926253 ];
T(:,1) = [205.877;
         2796.02; 
          907.116];
      
R(:,:,2) = [-0.123936 0.643885 0.755018 ;
             0.816604 0.464468 -0.264489;
            -0.526679 0.602176 -0.599994];
T(:,2) = [ 290.423;
           2734.71; 
           849.983 ];
R(:,:,3) = [-0.28978 0.539335 -0.790661;
            0.383489 -0.691464 -0.61222;
           -0.876905 -0.48062 -0.00645646 ];
T(:,3)= [ 422.686;
          2660.191;
          920.816];
      
R(:,:,4)= [-0.526679 0.602176 -0.599994;
           0.114287 0.928046 0.354498;
           0.575541 0.229001 -0.785055];
       
T(:,4)= [ 306.23;
          2898.27;
          887.644];  

% 自适应设置初始坐标值
x_start = min(T(1,:)) - 10;
y_start = min(T(2,:)) - 10;
z_start = min(T(3,:)) - 10;

% 创建坐标轴
figure;
hold on;
xlabel('X');
ylabel('Y');
zlabel('Z');
axis equal;

% 绘制坐标轴起始点
%scatter3(x_start, y_start, z_start, 'filled', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k');

% 绘制N组坐标轴情况和轴原点位置
for i = 1:N
    % 获取当前TF矩阵
    R_curr = R(:,:,i);
    T_curr = T(:,i);
    
    % 定义箭头长度
    arrow_length = 200;
    
    % 计算坐标轴末端点
    x_end = R_curr(1, :) * arrow_length + T_curr(1);
    y_end = R_curr(2, :) * arrow_length + T_curr(2);
    z_end = R_curr(3, :) * arrow_length + T_curr(3);
    
    % 绘制坐标轴
    quiver3(T_curr(1), T_curr(2), T_curr(3), R_curr(1, 1) * arrow_length, R_curr(2, 1) * arrow_length, R_curr(3, 1) * arrow_length, 'Color', 'r', 'LineWidth', 2); % X轴
    quiver3(T_curr(1), T_curr(2), T_curr(3), R_curr(1, 2) * arrow_length, R_curr(2, 2) * arrow_length, R_curr(3, 2) * arrow_length, 'Color', 'g', 'LineWidth', 2); % Y轴
    quiver3(T_curr(1), T_curr(2), T_curr(3), R_curr(1, 3) * arrow_length, R_curr(2, 3) * arrow_length, R_curr(3, 3) * arrow_length, 'Color', 'b', 'LineWidth', 2); % Z轴
    
  % 绘制坐标轴末端点
    %scatter3(x_end(1), x_end(2), x_end(3), 'filled', 'MarkerFaceColor', 'r'); % X轴末端点
    %scatter3(y_end(1), y_end(2), y_end(3), 'filled', 'MarkerFaceColor', 'g'); % Y轴末端点
    %scatter3(z_end(1), z_end(2), z_end(3), 'filled', 'MarkerFaceColor', 'b'); % Z轴末端点
    
end

hold off;
% 设置坐标轴比例相等，并锁定三维视图
axis equal;
view(3);
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
hold off;