matlab画姿态图

柔光的暖阳◎ 2022-12-06 05:23 311阅读 0赞

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)