// this function is called each frame
void glutDisplay (void)
{
// Read next available data
g_Context.WaitAnyUpdateAll();
// Process the data
g_pSessionManager->Update(&g_Context);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
#ifdef USE_GLUT
glOrtho(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
#else
glOrthof(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
#endif
glDisable(GL_TEXTURE_2D);
// Draw the TrackPad
DrowTrackPad();
#ifdef USE_GLUT
glutSwapBuffers();
#endif
}
// this function is called each frame
void glutDisplay (void)
{
// Read next available data
g_Context.WaitAnyUpdateAll();
// Process the data
g_pSessionManager->Update(&g_Context);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
#ifdef USE_GLUT
//glOrtho(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
glOrtho(0, 1.0, 1.0, 0, -1.0, 1.0);
#else
//glOrthof(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
glOrthof(0, 1.0, 1.0, 0, -1.0, 1.0);
#endif
// draw
if (g_bDrawFrame)
{
DrawFrame(xnCreatePoint3D(0.0, 0.0, 0), xnCreatePoint3D(1.0, 1.0, 0), 8, g_fFrameR, g_fFrameG, g_fFrameB);
}
if (g_bDrawCircle)
{
XnV3DVector vec(0.5, 0.5, 0);
vec += XnV3DVector(sin(g_fCircleAngle), -cos(g_fCircleAngle), 0) * 0.3;
DrawCircle(xnCreatePoint3D(0.5, 0.5, 0), 0.3, 3, g_fCircleR, g_fCircleG, g_fCircleB);
DrawLine(xnCreatePoint3D(0.5, 0.5, 0), vec, 4, g_fCircleLineR, g_fCircleLineG, g_fCircleLineB);
}
// flip surfaces
#ifdef USE_GLUT
glutSwapBuffers();
#else
eglSwapBuffers(display, surface);
#endif
}
void CPNUIPlugin::OpenNIThread(boost::weak_ptr<CPNUIPluginAPI> JSAPI,
xn::Context& Context)
{
printf("Started OpenNI thread\n");
extern boost::weak_ptr<CPNUIPluginAPI> GJSAPI;
GJSAPI = JSAPI;
while (!JSAPI.expired())
{
XnStatus rc = Context.WaitAnyUpdateAll();
if (rc != XN_STATUS_OK)
{
printf("Read failed: %s\n", xnGetStatusString(rc));
//m_NUIAvailable = false;
return;
}
}
printf("JSAPI pointer expired!\n");
}
int main ( int argc, char ** argv )
{
//
// Initializing Calibration Related
//
// ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_FILE, ARTAG_POS_FILE );
ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_A3_FILE, ARTAG_POS_A3_FILE );
ExtrCalibrator extrCalibrator ( 6, KINECT_INTR_FILE, KINECT_DIST_FILE );
// unsigned char * kinectImgBuf = new unsigned char[colorImgWidth * colorImgHeight * 3];
//
// Initializing OpenNI Settings
//
int ctlWndKey = -1;
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
//
// Initialize Context Object
//
nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, g_ScriptNode, &errors );
if ( nRetVal == XN_STATUS_NO_NODE_PRESENT )
{
XnChar strError[1024];
errors.ToString ( strError, 1024 );
printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError );
system ( "pause" );
return ( nRetVal );
}
else if ( nRetVal != XN_STATUS_OK )
{
printf ( "Open FAILED:\n%s\n", xnGetStatusString ( nRetVal ) );
system ( "pause" );
return ( nRetVal );
}
//
// Handle the Depth Generator Node.
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Depth Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
//
// Handle the Image Generator node
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Image Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
// g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen );
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 );
assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT );
//
// Create OpenCV Showing Window and Related Data Structures
//
cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::Mat depthImgMat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 );
cv::Mat depthImgShow ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC3 );
cv::Mat colorImgMat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 );
#define ARTAG_DEBUG
#ifdef ARTAG_DEBUG
cv::setMouseCallback ( IMAGE_WIN_NAME, ClickOnMouse, 0 );
#endif
bool flipColor = true;
//
// Start to Loop
//
while ( ctlWndKey != ESC_KEY_VALUE )
{
//
// Try to Get New Frame From Kinect
//
nRetVal = g_Context.WaitAnyUpdateAll ();
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() && g_DepthMD.FullYRes() == g_DepthMD.YRes() );
assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() && g_ImageMD.FullYRes() == g_ImageMD.YRes() );
GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), colorImgMat );
#ifdef SHOW_DEPTH_WINDOW
GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), depthImgMat );
// GlobalUtility::ConvertDepthCvMat16uToYellowCvMat ( depthImgMat, depthImgShow );
GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( depthImgMat, depthImgShow );
cv::imshow ( DEPTH_WIN_NAME, depthImgShow );
#endif
ctlWndKey = cvWaitKey ( 15 );
if ( ctlWndKey == 'f' || ctlWndKey == 'F' )
{
artagHelper.Clear();
artagHelper.FindMarkerCorners ( (unsigned char *)(g_ImageMD.Data()) );
artagHelper.PrintMarkerCornersPos2dInCam ();
extrCalibrator.ExtrCalib ( artagHelper );
std::cout << "\nKinect Extr Matrix:" << std::endl;
extrCalibrator.PrintMatrix ( extrCalibrator.GetMatrix ( ExtrCalibrator::EXTR ) );
std::cout << "Reprojection ERROR = "
<< extrCalibrator.ComputeReprojectionErr ( artagHelper ) << std::endl
// << extrCalibrator.ComputeReprojectionErr ( artagHelper.m_MarkerCornerPosCam2d, artagHelper.m_MarkerCornerPos3d, 24 ) << std::endl
<< "Valid Marker Number = " << artagHelper.GetValidMarkerNumber() << std::endl
<< std::endl;
extrCalibrator.SaveMatrix ( ExtrCalibrator::EXTR, KINECT_EXTR_FILE );
}
if ( ctlWndKey == 's' || ctlWndKey == 'S' )
{
flipColor = !flipColor;
}
if ( flipColor ) {
cv::cvtColor ( colorImgMat, colorImgMat, CV_RGB2BGR );
}
artagHelper.DrawMarkersInCameraImage ( colorImgMat );
cv::imshow ( IMAGE_WIN_NAME, colorImgMat );
}
g_Context.Release ();
system ( "pause" );
exit ( EXIT_SUCCESS );
}
/***********************************************************************************
* This routine will draw the scene. This is the OpenGL display function / idle
function callback routine. There have been additions to this routine (from the depth
map sample).
a. Along with updating the context, we also update the session manager.
b. We draw the hand point as well. The hand point is represented using a white
square.
***********************************************************************************/
void draw(){
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //clear the gl buffers
rc = cxt.WaitAnyUpdateAll(); //first update the context - refresh the depth/image data coming from the sensor
if(rc != XN_STATUS_OK){ //if the update failed, i.e. couldn't be read
printf("\nERROR:Read failed... Quitting!\n"); //print error message
exit(0); //exit the program
}
else if(inSession){
sessionMgr->Update(&cxt); //NEW ADDITION - update the session manager
depthGen.GetMetaData(depthMapMetaData); //grab the depth map meta data
long xSize = depthMapMetaData.XRes(); //this meta data will give us the true width
long ySize = depthMapMetaData.YRes(); //and height of the scene
long totalSize = xSize * ySize; //total size of the depth map is gotten this way
const XnDepthPixel* depthMapData; //this array will contain the value of depth of each pixel in depth map
depthMapData = depthMapMetaData.Data(); //this will grab the depth values and store it into the array
int i, j,colorToSet; //color to set will be calculated for the pixel based on its depth
int depth; //to store the depth at a point
glLoadIdentity(); //set the matrix to an identity matrix
glOrtho(0, xSize, ySize, 0, -1, 1); //set the orthogonal view. -1 and 1 define the near and far clip off points along z axis
/************************************** DRAWS THE DEPTH MAP *************************************/
glBegin(GL_POINTS); //start the drawing of points
/*
Note: the depthMapData is a pointer to an array that is linear. It contains integer values of depth in millimeters. This linear array must be imagined
as a matrix of dimensions --> xSize,ySize
*/
for(i = 0;i<xSize;i++) //this loop will run through width of the depth map
{
for(j=0;j<ySize;j++) //this will run through the height
{
depth = depthMapMetaData(i,j); //get the depth info from the depthMapData. lvalue is (i*Xmax + j) in order to translate matrix coords to linear ones
colorToSet = MAX_COLOR - depth/COLOR_ZONES;
if((depth < DEPTH_THRESH_FAR) && (depth > DEPTH_THRESH_NEAR)) { //if the current pixel depth is within the threshold limits (acceptable range in millimeters)
glColor3ub(0,colorToSet,0); //then paint this pixel
glVertex3f(i,j,0);
}
}
}
glEnd(); //end drawing sequence
glBegin(GL_POINTS);
pixelmap[4][4].g=255;
for(i = 0;i<xSize;i++) //this loop will run through width of the depth map
{
for(j=0;j<ySize;j++) //this will run through the height
{
if (pixelmap[i][j].r > 0) {
glColor3ub(pixelmap[i][j].r,pixelmap[i][j].g, pixelmap[i][j].b); //then paint this pixel
glVertex3f(i,j,0);
}
}
}
glEnd();
/********************************* DRAWS THE HAND POINT ***************************************/
if(isHandPointNull() == false){
glColor3f(255,255,255); //set the color to white
glBegin(GL_POLYGON); //start drawing the polygon
int xCo = handPointCoords.X; //xCoord of the hand point
int yCo = handPointCoords.Y; //yCoord of the hand point
int zCo = handPointCoords.Z;
int size = 4; //size of this square
glVertex3f(xCo-size,yCo-size,0); //plot the top left corner
glVertex3f(xCo+size,yCo-size,0); //plot the top right corner
glVertex3f(xCo+size,yCo+size,0); //plot the bot right corner
glVertex3f(xCo-size,yCo+size,0); //plot the bot left corner
glEnd(); //stop drawing the polygon
//draws a bounding box around the hand using lines
int boxSize = 200;
int xBox = handPointCoords.X - boxSize/2;
int yBox = handPointCoords.Y - boxSize/2;
//pixelmap[-1][-1].g=233;
if (zCo < g_dist) {
glColor3f(255,0,0);
for (int i = -5; i <= 5; i++)
for (int j = -5; j <= 5; j++)
pixelmap[(xCo + i)%640][(yCo + j)%480].r = 255;
}
glBegin(GL_LINES);
glVertex3f(xBox,yBox,0); //top edge
glVertex3f(xBox+boxSize,yBox,0);
glVertex3f(xBox,yBox,0); //left edge
glVertex3f(xBox,yBox+boxSize,0);
glVertex3f(xBox+boxSize,yBox,0); //right edge
glVertex3f(xBox+boxSize,yBox+boxSize,0);
glVertex3f(xBox,yBox+boxSize,0); //bottom edge
glVertex3f(xBox+boxSize,yBox+boxSize,0);
glEnd();
}
/**********************************************************************************************/
}
glutSwapBuffers(); //for double buffering
}
void Context_WaitAnyUpdateAll_wrapped(xn::Context& self) {
check( self.WaitAnyUpdateAll() );
}
int main() {
printf("main() START\n");
signal(SIGINT, onSignalReceived); // hit CTRL-C keys in terminal (2)
signal(SIGTERM, onSignalReceived); // hit stop button in eclipse CDT (15)
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS = 30;
CHECK_RC(ctx.Init(), "init");
CHECK_RC(depthGenerator.Create(ctx), "create depth");
depthGenerator.SetMapOutputMode(mapMode);
XnStatus userAvailable = ctx.FindExistingNode(XN_NODE_TYPE_USER, userGenerator);
if(userAvailable != XN_STATUS_OK) {
CHECK_RC(userGenerator.Create(ctx), "create user");
}
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
xn::SkeletonCapability skel = userGenerator.GetSkeletonCap();
CHECK_RC(userGenerator.RegisterUserCallbacks(onUserNew, onUserLost, NULL, hUserCallbacks), "register user");
CHECK_RC(skel.RegisterCalibrationCallbacks(onCalibrationStart, onCalibrationEnd, NULL, hCalibrationCallbacks), "register calib");
CHECK_RC(userGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(onPoseDetected, NULL, NULL, hPoseCallbacks), "register pose");
XnChar poseName[20] = "";
CHECK_RC(skel.GetCalibrationPose(poseName), "get posename");
printf("poseName: %s\n", poseName);
CHECK_RC(skel.SetSkeletonProfile(XN_SKEL_PROFILE_ALL), "set skel profile");
CHECK_RC(skel.SetSmoothing(0.8), "set smoothing");
// xnSetMirror(depth, !mirrorMode);
CHECK_RC(ctx.StartGeneratingAll(), "start generating");
printf("waitAnyUpdateAll ...\n");
while(!shouldTerminate) {
ctx.WaitAnyUpdateAll();
// depthGenerator.GetMetaData(tempDepthMetaData);
const XnUInt16 userCount = userGenerator.GetNumberOfUsers();
// printf("userCount: %i\n", userCount);
XnUserID aUsers[userCount];
XnUInt16 nUsers = userCount;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
XnUserID currentUserId = aUsers[i];
if (skel.IsTracking(aUsers[i])) {
XnSkeletonJointPosition joint;
skel.GetSkeletonJointPosition(currentUserId, XN_SKEL_HEAD, joint);
XnFloat x = joint.position.X;
XnFloat y = joint.position.Y;
XnFloat z = joint.position.Z;
printf("joint position: %.2f x %.2f x %.2f\n", x, y, z);
printf("joint.fConfidence: %.2f\n", joint.fConfidence);
}
}
}
printf("STOP\n");
CHECK_RC(ctx.StopGeneratingAll(), "stop generating");
ctx.Shutdown();
printf("main() END\n");
}
