Matlab atan2用于两个3D矢量之间的角度

Question

我需要计算两个平面之间的角度。一个平面是手平面，第二个是前臂平面。我计算了这些平面的法线，并在MATLAB中使用了公式atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2));。我想看到手腕的屈曲/伸展角度，其特征是正峰值和负峰值，但使用此公式我只获得正峰值。

%% Script to compute the angles of wrist flexion/extension and adduction/abduction based on Vicon data
% REF: Cheryl et al., March 2008
% Order of the markers: 1.WRR 2.WRU 3.FAU 4.FAR 5.CMC2 6.CMC5 7.MCP5 8.MCP2

clc
close all
clear all

%% Initialization
% dir_kinematic = input('Path of the folder containing the kinematic files: ','s');
dir_kinematic = 'C:\Users\Utente\Desktop\TESI\Vicon\test.mat';
cd(dir_kinematic);

fileList = getAllFiles(dir_kinematic,0); % Get names of all kinematic files
f=1;
%% Conversion to angles
 for f = 1:length(fileList)
    if ~isempty(strfind(fileList{f},'mat')) % Take only mat files


        % 0. Loading
        load(fileList{f});

        % 1. Filtering
        frameRate = kinematic.framerate;
        n = 9; 
        Wn = 2/(frameRate/2);
        ftype = 'low';
        [b,a] = butter(n,Wn,ftype);
        kinematic.x = filtfilt(b,a,kinematic.x);
        kinematic.y = filtfilt(b,a,kinematic.y);
        kinematic.z = filtfilt(b,a,kinematic.z);


        % 2. Create vectors
        var.n=length(kinematic.x);

        % Forearm plane
        var.FAU_WRU=[kinematic.x(:,2)-kinematic.x(:,3),kinematic.y(:,2)-kinematic.y(:,3),kinematic.z(:,2)-kinematic.z(:,3)];
        var.WRR_WRU=[kinematic.x(:,2)-kinematic.x(:,1),kinematic.y(:,2)-kinematic.y(:,1),kinematic.z(:,2)-kinematic.z(:,1)];

        % Hand plane
        var.CMC5_MCP5=[kinematic.x(:,7)-kinematic.x(:,6),kinematic.y(:,7)-kinematic.y(:,6),kinematic.z(:,7)-kinematic.z(:,6)];
        var.MCP2_MCP5=[kinematic.x(:,7)-kinematic.x(:,8),kinematic.y(:,7)-kinematic.y(:,8),kinematic.z(:,7)-kinematic.z(:,8)];

        % Transpose
        var.FAU_WRU = var.FAU_WRU';
        var.WRR_WRU = var.WRR_WRU';
        var.CMC5_MCP5 = var.CMC5_MCP5';
        var.MCP2_MCP5 = var.MCP2_MCP5';

        % 3. Calculate angle of wrist flexion/extension
        % Cross vector function for all time => create normal vector plane
        var.forearm_n=[];
        var.hand_n=[];
        var.theta_rad=[];

        for i = 1:var.n % Loop through experiment

            % vector x and y of the forearm plane
            var.v1=var.FAU_WRU(:,i); % take x,y,z of the vector for every time
            var.v2=var.WRR_WRU(:,i);

            % vector x and y of the hand plane
            var.v3=var.CMC5_MCP5(:,i);
            var.v4=var.MCP2_MCP5(:,i);

            var.forearm_n= [var.forearm_n, cross(var.v1,var.v2)];  
            var.hand_n=[var.hand_n, cross(var.v3,var.v4)];

        end

        % Calculate angle  
        for i = 1:var.n

            var.n1=(var.forearm_n(:,i)); 
            var.n2=var.hand_n(:,i);

            var.scalar_product(i) = dot(var.n1,var.n2); 

            %Equation (2) of the paper
            var.theta_rad=[var.theta_rad, atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))]; % result in radian

            angle.flex_deflex_wrist{f}=(var.theta_rad*180)/pi;

        end

        % 4. Calculate angle of wrist adduction/abduction

        % Projection vector onto plane 

        var.MCP2_MCP5_forearmproj=[];
        var.WRR_WRU_forearmproj=[];
        var.rad_ul_angle_rad=[];

        for i=1:var.n

            %take x,y,z of the vector for each time
            var.v1=var.MCP2_MCP5(:,i);
            var.v2=var.WRR_WRU(:,i);

            % vector x and y of the forearm plane
            var.vfx=var.FAU_WRU(:,i); % take x,y,z of the vector for every time
            var.vfy=var.WRR_WRU(:,i);

            %projection of vector MCP2_MCP5 and WRR_WRU onto forearm plane
            var.squNorm1=(norm(var.vfx)*norm(var.vfx));
            var.squNorm2=(norm(var.vfy)*norm(var.vfy));

            var.MCP2_MCP5_forearmproj=[var.MCP2_MCP5_forearmproj,((((var.v1')*var.vfx)*var.vfx/var.squNorm1)+(((var.v1')*var.vfy)*var.vfy/var.squNorm2))];
            var.WRR_WRU_forearmproj=[var.WRR_WRU_forearmproj,((var.vfx*((var.v2')*var.vfx/var.squNorm1))+(var.vfy*((var.v2')*var.vfy/var.squNorm2)))];
        end

        % Calculate angle

        for i=1:var.n

            var.n1=var.MCP2_MCP5_forearmproj(:,i)';
            var.n2=var.WRR_WRU_forearmproj(:,i);
            var.product=var.n1*var.n2;

            var.rad_ul_angle_rad=[var.rad_ul_angle_rad, atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))];% en rad
            angle.rad_ul_wrist{f}=(var.rad_ul_angle_rad*180)/pi;
        end

    end
end

我想知道为什么我的角度总是积极的？我需要看到积极和消极的高峰...... 谢谢你的帮助！

Answer 1

由于两个平面之间的角度与它的法线之间的角度相同，我将讨论约束为两个矢量之间的角度。

我们知道两个向量（ a 和 b ）之间的交叉积是另一个向量，垂直于它们。

我们正在处理区分角度-T和+ T的问题，其中T是某个角度。

使用您使用的公式，由于使用的基本公式本身，这两个角度会产生相同的结果： atan2（| a x b |，a.b）

这是因为虽然a.b在两种情况下都是相同的，但是x b在两个向量的法线符号中不同，完全，而没有别的（使用手动规则验证自己）。当我们计算此向量的范数时，有关其符号的信息将丢失，因此该函数始终返回正值。

你可以做些什么

您需要跟踪x b的符号，以确定角度是正还是负。

注意：当我从手机回复时，我无法添加更好的格式或代码，很快就会更新答案。