matlab画姿态图

clear;
close all;
clc;
data = importdata('data.csv'); %读取csv数据文件
[row, col] = size(data);
PoseMat = [];
% 算出姿态矩阵
for index = 1 : row
    PoseMat = [PoseMat; rotz(data(index, 6))* roty(data(index, 5)) * rotx(data(index, 4))];
end
% 开始进行画图的操作
for drawIndex = 1 : row
    startpoint = data(drawIndex, 1 : 3); % 提取出xyz值
    xEndPoint = data(drawIndex, 1 : 3) + PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 1)' * 10;
    yEndPoint = data(drawIndex, 1 : 3) + PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 2)' * 10;
    zEndPoint = data(drawIndex, 1 : 3) + PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 3)' * 10;
    dot(PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 2), PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 1))
    dot(PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 3), PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 1))
    dot(PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 3), PoseMat(3 * drawIndex - 2 : 3 * drawIndex, 2))
    willDrawX = [startpoint; xEndPoint];
    willDrawY = [startpoint; yEndPoint];
    willDrawZ = [startpoint; zEndPoint];
%     subplot(121);
    plot3(willDrawX(:,1),willDrawX(:,2),willDrawX(:,3),'-b*','LineWidth',3);hold on;
    plot3(willDrawY(:,1),willDrawY(:,2),willDrawY(:,3),'-r*','LineWidth',3);hold on;
    plot3(willDrawZ(:,1),willDrawZ(:,2),willDrawZ(:,3),'-k*','LineWidth',3);hold on;
    title('三维位姿图分析');
end
% for i = 1 : row
%     if i < row
%         subplot(121);
%         plot3([data(i, 1); data(i+1, 1)],[data(i, 2); data(i+1, 2)],[data(i, 3); data(i+1, 3)],'-cs','LineWidth',1);hold on;
%     else
%         subplot(121);
%         plot3([data(1, 1); data(i, 1)],[data(1, 2); data(i, 2)],[data(1, 3); data(i, 3)],'-cs','LineWidth',1);hold on;
%     end
% end
% legend('X 坐标','Y 坐标','Z 坐标');
% xlabel('X(mm)');
% ylabel('Y(mm)');
% zlabel('Z(mm)')
% % 绘制二维角度折线图操作
% xRange = 1 : row;
% thetaA = data(:, 4);
% thetaB = data(:, 5);
% thetaC = data(:, 6);
% subplot(122);
% plot(xRange, thetaA, '-b*','LineWidth', 1);hold on;
% plot(xRange, thetaB, '-r+','LineWidth', 1);hold on;
% plot(xRange, thetaC, '-ko','LineWidth', 1);hold on;
% xlabel('第几个点');  % 添加x轴信息
% ylabel('角度（度数）')
% title('欧拉角分析');  % 添加标题
% legend('欧拉角A','欧拉角B','欧拉角C');  % 添加线段标签
% % plot(xRange, thetaA)