Kinect体感机器人(三)—— 空间向量法计算关节角度
By 马冬亮(凝霜 Loki)
一个人的战争(http://blog.csdn.net/MDL13412)
终于写到体感机器人的核心代码了,如何过滤、计算骨骼点是机器人控制的关键。经过摸索、评估、测试,最终得出了一个使用空间坐标进行计算的算法,下面我将进行详细讲解。
为什么是空间向量
说到角度计算,那么我们首先想到的就是解析几何,因为这是我们人类思考的方式。但是解析几何带来了一个问题——边界条件,使用其进行计算时,需要考虑各种特殊情况:平行、重叠、垂直、相交。。。这直接导致了代码量的爆炸性增长,而我们又知道,代码的BUG是与其长度呈指数级增长的,这给我们带来了沉重的心智负担,编码和调试都变得异常困难。
说到这,有人要说了,解析几何的边界条件无非就那么几种,我分模块进行编码就可以减少复杂度了,并不会损失太多。那么,请设想如下情况,如何计算手臂平面与地面的夹角?如下图:
空间解析几何带来了更多的边界条件,而Kinect在采集的过程中是不能下断点进行联机调试的,证明算法的正确性变得异常困难,这么多的边界条件,很难一一验证。
下面我们来看一组公式:
从上面这组公式可以看出,通过向量,我们可以完全摆脱边界条件的繁琐(对于骨骼点的重叠,可以通过滤波解决,见后文),只需编写通用的公式即可完成所有情况的计算,简单与优雅并存。
坐标映射
向量法使用的是常规的数学坐标系,而Kinect的坐标系与常规数学坐标系稍有不同,如下图所示:
由此可知,要使用向量,首先就要将Kinect的坐标系映射到数学坐标系上,其方法非常简单:由向量的可平移性质及方向性,可以推导出Kinect坐标系中任意两个不重合的坐标点A(x1, y1, z1),B(x2, y2, z2)经过变换,可转化到数学坐标系中,对其组成的向量AB,可以认为是从坐标轴零点引出,转化公式如下:
根据上述性质,可以将人体关节角度计算简化为对空间向量夹角的计算。
空间向量法计算关节角度
由于所选用机器人的关节处舵机存在诸多限制,对于大臂保持静止,小臂与大臂垂直的旋转动作,需要借助于肩膀上的舵机进行联合调节。这就要求不能简单的只计算两空间向量的夹角,为此特提出了一种渐进算法,即求空间平面xOz与肩膀、肘关节、手所组成平面的夹角,并以其夹角完成对肩膀舵机的调速工作。
下面是实际人体左臂动作的计算过程,实拍人体动作照片见图A,左臂提取出的关节点在Kinect空间坐标系中的向量表示见图B,经过变换后转化为普通坐标系中的向量见图C。
图A 人体动作
图B 左臂关节点在Kinect困难关键坐标系中的向量表示
上图中:各个关节点(肩膀,肘,手)是处在空间平面中,对应z轴从里到外分别为:肩膀,肘,手,且三点在向量图中均处于z轴负半轴。
图C 经过变换后转化为数学坐标系中的向量
对于肘关节角度的计算,可以直接使用空间向量ES和EH的夹角得出,计算过程如下:
对于大臂的上下摆动角度,可以将向量ES投影到xOy平面上,并求其与y坐标轴的夹角得出,计算过程及公式类似于肘关节角度的计算过程。
对于协助小臂转动的肩膀舵机的角度计算,其向量转化关系下所示:
为了求取空间平面夹角,需要首先求取两平面的法向量,再根据法向量计算出两平面夹角。计算过程如下:
式(3-5)和式(3-6)分别计算出向量ES和向量EH,分别对应肘关节指向肩膀和肘关节指向手腕的两条向量;式(3-7)通过叉乘计算出肩膀、肘、手所构成空间平面的法向量n1;式(3-8)代表空间平面xOz的法向量;式(3-9)求取法向量n1与法向量n2的夹角,从而完成对协助小臂转动的肩膀舵机的角度计算。
对于腿部的识别,由于人体小腿无法旋转,故只需采用两向量夹角及投影到平面的方式进行求取,与手臂部分相似,不再详述。
腿部姿态检测
首先,由于机器人模仿人体腿部动作时会遇到平衡问题,为了解决此问题,需要给机器人加装陀螺仪和及加速度传感器,实时调整机器人重心,保持机器人站立的稳定性。但是在机器人调整稳定性同时,会导致机器人上肢的晃动,在机器人实际工作时,会造成手臂动作发生异常,可能导致意外发生。其次,机器人腿部动作大多局限于行走、转向、下蹲、站立等几个固定动作,让机器人完全模仿人体腿部动作,会给用户带来非常多的冗余操作,使用户不能专注于业务细节而需要专注于控制机器人腿部动作;最后,由于本文使用的人形机器人关节并不与人体关节一一对应,势必会造成控制上的误差,这可能带来灾难性的后果。
综上所述,通过识别人体腿部特定动作,支持机器人前进、后退、左转、右转、下蹲、站立,即可满足绝大多数情况下对机器人腿部动作的要求,并且有效的减少了用户的操作复杂度,让用户可以专注于业务细节。
为了支持机器人前进、后退、左转、右转、下蹲、站立这几个固定动作,需要对人体腿部姿态进行检测,从而控制机器人完成相应动作。检测算法首先检测左腿髋关节是否达到确认度阀值,若达到,则先检测是否为下蹲姿势,若不为下蹲,则检测左侧髋关节指向膝关节的向量相对于前、左、后三个方向哪个方向的权值更大,并取其权值最大的作为机器人控制信号,其分别对应与机器人的前进、左转、后退动作;若未达到阀值,则检测右髋关节是否达到确认度阀值,若达到,则检测右侧髋关节指向膝关节的向量相对于前、右、后三个方向哪个方向的权值更大,并取其权值最大的作为机器人控制信号,其分别对应与机器人的前进、右转、后退动作;若未达到阀值,则判定为机器人站立动作。
腿部姿态详细检测流程如下图所示:
滤波算法
由于Kinect传感器采集到的数据会有扰动,从而造成机器人控制的不稳定性,因此必须对识别出来的骨骼点进行滤波处理,以保证机器人动作稳定、连贯。
对于滤波算法的选择,要综合考虑运算速度、内存占用、准确性、抗随机干扰能力等技术指标。这就要求对采样数据进行分析,从而选取滤波效果最好的算法。
本识别程序运行于EPCM-505C开发平台,在只进行关节识别的情况下,每秒能识别6-8帧图像,加上空间坐标向量运算及腿部姿势识别后,每秒能处理4-5帧图像。由于期望尽可能快的向机器人发送控制数据,以提高机器人的响应速度。因此,所选择的滤波算法应尽可能快速。
经过对OpenNI识别出的关节点空间坐标分析可知,其扰动一般是在人体实际关节坐标的四周做小幅度波动,另外存在一些识别死区,此时无法检测到关节点。因此,所选用的滤波算法要保证机器人的正确运行,对无法识别的关节点做相应处理,对小幅度波动的关节点保持上一状态不变。
综上所述,本文提出了一种改进型的限幅滤波算法,此滤波算法采用了动态规划的思想,保证每次滤波后的结果都是最优解,从而从整体上得出最优解。滤波算法的详细流程下图所示:
经过与常用滤波算法对比实验证明,此算法滤波效果良好,能满足对机器人控制的需求。详细对比结果如下表所示:
算法名称 技术指标 | 改进型限幅滤波算法 | 限幅滤波 算法 | 算术平均值 滤波算法 | 滑动平均值 滤波算法 |
速度 | 快 | 快 | 慢 | 较快 |
内存占用 | 少 | 少 | 多 | 多 |
能识别细微幅度动作 | 否 | 否 | 能 | 能 |
对小幅度扰动过滤效果 | 优 | 优 | 优 | 优 |
滤波结果是否正确 | 是 | 是 | 不一定 | 不一定 |
由于有16个关节角度需要计算,在PC上每秒可以运行30帧,即16 * 30 = 480次三角函数运算,这很明显是需要用打表进行优化的,下面是哈系表的代码,如果不明白,请绘制cos函数曲线,再进行对比阅读:
- /**
- * @brief 性能分析代码.
- * @ingroup ProgramHelper
- *
- * @details 用于分析查询哈希表和直接使用C库的三角函数计算角度值的性能.
- *
- * @code
- *
- #include <cstdlib>
- #include <iostream>
- #include <fstream>
- #include <cmath>
- #include <iomanip>
- #include <ctime>
- using namespace std;
- int main(int argc, char** argv)
- {
- clock_t start,finish;
- volatile double dummy;
- start=clock();
- // for (int i = 0; i < 1000; ++i)
- // for (int j = 0; j < 100000; ++j)
- // dummy = cos((i % 3) * M_PI / 180);
- for (int i = 0; i < 1000; ++i)
- for (int j = 0; j < 100000; ++j)
- dummy = (int)(((i % 3) * 1000)) % 100000;
- // for (int i = 0; i < 1000; ++i)
- // for (int j = 0; j < 100000; ++j)
- // dummy = i;
- finish = clock();
- cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;
- return 0;
- }
- *
- * @endcode
- */
/**
* @brief 性能分析代码.
* @ingroup ProgramHelper
*
* @details 用于分析查询哈希表和直接使用C库的三角函数计算角度值的性能.
*
* @code
*
include <cstdlib>
include <iostream>
include <fstream>
include <cmath>
include <iomanip>
include <ctime>
using namespace std;
int main(int argc, char** argv)
{
clock_t start,finish;
volatile double dummy;
start=clock();
// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = cos((i % 3) * M_PI / 180);
for (int i = 0; i < 1000; ++i)
for (int j = 0; j < 100000; ++j)
dummy = (int)(((i % 3) * 1000)) % 100000;
// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = i;
finish = clock();
cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;
return 0;
}
*
* @endcode
*/
- /
- @brief 生成cos哈希表的索引范围.
- @ingroup ProgramHelper
- @details 将1-90度的cos值经过Hash函数变换, 得出一个哈希范围.
- @see CosHashTable
- @code
- #include <cstdlib>
- #include <iostream>
- #include <fstream>
- #include <cmath>
- #include <iomanip>
- using namespace std;
- int main(int argc, char argv)
- {
- ofstream fout(“a.txt”);
- for (int i = 90; i > 0; –i)
- {
- fout << setw(6) << (((int)(cos((double)i M_PI / 180) 100000)) % 100001);
- if (0 == i % 10)
- fout << ”,” << endl;
- else
- fout << ”,”;
- }
- fout << “100000”;
- fout.flush();
- fout.close();
- return 0;
- }
- @endcode
- /
- static int s_initArccosHash[] =
- {
- 1745, 3489, 5233, 6975, 8715, 10452, 12186, 13917, 15643, 17364,
- 19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
- 35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
- 51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
- 65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
- 77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
- 87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
- 94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
- 98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
- };
/*
* @brief 生成cos哈希表的索引范围.
* @ingroup ProgramHelper
*
* @details 将1-90度的cos值经过Hash函数变换, 得出一个哈希范围.
* @see CosHashTable
*
* @code
*
include <cstdlib>
include <iostream>
include <fstream>
include <cmath>
include <iomanip>
using namespace std;
int main(int argc, char** argv)
{
ofstream fout(“a.txt”);
for (int i = 90; i > 0; --i)
{
fout << setw(6) << (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001);
if (0 == i % 10)
fout << "," << endl;
else
fout << ",";
}
fout << "100000";
fout.flush();
fout.close();
return 0;
}
*
* @endcode
*/
static int s_initArccosHash[] =
{
1745, 3489, 5233, 6975, 8715, 10452, 12186, 13917, 15643, 17364,
19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
};
- /
- @brief acrcos哈希表
- @ingroup ProgramHelper
- @details 哈希函数:
- 角度 = s_arccosHash[((int)(cos(degree) 100000) % 100001]
- */
- static int s_arccosHash[100001];
/
* @brief acrcos哈希表
* @ingroup ProgramHelper
*
* @details 哈希函数:
* 角度 = s_arccosHash[((int)(cos(degree) * 100000) % 100001]
*/
static int s_arccosHash[100001];
这里只给出左臂的关键代码,右臂及腿部类似,不再类述:
- // 计算机器人关节角度
- /**
- * @brief 计算机器人的左臂上3个舵机的角度.
- * @ingroup SceneDrawer
- *
- * @param shoulder 肩膀的坐标
- * @param elbow 肘关节的坐标
- * @param hand 手(腕)的坐标
- *
- * @details 所有坐标均采用向量法进行计算.
- */
- inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
- const XnSkeletonJointPosition &elbow,
- const XnSkeletonJointPosition &hand)
- {
- MyVector3D vector1;
- MyVector3D vector2;
- MyVector3D normal1;
- MyVector3D normal2;
- double deltaNormal1;
- double deltaNormal2;
- double deltaVector1;
- double deltaVector2;
- double cosAngle;
- if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
- {
- // vector1 -> shoulder - elbow
- // vector2 -> hand - elbow
- vector1.X = shoulder.position.X - elbow.position.X;
- vector1.Y = shoulder.position.Y - elbow.position.Y;
- vector1.Z = elbow.position.Z - shoulder.position.Z;
- vector2.X = hand.position.X - elbow.position.X;
- vector2.Y = hand.position.Y - elbow.position.Y;
- vector2.Z = elbow.position.Z - hand.position.Z;
- // normal1 = vector1 x vector2
- normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
- normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
- normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;
- normal2.X = 0.0;
- normal2.Y = -150.0;
- normal2.Z = 0.0;
- deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
- deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);
- if (deltaNormal1 * deltaNormal2 > 0.0)
- {
- cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);
- if (cosAngle < 0.0)
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
- else
- {
- if (shoulder.position.Y < hand.position.Y)
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];
- }
- }
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
- vector1.X = elbow.position.X - shoulder.position.X;
- vector1.Y = elbow.position.Y - shoulder.position.Y;
- vector1.Z = 0.0;
- vector2.X = 0.0;
- vector2.Y = 100;
- vector2.Z = 0.0;
- deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
- deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
- if (deltaVector1 * deltaVector2 > 0.0)
- {
- cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
- if (cosAngle < 0.0)
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
- }
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
- vector1.X = shoulder.position.X - elbow.position.X;
- vector1.Y = shoulder.position.Y - elbow.position.Y;
- vector1.Z = elbow.position.Z - shoulder.position.Z;
- vector2.X = hand.position.X - elbow.position.X;
- vector2.Y = hand.position.Y - elbow.position.Y;
- vector2.Z = elbow.position.Z - hand.position.Z;
- deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
- deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
- if (deltaVector1 * deltaVector2 > 0.0)
- {
- cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
- if (cosAngle < 0.0)
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
- }
- else
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
- } // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
- } // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
- #ifdef DEBUG_MSG_LEFT_ARM
- char bufferLeftArm[200];
- snprintf(bufferLeftArm, sizeof(bufferLeftArm),
- ”LEFT_SHOULDER_VERTICAL = %4d LEFT_SHOULDER_HORIZEN = %4d LEFT_ELBOW = %4d”,
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],
- g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
- std::cout << bufferLeftArm << std::endl;
- NsLog()->info(bufferLeftArm);
- #endif
- }
// 计算机器人关节角度
/**
* @brief 计算机器人的左臂上3个舵机的角度.
* @ingroup SceneDrawer
*
* @param shoulder 肩膀的坐标
* @param elbow 肘关节的坐标
* @param hand 手(腕)的坐标
*
* @details 所有坐标均采用向量法进行计算.
*/
inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
const XnSkeletonJointPosition &elbow,
const XnSkeletonJointPosition &hand)
{
MyVector3D vector1;
MyVector3D vector2;
MyVector3D normal1;
MyVector3D normal2;
double deltaNormal1;
double deltaNormal2;
double deltaVector1;
double deltaVector2;
double cosAngle;
if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
{
// vector1 -> shoulder - elbow
// vector2 -> hand - elbow
vector1.X = shoulder.position.X - elbow.position.X;
vector1.Y = shoulder.position.Y - elbow.position.Y;
vector1.Z = elbow.position.Z - shoulder.position.Z;
vector2.X = hand.position.X - elbow.position.X;
vector2.Y = hand.position.Y - elbow.position.Y;
vector2.Z = elbow.position.Z - hand.position.Z;
// normal1 = vector1 x vector2
normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;
normal2.X = 0.0;
normal2.Y = -150.0;
normal2.Z = 0.0;
deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);
if (deltaNormal1 * deltaNormal2 > 0.0)
{
cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
else
{
if (shoulder.position.Y < hand.position.Y)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
vector1.X = elbow.position.X - shoulder.position.X;
vector1.Y = elbow.position.Y - shoulder.position.Y;
vector1.Z = 0.0;
vector2.X = 0.0;
vector2.Y = 100;
vector2.Z = 0.0;
deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
if (deltaVector1 * deltaVector2 > 0.0)
{
cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
vector1.X = shoulder.position.X - elbow.position.X;
vector1.Y = shoulder.position.Y - elbow.position.Y;
vector1.Z = elbow.position.Z - shoulder.position.Z;
vector2.X = hand.position.X - elbow.position.X;
vector2.Y = hand.position.Y - elbow.position.Y;
vector2.Z = elbow.position.Z - hand.position.Z;
deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);
if (deltaVector1 * deltaVector2 > 0.0)
{
cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);
if (cosAngle < 0.0)
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
}
else
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
} // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
else
{
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
} // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
ifdef DEBUG_MSG_LEFT_ARM
char bufferLeftArm[200];
snprintf(bufferLeftArm, sizeof(bufferLeftArm),
"LEFT_SHOULDER_VERTICAL = %4d LEFT_SHOULDER_HORIZEN = %4d LEFT_ELBOW = %4d",
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],
g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],
g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
std::cout << bufferLeftArm << std::endl;
NsLog()->info(bufferLeftArm);
endif
}
- /**
- * @brief 判别机器人行走及下蹲.
- * @ingroup SceneDrawer
- *
- * @details 前后左右行走,下蹲.
- */
- inline void PoseDetect()
- {
- // 首先判断左腿
- if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
- {
- // 判断是否为蹲
- if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
- g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
- return;
- }
- // 需要判断向左踢腿的情况
- if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
- {
- // 判断是否达到向左踢腿的阀值
- if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
- {
- // 判断哪个方向的分量的权值更大
- if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
- abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
- return;
- }
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
- abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
- return;
- }
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
- return;
- }
- }
- else
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- }
- }
- else // 直接判断是否是前进姿势
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- }
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
- }
- if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
- {
- if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
- abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
- return;
- }
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
- abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
- return;
- }
- }
- else
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
- return;
- }
- }
- else
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- }
- }
- else // 直接判断是否是前进姿势
- {
- if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
- return;
- }
- else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
- {
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
- return;
- }
- }
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
- }
- g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
- }
/**
* @brief 判别机器人行走及下蹲.
* @ingroup SceneDrawer
*
* @details 前后左右行走,下蹲.
*/
inline void PoseDetect()
{
// 首先判断左腿
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
{
// 判断是否为蹲
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
return;
}
// 需要判断向左踢腿的情况
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
{
// 判断是否达到向左踢腿的阀值
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
{
// 判断哪个方向的分量的权值更大
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
return;
}
}
else
{
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
}
else // 直接判断是否是前进姿势
{
if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
{
if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
return;
}
}
else
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
}
else // 直接判断是否是前进姿势
{
if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
return;
}
else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
{
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
return;
}
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}
g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}
- /**
- * @brief 绘制骨骼图, 并返回相应的坐标点.
- * @ingroup SceneDrawer
- *
- * @param player 用户ID
- * @param eJoint1 第一个关节点ID
- * @param eJoint2 第二个关节点ID
- * @param joint1 [out] 关节点1
- * @param joint2 [out] 关节点2
- */
- inline void DrawLimbAndGetJoint(XnUserID player,
- XnSkeletonJoint eJoint1,
- XnSkeletonJoint eJoint2,
- XnSkeletonJointPosition &joint1,
- XnSkeletonJointPosition &joint2)
- {
- if (!TrackerViewer::getInstance().UserGenerator
- .GetSkeletonCap().IsTracking(player))
- {
- printf(”not tracked!\n”);
- return;
- }
- TrackerViewer::getInstance().UserGenerator
- .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
- TrackerViewer::getInstance().UserGenerator
- .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
- if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
- {
- return;
- }
- XnPoint3D pt[2];
- pt[0] = joint1.position;
- pt[1] = joint2.position;
- TrackerViewer::getInstance().DepthGenerator.
- ConvertRealWorldToProjective(2, pt, pt);
- glVertex3i(pt[0].X, pt[0].Y, 0);
- glVertex3i(pt[1].X, pt[1].Y, 0);
- }
/**
* @brief 绘制骨骼图, 并返回相应的坐标点.
* @ingroup SceneDrawer
*
* @param player 用户ID
* @param eJoint1 第一个关节点ID
* @param eJoint2 第二个关节点ID
* @param joint1 [out] 关节点1
* @param joint2 [out] 关节点2
*/
inline void DrawLimbAndGetJoint(XnUserID player,
XnSkeletonJoint eJoint1,
XnSkeletonJoint eJoint2,
XnSkeletonJointPosition &joint1,
XnSkeletonJointPosition &joint2)
{
if (!TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
TrackerViewer::getInstance().UserGenerator
.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
TrackerViewer::getInstance().DepthGenerator.
ConvertRealWorldToProjective(2, pt, pt);
glVertex3i(pt[0].X, pt[0].Y, 0);
glVertex3i(pt[1].X, pt[1].Y, 0);
}
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);
- s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
- s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
- s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);
- s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
- s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
- s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);
- s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
- s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
- s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
- DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);
- s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
- s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
- s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);
s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);
s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);
s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);
s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;
- static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];
- /**
- * @brief 对空间坐标点进行过滤.
- * @ingroup SceneDrawer
- *
- * @param id 关节ID
- * @param point [in out] 要过滤的点
- * @param filter 过滤阀值
- */
- inline void PointFilter(enum XnSkeletonJoint id,
- XnSkeletonJointPosition &point,
- const int filter)
- {
- if (point.fConfidence < JOINT_CONFIDENCE) // 过滤掉阀值以下的点
- {
- point = s_pointFilter[id];
- }
- else
- {
- if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
- {
- s_pointFilter[id] = point;
- }
- else
- {
- if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
- abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
- abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
- {
- point = s_pointFilter[id];
- }
- else
- {
- s_pointFilter[id] = point;
- }
- }
- }
- }
static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];
/**
* @brief 对空间坐标点进行过滤.
* @ingroup SceneDrawer
*
* @param id 关节ID
* @param point [in out] 要过滤的点
* @param filter 过滤阀值
*/
inline void PointFilter(enum XnSkeletonJoint id,
XnSkeletonJointPosition &point,
const int filter)
{
if (point.fConfidence < JOINT_CONFIDENCE) // 过滤掉阀值以下的点
{
point = s_pointFilter[id];
}
else
{
if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
{
s_pointFilter[id] = point;
}
else
{
if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
{
point = s_pointFilter[id];
}
else
{
s_pointFilter[id] = point;
}
}
}
}
完整源码及论文
2015年9月6日更新下载链接:
2017年5月8日更新下载链接