Kinect v2关节角度计算 [英] Kinect v2 joint angle calculation

问题描述

您好，

我使用kienct v2计算关节角度。使用x，y，z位置（如JointArr [JointType_ElbowLeft] .Positon.X）。我使用余弦定律计算
角度。但角度程度在大多数时候都不准确。可以有人给我一些建议。谢谢！

以下是角度计算的代码。

const float AngleCalculaton（float JointStart_X，float JointStart_Y，float JointStart_Z，float JointMid_X，float JointMid_Y，float JointMid_Z， 

float JointEnd_X，float JointEnd_Y，float JointEnd_Z）

{

float JointAngle = 0;



float myJointStart_X = JointStart_X;

float myJointStart_Y = JointStart_Y;

float myJointStart_Z = JointStart_Z;

float myJointMid_X = JointMid_X;

float myJointMid_Y = JointMid_Y;

float myJointMid_Z = JointMid_Z;

float myJointEnd_X = JointEnd_X;

float myJointEnd_Y = JointEnd_Y;

float myJointEnd_Z = JointEnd_Z;
$


float JointMidToStart_X = 0;

float JointMidToStart_Y = 0;

float JointMidToStart_Z = 0;

float JointMidToEnd_X = 0;

float JointMidToEnd_Y = 0;

float JointMidToEnd_Z = 0;

float JointStartToEnd_X = 0;

float JointStartToEnd_Y = 0;

float JointStartToEnd_Z = 0;



float LenghtMidToStart;

float LenghtMidToEnd;

float LenghtStartToEnd;



JointMidToStart_X = myJointStart_X - myJointMid_X;

JointMidToStart_Y = myJointStart_Y - myJointMid_Y;


JointMidToStart_Z = myJointStart_Z - myJointMid_Z;

JointMidToEnd_X = myJointEnd_X - myJointMid_X;

JointMidToEnd_Y = myJointEnd_Y - myJointMid_Y;


JointMidToEnd_Z = myJointEnd_Z - myJointMid_Z;

JointStartToEnd_X = myJointEnd_X - myJointStart_X;

JointStartToEnd_Y = myJointEnd_Y - myJointStart_Y;

JointStartToEnd_Z = myJointEnd_Z - myJointStart_Z;



LenghtMidToStart = sqrt（（JointMidToStart_X * JointMidToStart_X）+（JointMidToStart_Y * JointMidToStart_Y）+（JointMidToStart_Z * JointMidToStart_Z） ）; $
LenghtMidToEnd = sqrt（（JointMidToEnd_X * JointMidToEnd_X）+（JointMidToEnd_Y * JointMidToEnd_Y）+（JointMidToEnd_Z * JointMidToEnd_Z））;

LenghtStartToEnd = sqrt（（JointStartToEnd_X * JointStartToEnd_X）+（JointStartToEnd_Y * JointStartToEnd_Y）+（JointStartToEnd_Z * JointStartToEnd_Z））;



JointAngle =（LenghtMidToStart * LenghtMidToStart + LenghtMidToEnd * LenghtMidToEnd - LenghtStartToEnd * LenghtStartToEnd）/（2 * LenghtMidToStart * LenghtMidToEnd ）; $


返回JointAngle * 180 / pi;

const float AngleCalculaton(float JointStart_X, float JointStart_Y, float JointStart_Z, float JointMid_X, float JointMid_Y, float JointMid_Z, 
float JointEnd_X, float JointEnd_Y, float JointEnd_Z)
{
float JointAngle = 0;

float myJointStart_X = JointStart_X;
float myJointStart_Y = JointStart_Y;
float myJointStart_Z = JointStart_Z;
float myJointMid_X = JointMid_X;
float myJointMid_Y = JointMid_Y;
float myJointMid_Z = JointMid_Z;
float myJointEnd_X = JointEnd_X;
float myJointEnd_Y = JointEnd_Y;
float myJointEnd_Z = JointEnd_Z;

float JointMidToStart_X = 0;
float JointMidToStart_Y = 0;
float JointMidToStart_Z = 0;
float JointMidToEnd_X = 0;
float JointMidToEnd_Y = 0;
float JointMidToEnd_Z = 0;
float JointStartToEnd_X = 0;
float JointStartToEnd_Y = 0;
float JointStartToEnd_Z = 0;

float LenghtMidToStart;
float LenghtMidToEnd;
float LenghtStartToEnd;

JointMidToStart_X = myJointStart_X - myJointMid_X;
JointMidToStart_Y = myJointStart_Y - myJointMid_Y;
JointMidToStart_Z = myJointStart_Z - myJointMid_Z;
JointMidToEnd_X = myJointEnd_X - myJointMid_X;
JointMidToEnd_Y = myJointEnd_Y - myJointMid_Y;
JointMidToEnd_Z = myJointEnd_Z - myJointMid_Z;
JointStartToEnd_X = myJointEnd_X - myJointStart_X;
JointStartToEnd_Y = myJointEnd_Y - myJointStart_Y;
JointStartToEnd_Z = myJointEnd_Z - myJointStart_Z;

LenghtMidToStart = sqrt((JointMidToStart_X*JointMidToStart_X) + (JointMidToStart_Y*JointMidToStart_Y) + (JointMidToStart_Z*JointMidToStart_Z));
LenghtMidToEnd = sqrt((JointMidToEnd_X*JointMidToEnd_X) + (JointMidToEnd_Y*JointMidToEnd_Y) + (JointMidToEnd_Z*JointMidToEnd_Z));
LenghtStartToEnd = sqrt((JointStartToEnd_X*JointStartToEnd_X) + (JointStartToEnd_Y*JointStartToEnd_Y) + (JointStartToEnd_Z*JointStartToEnd_Z));

JointAngle = (LenghtMidToStart*LenghtMidToStart + LenghtMidToEnd*LenghtMidToEnd - LenghtStartToEnd*LenghtStartToEnd )/ (2 * LenghtMidToStart*LenghtMidToEnd);

return JointAngle * 180 / pi;

}

推荐答案

为什么要计算这个，而不是做一个简单的转换使用JointOrientation四元数到欧拉角？

Why are you calculating this, instead of doing a simple conversion of quaternion to euler angles using JointOrientation?

这篇关于Kinect v2关节角度计算的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！