void addJoint(XnUserID userID, XnSkeletonJoint eJoint, int dstJointID, PackedType<Vector3<double> > & packedVector3) {
XnSkeletonJointPosition joint;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(userID, eJoint, joint);
if (joint.fConfidence == 0) {
return;
}
XnPoint3D pt;
pt = joint.position;
g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
//@see: http://graphics.stanford.edu/~mdfisher/Kinect.html
static const double fx_d = 1.0/5.9421434211923247e+02;
static const double fy_d = 1.0/5.9104053696870778e+02;
static const double cx_d = 3.3930780975300314e+02;
static const double cy_d = 2.4273913761751615e+02;
double z = pt.Z/1000;
double x = (pt.X - cx_d)*z*fx_d;
double y = (pt.Y - cy_d)*z*fy_d*-1;
vector<pair<int, Vector3<double> > > & v = packedVector3.getItems();
v.push_back(pair<int, Vector3<double> >(dstJointID, Vector3<double>(x, y, z)));
}
bool DumpJoint(XnUserID player, XnSkeletonJoint eJoint, Joint &out_joint)
{
// Check the user is being tracked
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
return false;
}
// Check the joint is actually there
if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
{
return false;
}
// Extract joint positions
XnSkeletonJointPosition joint;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
XnPoint3D pt, imagePt;
pt = joint.position;
g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &imagePt);
out_joint.id = eJoint;
out_joint.confidence = joint.fConfidence;
out_joint.x = pt.X;
out_joint.y = pt.Y;
out_joint.z = pt.Z;
out_joint.u = imagePt.X;
out_joint.v = imagePt.Y;
out_joint.w = imagePt.Z;
return true;
}
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
XnSkeletonJointPosition joint1, joint2;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
#ifndef USE_GLES
glVertex3i(pt[0].X, pt[0].Y, 0);
glVertex3i(pt[1].X, pt[1].Y, 0);
#else
GLfloat verts[4] = {pt[0].X, pt[0].Y, pt[1].X, pt[1].Y};
glVertexPointer(2, GL_FLOAT, 0, verts);
glDrawArrays(GL_LINES, 0, 2);
glFlush();
#endif
}
//draws one limb
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
XnSkeletonJointPosition joint1, joint2;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
glVertex3i(pt[0].X, pt[0].Y, 0);
glVertex3i(pt[1].X, pt[1].Y, 0);
//std::cout << pt[0].X << " ," << pt[0].Y << std::endl;
}
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
{
printf("not calibrated!\n");
return;
}
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
XnSkeletonJointPosition joint1, joint2;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);
if (joint1.fConfidence < 0.75 || joint2.fConfidence < 0.75)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;bool isCalibrated = false;
pt[1] = joint2.position;
g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
glVertex3i(pt[0].X, pt[0].Y, 0);
glVertex3i(pt[1].X, pt[1].Y, 0);
// printf("Points: %f, %f, %f \n");
}
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint)
{
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
{
return;
}
XnSkeletonJointPosition joint;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
if (joint.fConfidence < 0.5)
{
return;
}
XnPoint3D pt;
pt = joint.position;
g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
drawCircle(pt.X, pt.Y, 2);
}
void XN_CALLBACK_TYPE Hand_Update(
xn::HandsGenerator& generator,
XnUserID nId,
const XnPoint3D* pPosition,
XnFloat fTime,
void* pCookie)
{
depth_generator.ConvertRealWorldToProjective(1, pPosition, &projective_point);
}
void ConvertSkeletonMarkersToScreenPoints(
xn::DepthGenerator& depth_generator,
std::vector<XnSkeletonJointPosition>& skeleton_markers,
std::vector<sf::Vector2f>& screen_points)
{
for (auto it = skeleton_markers.begin(); it != skeleton_markers.end(); ++it)
{
XnPoint3D screen_point;
depth_generator.ConvertRealWorldToProjective(1, &(it->position), &screen_point);
screen_points.push_back(sf::Vector2f(screen_point.X, screen_point.Y));
}
}
XnPoint3D PointForJoint(XnUserID user, XnSkeletonJoint joint)
{
XnSkeletonJointPosition jointPos;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, jointPos);
XnPoint3D pt = jointPos.position;
// printf("%f %f %f confidence %f\n",jointPos.position.X, jointPos.position.Y, jointPos.position.Z, jointPos.fConfidence);
g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
return pt;
}
void gl_labelCapturingUser(XnUserID id)
{
XnVector3D pos;
gUserGenerator.GetCoM(id, pos);
gDepthGenerator.ConvertRealWorldToProjective(1, &pos, &pos);
glColor4f(0, 0, 0, 0);
xn::DepthMetaData depthMD;
gDepthGenerator.GetMetaData(depthMD);
glRasterPos2i(pos.X/depthMD.XRes()*WIN_SIZE_X,
pos.Y/depthMD.YRes()*WIN_SIZE_Y);
gl_printString(GLUT_BITMAP_HELVETICA_18, "Capturing");
glColor4f(1.0, 1.0, 1.0, 1.0);
}
// スケルトンを描画する
void DrawSkelton(XnUserID player, int idx){
// 線を引く開始と終了のJointの定義
XnSkeletonJoint joints[][2] = {
{XN_SKEL_HEAD, XN_SKEL_NECK},
{XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER},
{XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW},
{XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND},
{XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER},
{XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW},
{XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND},
{XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO},
{XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO},
{XN_SKEL_TORSO, XN_SKEL_LEFT_HIP},
{XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE},
{XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT},
{XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP},
{XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE},
{XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT},
{XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP}
};
XnSkeletonJointPosition joint1, joint2;
int nJointsCount = sizeof(joints) / sizeof(joints[0]);
int color_idx = idx;
if( color_idx > (sizeof(g_Colors) / sizeof(g_Colors[0])) ){
color_idx = (sizeof(g_Colors) / sizeof(g_Colors[0])) - 1;
}
for(int i = 0; i < nJointsCount;i++){
g_SkeletonCap.GetSkeletonJointPosition(player, joints[i][0], joint1);
g_SkeletonCap.GetSkeletonJointPosition(player, joints[i][1], joint2);
if (joint1.fConfidence < 0.2 || joint2.fConfidence < 0.2){
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
// 線で結んで
cvLine( g_rgbImage, cvPoint(pt[0].X, pt[0].Y), cvPoint(pt[1].X, pt[1].Y), g_Colors[color_idx], 1, CV_AA);
// それぞれの点を塗りつぶす
cvCircle(g_rgbImage, cvPoint(pt[0].X, pt[0].Y), 2, g_Colors[color_idx], -1, CV_AA, 0);
cvCircle(g_rgbImage, cvPoint(pt[1].X, pt[1].Y), 2, g_Colors[color_idx], -1, CV_AA, 0);
}
}
/**
* Body に画像をコラージュ
*/
void drawBodyPreLoad(xn::DepthGenerator& depth, xn::SkeletonCapability& capability, XnSkeletonJoint joint, XnUserID user, XnMapOutputMode mapMode, IplImage* preLoadImage, IplImage **rgbImage) {
XnSkeletonJointPosition pos;
IplImage *partImage = NULL;
IplImage *fallImage = NULL;
// ジョイント座標の取得
capability.GetSkeletonJointPosition(user, joint, pos);
XnPoint3D pReal[1] = {pos.position};
XnPoint3D pProjective[1];
// 世界座標系から表示座標系に変換した座標を取得
depth.ConvertRealWorldToProjective(1, pReal, pProjective);
// 重ね合わせよう画像
partImage = preLoadImage;
fallImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);
CvPoint2D32f original[3];
CvPoint2D32f transform[3];
original[0] = cvPoint2D32f(0, 0);
original[1] = cvPoint2D32f(mapMode.nXRes, 0);
original[2] = cvPoint2D32f( 0, mapMode.nYRes);
CvSize sizeOfPart = cvGetSize(partImage);
int transX = pProjective[0].X - (sizeOfPart.width / 2);
int transY = pProjective[0].Y - (sizeOfPart.height / 2);
transform[0] = cvPoint2D32f( transX, transY);
transform[1] = cvPoint2D32f( transX + mapMode.nXRes, transY);
transform[2] = cvPoint2D32f( transX , transY + mapMode.nYRes);
// 行列作成
CvMat *affineMatrix = cvCreateMat(2, 3, CV_32FC1);
cvGetAffineTransform(original, transform, affineMatrix);
// 移動
cvWarpAffine(partImage, fallImage, affineMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
// 画像の重ね合わせ
fallPartImage(fallImage, *rgbImage);
// 解放
cvReleaseImage(&fallImage);
//cvReleaseImage(&partImage);
}
void handtrajectory(xn::UserGenerator& userGenerator,
xn::DepthGenerator& depthGenerator,
XnUserID player, XnSkeletonJoint eJoint, bool updateHistory)
{
XnSkeletonJointPosition joint;
userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
if (joint.fConfidence < 0.5){
return;
}
XnPoint3D pt_world, pt_screen;
pt_world = joint.position;
depthGenerator.ConvertRealWorldToProjective(1, &pt_world, &pt_screen);
History *history;
if (GetHistoryForJoint (eJoint, &history) == false) return;
if (updateHistory) history->StoreValue (pt_world, pt_screen); // store value in the history
// Visualize history
//
XnFloat pVertexBuffer [HISTORY_DRAW_SIZE * sizeof (float) * 3];
XnFloat *pVertex = pVertexBuffer;
// Prepare vertex buffer for drawing
XnPoint3D pt;
for (int k = 0; k < history->Size(); ++k) {
history->GetValueScreen (k, pt);
*pVertex++ = pt.X;
*pVertex++ = pt.Y;
*pVertex++ = 0.0f;
}
glColor3f(0.f, 1.f, 0.f);
glVertexPointer(3, GL_FLOAT, 0, pVertexBuffer);
// draw trajectory
glLineWidth(2);
glDrawArrays(GL_LINE_STRIP, 0, history->Size());
// draw history points
glPointSize(8);
glDrawArrays(GL_POINTS, 0, history->Size());
}
void Skeleton :: computeJoints(int user_id,
xn::UserGenerator& user_generator,
xn::DepthGenerator& depth_generator)
{
XnPoint3D ni_p[NumJoints];
for (int j = 0; j < NumJoints; ++j)
{
XnSkeletonJointPosition joint;
user_generator.GetSkeletonCap().GetSkeletonJointPosition(user_id,
xnJointList(j),
joint);
ni_p[j] = joint.position;
m_joints[j] = Point3f(ni_p[0].X, ni_p[0].Y, ni_p[0].Z);
}
depth_generator.ConvertRealWorldToProjective(NumJoints, ni_p, ni_p);
for (int j = 0; j < NumJoints; ++j)
{
m_projected_joints[j] = Point3f(ni_p[j].X, ni_p[j].Y, ni_p[j].Z);
}
}
XnPoint3D getJointPoint(XnUserID player, XnSkeletonJoint eJoint) {
XnPoint3D pt;
if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
{
printf("not calibrated!\n");
return pt;
}
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return pt;
}
XnSkeletonJointPosition joint;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
if (joint.fConfidence < 0.5)
{
return pt;
}
g_DepthGenerator.ConvertRealWorldToProjective(2, &joint.position, &pt);
return pt;
}
void DrawCircle(xn::UserGenerator& userGenerator,
xn::DepthGenerator& depthGenerator,
XnUserID player, XnSkeletonJoint eJoint, float radius, XnFloat *color3f)
{
XnSkeletonJointPosition joint;
userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
if (joint.fConfidence < 0.5){
return;
}
XnPoint3D pt;
pt = joint.position;
depthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
float cx = pt.X;
float cy = pt.Y;
float r = radius;
int num_segments = 16;
glColor3f(color3f[0], color3f[1], color3f[2]);
glBegin(GL_TRIANGLE_FAN);
glVertex2f(cx , cy);
for(int i = 0; i <= num_segments; i++)
{
float theta = 2.0f * 3.1415926f * float(i) / float(num_segments);//get the current angle
float x = r * cosf(theta);//calculate the x component
float y = r * sinf(theta);//calculate the y component
glVertex2f(x +cx , y + cy);//output vertex
}
glEnd();
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
#ifndef USE_GLES
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
sprintf(strLabel, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
sprintf(strLabel, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
#endif
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
#ifndef USE_GLES
glBegin(GL_LINES);
#endif
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
//---------------- get skeleton data ----------------------
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_HEAD, head);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_NECK, neck);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_TORSO, torso);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_SHOULDER, leftShoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_ELBOW, leftElbow);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_HAND, leftHand);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_SHOULDER, rightShoulder);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_ELBOW, rightElbow);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_HAND, rightHand);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_HIP, leftHip);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_KNEE, leftKnee);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_LEFT_FOOT, leftFoot);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_HIP, rightHip);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_KNEE, rightKnee);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
XN_SKEL_RIGHT_FOOT, rightFoot);
//-------------- store frame data in frameData ------------
char headX[30], headY[30], headZ[30], headC[30];
strcpy(frameData, "");
char space[2] = " ";
sprintf(headX, "%f", head.position.X);
sprintf(headY, "%f", head.position.Y);
sprintf(headZ, "%f", head.position.Z);
sprintf(headC, "%f", head.fConfidence);
strcat(frameData, headX);
strcat(frameData, space);
strcat(frameData, headY);
strcat(frameData, space);
strcat(frameData, headZ);
strcat(frameData, space);
strcat(frameData, headC);
strcat(frameData, space);
//std::cout << "Frame data: " << frameData << std::endl;
std::cout << "frameData to be sent: " << frameData << std::endl;
HandleConnection(frameData);
////------- print to console -------
//xnOSGetTimeStamp(&nNow);
//nNow /= 1000;
//std::cout << "-------------time: " << nNow << "---------------\n"
// << "User " << aUsers[i] <<std::endl
// << " Head: " << head.position.X << " "
// << head.position.Y << " " << head.position.Z
// << head.fConfidence << std::endl;
#ifndef USE_GLES
glEnd();
#endif
}
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
sprintf(strLabel, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
sprintf(strLabel, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
//Draw skeleton
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glBegin(GL_LINES);
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
glEnd();
}
}
}
//左上腕(LEFT_ELBOW - LEFT_SHOULDER)に画像をコラージュ
void drawLeftArm(xn::DepthGenerator& depth, xn::SkeletonCapability& capability, XnUserID user, XnMapOutputMode mapMode, IplImage* preLoadImage, IplImage **rgbImage) {
XnSkeletonJointPosition pos1,pos2;
IplImage *partImage = NULL;
IplImage *fallImage = NULL;
// elbowのジョイント座標の取得
capability.GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, pos1);
XnPoint3D pReal1[1] = {pos1.position};
XnPoint3D pProjective1[1];
// 世界座標系から表示座標系に変換した座標を取得
depth.ConvertRealWorldToProjective(1, pReal1, pProjective1);
// sholderのジョイント座標の取得
capability.GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, pos2);
XnPoint3D pReal2[1] = {pos2.position};
XnPoint3D pProjective2[1];
// 世界座標系から表示座標系に変換した座標を取得
depth.ConvertRealWorldToProjective(1, pReal2, pProjective2);
// 重ね合わせよう画像
partImage = preLoadImage;
fallImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);
CvPoint2D32f original[3];
CvPoint2D32f transform[3];
original[0] = cvPoint2D32f(0, 0);
original[1] = cvPoint2D32f(mapMode.nXRes, 0);
original[2] = cvPoint2D32f( 0, mapMode.nYRes);
CvSize sizeOfPart = cvGetSize(partImage);
//ELBOWの位置
int transX1 = pProjective1[0].X;
int transY1 = pProjective1[0].Y;
//SHOULDERの位置
int transX2 = pProjective2[0].X;
int transY2 = pProjective2[0].Y;
//中間の座標
int transX3 = (transX1 + transX2) / 2;
int transY3 = (transY1 + transY2) / 2;
//画像の始点
int transX = transX3 - (sizeOfPart.width / 2);
int transY = transY3 - (sizeOfPart.height / 2);
//角度
float ang = cvFastArctan(transY2 - transY1, transX2 - transX1); //+ cvFastArctan(transY1, transX1);
transform[0] = cvPoint2D32f( transX, transY);
transform[1] = cvPoint2D32f( transX + mapMode.nXRes, transY);
transform[2] = cvPoint2D32f( transX, transY + mapMode.nYRes);
// 行列作成
CvMat *affineMatrix = cvCreateMat(2, 3, CV_32FC1);
cvGetAffineTransform(original, transform, affineMatrix);
// 移動
cvWarpAffine(partImage, fallImage, affineMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
//回転行列作成
CvPoint2D32f center = cvPoint2D32f(transX3, transY3);
IplImage *fallImage2 = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);
CvMat *rotationMatrix = cvCreateMat(2, 3, CV_32FC1);
cv2DRotationMatrix(center, 90.0 - ang, 1.0, rotationMatrix);
//回転
cvWarpAffine(fallImage, fallImage2, rotationMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
// 画像の重ね合わせ
fallPartImage(fallImage2, *rgbImage);
// 解放
cvReleaseImage(&fallImage);
cvReleaseImage(&fallImage2);
}
cv::Point xncv::worldToProjective(const XnPoint3D& point, const xn::DepthGenerator& depth)
{
XnPoint3D p;
depth.ConvertRealWorldToProjective(1, &point, &p);
return cv::Point(static_cast<int>(p.X), static_cast<int>(p.Y));
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
XnUInt64 *MXadress;
double *Pos;
int Jdimsc[2];
Jdimsc[0]=225; Jdimsc[1]=7;
plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL);
Pos = mxGetPr(plhs[0]);
if(nrhs==0)
{
printf("Open failed: Give Pointer to Kinect as input\n");
mexErrMsgTxt("Kinect Error");
}
MXadress = (XnUInt64*)mxGetData(prhs[0]);
if(MXadress[0]>0){ g_Context = ((xn::Context*) MXadress[0])[0]; }
if(MXadress[2]>0)
{
g_DepthGenerator = ((xn::DepthGenerator*) MXadress[2])[0];
}
else
{
mexErrMsgTxt("No Depth Node in Kinect Context");
}
if(MXadress[4]>0)
{
g_UserGenerator = ((xn::UserGenerator*) MXadress[4])[0];
}
else
{
mexErrMsgTxt("No User Node in Kinect Context");
}
XnStatus nRetVal = XN_STATUS_OK;
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return;
}
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
return;
}
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
int r=0;
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
// Process the data
g_DepthGenerator.GetMetaData(depthMD);
g_UserGenerator.GetUserPixels(0, sceneMD);
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
//printf(strLabel, "%d - Looking for pose", aUsers[i]);
XnSkeletonJointPosition joint[15];
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_HEAD, joint[0]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_NECK, joint[1]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_SHOULDER, joint[2]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_ELBOW, joint[3]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HAND, joint[4]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_SHOULDER, joint[5]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_ELBOW, joint[6]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HAND, joint[7]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_TORSO, joint[8]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HIP, joint[9]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_KNEE, joint[10]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_FOOT, joint[11]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HIP, joint[12]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_KNEE, joint[13]);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_FOOT, joint[14]);
XnPoint3D pt[1];
for(int j=0; j<15; j++)
{
Pos[j +r]=aUsers[i];
Pos[j+Jdimsc[0] +r]=joint[j].fConfidence;
Pos[j+Jdimsc[0]*2+r]=joint[j].position.X;
Pos[j+Jdimsc[0]*3+r]=joint[j].position.Y;
Pos[j+Jdimsc[0]*4+r]=joint[j].position.Z;
pt[0] = joint[j].position;
g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);
Pos[j+Jdimsc[0]*5+r]=pt[0].X;
Pos[j+Jdimsc[0]*6+r]=pt[0].Y;
}
r+=15;
}
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
else
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[20] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
if (aUsers[i] == player)
sprintf(strLabel, "%d (Player)", aUsers[i]);
else
sprintf(strLabel, "%d", aUsers[i]);
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
// Draw skeleton of user
if (player != 0)
{
glBegin(GL_LINES);
glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
// gesture
static int gesture = 0;
static XnPoint3D previousLeftHandPt;
XnPoint3D newLeftHandPt;
newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND);
if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640)
if(previousLeftHandPt.X - newLeftHandPt.X > 60)
gesture = 1;
else if(previousLeftHandPt.X - newLeftHandPt.X < -60)
gesture = 2;
else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60)
gesture = 3;
else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60)
gesture = 4;
else
gesture = 0;
if(gesture != 0)
printf("gesture: %d\n", gesture);
previousLeftHandPt = newLeftHandPt;
// head
XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD);
// save
saveLocation(pt, newLeftHandPt, gesture);
DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
glEnd();
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
/*
if not initialized
set parameters and reserve memory space
*/
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// get the depth resolution
static unsigned int nZRes = dmd.ZRes();
static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));
// Calculate the accumulative histogram
memset(pDepthHist, 0, nZRes*sizeof(float));
// count the number of pixels of every possible depth value
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] += pDepthHist[nIndex-1];
}
// calculate percentage for every depth value
// the larger the value is, the darker the pixel should be
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
#ifndef USE_GLES
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
XnUInt32 nDummy = 0;
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
#endif
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
// Draw Joints
if (g_bMarkJoints)
{
// Try to draw all joints
DrawJoint(aUsers[i], XN_SKEL_HEAD);
DrawJoint(aUsers[i], XN_SKEL_NECK);
DrawJoint(aUsers[i], XN_SKEL_TORSO);
DrawJoint(aUsers[i], XN_SKEL_WAIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT);
}
#ifndef USE_GLES
glBegin(GL_LINES);
#endif
// Draw Limbs
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_WRIST, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HAND, XN_SKEL_LEFT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
#ifndef USE_GLES
glEnd();
#endif
}
}
if (g_bPrintFrameID)
{
static XnChar strFrameID[80];
xnOSMemSet(strFrameID, 0, 80);
XnUInt32 nDummy = 0;
xnOSStrFormat(strFrameID, sizeof(strFrameID), &nDummy, "%d", dmd.FrameID());
glColor4f(1, 0, 0, 1);
glRasterPos2i(10, 10);
glPrintString(GLUT_BITMAP_HELVETICA_18, strFrameID);
}
}
/**********************************************************************************
Following the point created method, any update in the hand point coordinates are
reflected through this event handler
**********************************************************************************/
void XN_CALLBACK_TYPE pointUpdate(const XnVHandPointContext *pContext, void *cxt){
XnPoint3D coords(pContext->ptPosition);
depthGen.ConvertRealWorldToProjective(1,&coords,&handPointCoords);
}
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint)
{
if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
{
printf("not tracked!\n");
return;
}
if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
{
return;
}
XnSkeletonJointPosition joint;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);
if (joint.fConfidence < 0.5)
{
return;
}
XnPoint3D pt;
pt = joint.position;
g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
drawCircle(pt.X, pt.Y, 2);
//////////////////////////////////code/////////////////////////////////////////
if(eJoint==XN_SKEL_LEFT_HAND && tmp==0 && player==1){//左手
//printf("ske,%d,0,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
//fflush(stdout);
za_tmpX[zaC]=pt.X;
za_tmpY[zaC]=pt.Y;
za_tmpZ[zaC]=pt.Z;
H_L_X=pt.X;
H_L_Y=pt.Y;
H_L_Z=pt.Z;
tmp=1;
zaC++;
}else if(eJoint==XN_SKEL_RIGHT_HAND && tmp==1 && player==1){//右手
//printf("ske,%d,1,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
za_tmpX[zaC]=pt.X;
za_tmpY[zaC]=pt.Y;
za_tmpZ[zaC]=pt.Z;
H_R_X=pt.X;
H_R_Y=pt.Y;
H_R_Z=pt.Z;
tmp=0;
zaC++;
LRh=1;
//printf("zaC=%d\n",zaC);
}
//左zaC_MIN<-0,2,4,6,8,10,12,14=zaC_MAX-2
//右zaC_MIN+1<-1,3...15=zaC_MAX-1
//za_tmpなんとか
//ズーム判定
//else if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
/*
if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
X=X-(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
}
if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
Y=Y-(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
}
*/
if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
X=X+(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
}
if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
Y=Y+(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
}
/*
if(X>=10000) X=10000;
if(X<=(-10000)) X=(-10000);
if(Y>=10000) X=10000;
if(Y<=(-10000)) Y=(-10000);
*/
printf("ske,%d,1,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],X,Y);
//printf("X=%f,Y=%f\n",X,Y);
fflush(stdout);
}else if(H_L_Z<kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//両手判定
/*
float scaleX_tmp=0;
float scaleY_tmp=0;
scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1]));
scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1]));
if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.1);
else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.1);
if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.1);
else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.1);
if(scaleX<=0.01) scaleX=0.01;
if(scaleX>=100) scaleX=100;
if(scaleY<=0.01) scaleY=0.01;
if(scaleY>=100) scaleY=100;
*/
float scaleX_tmp=0;
float scaleY_tmp=0;
float scaleX_tmp2=0;
float scaleY_tmp2=0;
scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1]));
scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1]));
//scaleX_tmp=SG( scaleX_tmp2*10.0 )/10.0;
//scaleY_tmp=SG( scaleY_tmp2*10.0 )/10.0;
if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.01);
else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.01);
if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.01);
else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.01);
if(scaleX<=0.01) scaleX=0.01;
if(scaleX>=100) scaleX=80;
if(scaleY<=0.01) scaleY=0.01;
if(scaleY>=100) scaleY=80;
//printf("scaleX=%f\nscaleY=%f\n",scaleX,scaleY);//テスト
printf("ske,%d,2,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],scaleX,scaleY);
fflush(stdout);
}else{
if(LRh==1 && player==1){
printf("ske,%d,0,%f,%f,%f,%f,%f,%f,\n",player,H_L_X,H_L_Y,H_L_Z,H_R_X,H_R_Y,H_R_Z);
//printf("Z=%f\n",H_L_Z);
fflush(stdout);
LRh=0;
}
}
if(zaC==zaC_MAX)zaC=0;
//if(LRh==1)LRh=0;
fflush(stdout);
//////////////////////////////////code/////////////////////////////////////////
}