C++ DepthGenerator::ConvertRealWorldToProjective方法代码示例

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;
}