algorithm - 没有获得关于 Kinect 的正确用户方向

Question

我在项目中使用 Microsoft Kinect。我必须完成的任务之一是找到用户 w.r.t Kinect 传感器的方向(当用户转动时，方向会改变) 为此，我试图找到连接肩膀的线与 Kinect 的 x 轴所成的角度。

我想出了以下代码，但它给了我非常小的角度值，即使我转了将近 40 度也是如此。

double vector_x=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].x-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].x;
    double vector_y=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].y-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].y;
    double vector_z=skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].z-skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].z;
    double len1=sqrtf(vector_x * vector_x + vector_y * vector_y + vector_z * vector_z);
    double vector_x1=1.0;
    double vector_y1=0.0;
    double vector_z1=0.0; 
    double len2=sqrtf(vector_x1 * vector_x1 + vector_y1 * vector_y1 + vector_z1 * vector_z1);
    double dot_product = vector_x * vector_x1 + vector_y * vector_y1 + vector_z * vector_z1; 
    double angle = dot_product / (len1 * len2); 
    coor_left=Convert(vector_x)+"\t"+Convert(vector_y)+"\t"+Convert(vector_z)+"\n";
OutputDebugStringA(Convert(acos(angle)).c_str());

当我添加弧度到度数的转换时，

double angle1=angle*180.0/3.14;

我得到的值从 -33 到 -57(当我面对 Kinect 时)，然后又变成 -33。

但实际上，它应该是负数，然后是 0，然后另一边是正数。我哪里错了？

Answer 1

我自己解决了。我意识到我正在寻找不正确向量之间的角度。

我需要做的就是将左肩和右肩投影到 x-z 平面上，然后将问题简化为找到平面中两个向量之间的角度。

这是我做的:

double CalcAngle(double p1x,double p1y, double p2x,double p2y, double p3x,double p3y, double p4x,double p4y)
{
//
// calculate the angle between the line from p1 to p2
// and the line from p3 to p4
//
double x1 = p1x - p2x;
double y1 = p1y - p2y;
double x2 = p3x - p4x;
double y2 = p3y - p4y;
//
double angle1 , angle2 , angle;
//
if (x1 != 0.0f)
angle1 = atan(y1/x1);
else
angle1 = 3.14159 / 2.0;  // 90 degrees
//
if (x2 != 0.0f)
angle2 = atan(y2/x2);
else
angle2 = 3.14159 / 2.0;  // 90 degrees
//
angle = fabs(angle2-angle1);
angle = angle * 180.0 / 3.14159;    // convert to degrees ???
//
return angle;
}

double myangle=CalcAngle(skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_LEFT].z,
        skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].x,skel.SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT].z,
        0,0,1,0);
        OutputDebugStringA(Convert(myangle).c_str());
    OutputDebugStringA("\n");