void Loop(void)
{
XnStatus nRetVal = XN_STATUS_OK;
while (g_notDone)
{
if ((nRetVal = g_context.WaitOneUpdateAll(g_depth)) != XN_STATUS_OK)
//if ((nRetVal = g_context.WaitAndUpdateAll()) != XN_STATUS_OK)
{
fprintf(stderr,"Could not update data: %s\n", xnGetStatusString(nRetVal));
continue;
}
if (g_haveDepth)
{
const XnDepthPixel* pDepthMap = g_depth.GetDepthMap();
ProcessDepthFrame(pDepthMap, g_depthWidth, g_depthHeight);
FindFingertip();
}
if (g_haveImage)
{
const XnRGB24Pixel* pImageMap = g_image.GetRGB24ImageMap();
ProcessImageFrame(pImageMap, g_depthWidth, g_depthHeight);
}
ShowFrame();
CheckKeys();
}
}
void Loop(int sock)
{
XnStatus nRetVal = XN_STATUS_OK;
struct timespec last,now;
double nowtime, starttime;
clock_gettime(CLOCK_REALTIME, &last);
double lasttime = (double)(last.tv_sec) + ((double)(last.tv_nsec))/1000000000;
int frames=0;
while (g_notDone)
{
if ((nRetVal = g_context.WaitAndUpdateAll()) != XN_STATUS_OK)
{
fprintf(stderr,"Could not update ir: %s\n", xnGetStatusString(nRetVal));
continue;
}
const XnDepthPixel* pDepthMap = g_depth.GetDepthMap();
const XnRGB24Pixel* pImage = NULL;//g_image.GetRGB24ImageMap();
const XnIRPixel* pIR = NULL;//g_ir.GetIRMap();
ProcessDepthFrame(pDepthMap, g_depthWidth, g_depthHeight);
FindFingertip();
frames++;
clock_gettime(CLOCK_REALTIME, &now);
nowtime = (double)(now.tv_sec) + ((double)(now.tv_nsec))/1000000000;
if (g_stabalize) // If we are still stablizing then don't do anything
{
if ((nowtime - starttime) >= STABILIZATION_TIME_SECS)
{
g_stabalize = FALSE;
g_set_background = TRUE;
}
}
else if (g_calibrate) // Do we need to calibrate?
Calibrate(sock);
else
SendFingers(sock); // otherwise just send the touches
/*
if (nowtime - lasttime >= 1.0)
{
printf("%d FPS\n", (int)(frames/(nowtime-lasttime)));
lasttime = nowtime;
frames = 0;
if (sock >= 0 && pDepthMap != 0 )// && pImage != 0 )//&& pIR != 0)
SendImage(sock,pDepthMap, pImage, pIR, g_depthWidth, g_depthHeight);
}
*/
}
}
void glut_display() {
xn::DepthMetaData pDepthMapMD;
xn::ImageMetaData pImageMapMD;
#ifdef DEBUGOUT
ofstream datei;
#endif
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluPerspective(45, WINDOW_SIZE_X/WINDOW_SIZE_Y, 1000, 5000);
// glOrtho(0, WINDOW_SIZE_X, WINDOW_SIZE_Y, 0, -128, 128);
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
// glTranslatef(-12.8/640.0, 9.0/480.0, 0);
// glTranslatef(-12.8/630.0, 9.0/480.0,0);
glScalef(scalex, scaley, 1.0);
glTranslatef(transx/630, transy/480, 0.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
rot_angle+=0.7;
// Warten auf neue Daten vom Tiefengenerator
nRetVal = context.WaitAndUpdateAll();
checkError("Fehler beim Aktualisieren der Daten", nRetVal);
// Aktuelle Depth Metadaten auslesen
depth.GetMetaData(pDepthMapMD);
// Aktuelle Depthmap auslesen
const XnDepthPixel* pDepthMap = depth.GetDepthMap();
if(maxdepth==-1)
maxdepth = getMaxDepth(pDepthMap);
// Aktuelle Image Metadaten auslesen
image.GetMetaData(pImageMapMD);
//Aktuelles Bild auslesen
const XnRGB24Pixel* pImageMap = image.GetRGB24ImageMap();
glColor3f(1, 1, 1);
// XnDepthPixel maxdepth = depth.GetDeviceMaxDepth();
const unsigned int xres = pDepthMapMD.XRes();
const unsigned int yres = pDepthMapMD.YRes();
#ifdef DEBUGOUT
datei.open("daniel.txt", ios::out);
#endif
for(unsigned int y=0; y<yres-1; y++) {
for(unsigned int x=0; x<xres; x++) {
aDepthMap[x+y*xres] = static_cast<GLubyte>(static_cast<float>(pDepthMap[x+y*xres])/static_cast<float>(maxdepth)*255);
}
}
/*
glEnable(GL_TEXTURE_2D);
glPushMatrix();
glLoadIdentity();
glTranslatef(-800, 0, -2000);
glBindTexture(GL_TEXTURE_2D, texture_rgb);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap);
glBegin(GL_QUADS);
glTexCoord2f(0,1); glVertex3f(0,0,0);
glTexCoord2f(1,1); glVertex3f(640,0,0);
glTexCoord2f(1,0); glVertex3f(640,480,0);
glTexCoord2f(0,0); glVertex3f(0,480,0);
glEnd();
glPopMatrix();
glPushMatrix();
glLoadIdentity();
glTranslatef(-800, 600, -2000);
glBindTexture(GL_TEXTURE_2D, texture_depth);
glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE8, 640, 480, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, aDepthMap);
glBegin(GL_QUADS);
glTexCoord2f(0,1); glVertex3f(0,0,0);
glTexCoord2f(1,1); glVertex3f(640,0,0);
glTexCoord2f(1,0); glVertex3f(640,480,0);
glTexCoord2f(0,0); glVertex3f(0,480,0);
glEnd();
glPopMatrix();*/
glPushMatrix();
glLoadIdentity();
glTranslatef(-100, -100, -2000);
glRotatef(cx,0,1,0);
glRotatef(cy,1,0,0);
glTranslatef(-320, -240, 1000);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, texture_rgb);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, pImageMap);
glBegin(GL_POINTS);
for(unsigned int y=0; y<yres-1; y++) {
for(unsigned int x=0; x<630; x++) {
if(pDepthMap[x+y*xres]!=0) {
glTexCoord2f(static_cast<float>(x)/static_cast<float>(630), static_cast<float>(y)/static_cast<float>(480));
glVertex3f(x, (yres-y), -pDepthMap[x+y*xres]/2.00);
#ifdef DEBUGOUT
datei << t_gamma[pDepthMap[x+y*xres]] << endl;
#endif
}
}
}
glEnd();
glPopMatrix();
glDisable(GL_TEXTURE_2D);
glutSwapBuffers();
#ifdef DEBUGOUT
datei.close();
exit(-1);
#endif
}
int main(int argc, char* argv[]) {
glue.Init(argc, argv, 640, 240, "TrackHand");
xn::Context context;
XnStatus status = context.Init();
bmg::OnError(status, []{
std::cout << "Couldn't init OpenNi!" << std::endl;
exit(1);
});
xn::ImageGenerator image_generator;
status = image_generator.Create(context);
bmg::OnError(status, []{
std::cout << "Couldn't create image generator!" << std::endl;
});
status = depth_generator.Create(context);
bmg::OnError(status, []{
std::cout << "Couldn't create depth generator!" << std::endl;
});
xn::ImageMetaData image_metadata;
xn::DepthMetaData depth_metadata;
// Create gesture & hands generators
status = gesture_generator.Create(context);
bmg::OnError(status, []{
std::cout << "Couldn't create gesture generator!" << std::endl;
});
status = hands_generator.Create(context);
bmg::OnError(status, []{
std::cout << "Couldn't create hands generator!" << std::endl;
});
// Register to callbacks
XnCallbackHandle h1, h2;
gesture_generator
.RegisterGestureCallbacks(Gesture_Recognized, Gesture_Process, NULL, h1);
hands_generator
.RegisterHandCallbacks(Hand_Create, Hand_Update, Hand_Destroy, NULL, h2);
status = context.StartGeneratingAll();
bmg::OnError(status, []{
std::cout << "Couldn't generate all data!" << std::endl;
});
status = gesture_generator.AddGesture(GESTURE, NULL);
bmg::OnError(status, []{
std::cout << "Couldn't add gesture!" << std::endl;
});
glue.BindDisplayFunc([&]{
glue.BeginDraw();
// here goes code for app main loop
XnStatus status = context.WaitAndUpdateAll();
bmg::OnError(status, []{
std::cout << "Couldn't update and wait for new data!" << std::endl;
});
image_generator.GetMetaData(image_metadata);
unsigned imageX = image_metadata.XRes();
unsigned imageY = image_metadata.YRes();
glue.DrawOnTexture(
(void*)image_metadata.RGB24Data(),
imageX, imageY,
imageX, imageY,
320, 0, 640, 240);
depth_generator.GetMetaData(depth_metadata);
unsigned depthX = depth_metadata.XRes();
unsigned depthY = depth_metadata.YRes();
XnRGB24Pixel* transformed_depth_map = new XnRGB24Pixel[depthX * depthY];
bmg::CalculateDepth(
depth_generator.GetDepthMap(), depthX, depthY, MAX_DEPTH, transformed_depth_map);
glue.DrawOnTexture(
(void*)transformed_depth_map,
depthX, depthY,
depthX, depthY,
0, 0,
320, 240);
delete [] transformed_depth_map;
if (hand_recognized) {
// Draw point over tracked hand
glue.DrawPointOverRegion(static_cast<unsigned>(projective_point.X), static_cast<unsigned>(projective_point.Y), 0, 0);
glue.DrawPointOverRegion(static_cast<unsigned>(projective_point.X), static_cast<unsigned>(projective_point.Y), 320, 0);
}
glue.EndDraw();
});
glue.BindKeyboardFunc([](unsigned char key, int x, int y){
switch(key) {
case 27:
exit(1);
}
});
glue.Run();
context.Release();
}
void start_kinect() {
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
UsersCount = 0;
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
//return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
//return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
//return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
CHECK_RC(nRetVal,"No depth");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
CHECK_RC(nRetVal,"No image");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
//return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
//return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
XnUserID aUsers[MAX_NUM_USERS];
XnUInt16 nUsers;
XnSkeletonJointTransformation anyjoint;
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
XnUInt32 epochTime = 0;
while (!xnOSWasKeyboardHit())
{
g_Context.WaitOneUpdateAll(g_UserGenerator);
// print the torso information for the first user already tracking
nUsers=MAX_NUM_USERS;
g_UserGenerator.GetUsers(aUsers, nUsers);
int numTracked=0;
int userToPrint=-1;
WriteLock w_lock(myLock);
pDepthMap = g_depth.GetDepthMap();
pPixelMap = g_image.GetRGB24ImageMap();
g_depth.GetMetaData(g_depthMD);
g_image.GetMetaData(g_imageMD);
pPixelPoint = g_imageMD.RGB24Data();
for(XnUInt16 i=0; i<nUsers; i++) {
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
continue;
{
/* Writing all new movements into structure*/
/* Head */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_HEAD,anyjoint);
Skeletons[i]["Head"]["X"] = anyjoint.position.position.X;
Skeletons[i]["Head"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["Head"]["Z"] = anyjoint.position.position.Z;
/* Neck */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,anyjoint);
Skeletons[i]["Neck"]["X"] = anyjoint.position.position.X;
Skeletons[i]["Neck"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["Neck"]["Z"] = anyjoint.position.position.Z;
/* Left Shoulder */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,anyjoint);
Skeletons[i]["LeftShoulder"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftShoulder"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftShoulder"]["Z"] = anyjoint.position.position.Z;
/* Right Shoulder */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,anyjoint);
Skeletons[i]["RightShoulder"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightShoulder"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightShoulder"]["Z"] = anyjoint.position.position.Z;
/* Torso */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,anyjoint);
Skeletons[i]["Torso"]["X"] = anyjoint.position.position.X;
Skeletons[i]["Torso"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["Torso"]["Z"] = anyjoint.position.position.Z;
/* Left Elbow */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_ELBOW,anyjoint);
Skeletons[i]["LeftElbow"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftElbow"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftElbow"]["Z"] = anyjoint.position.position.Z;
/* Right Elbow */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_ELBOW,anyjoint);
Skeletons[i]["RightElbow"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightElbow"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightElbow"]["Z"] = anyjoint.position.position.Z;
/* Left Hip */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HIP,anyjoint);
Skeletons[i]["LeftHip"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftHip"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftHip"]["Z"] = anyjoint.position.position.Z;
/* Right Hip */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HIP,anyjoint);
Skeletons[i]["RightHip"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightHip"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightHip"]["Z"] = anyjoint.position.position.Z;
/* Left Hand */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HAND,anyjoint);
Skeletons[i]["LeftHand"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftHand"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftHand"]["Z"] = anyjoint.position.position.Z;
/* Right Hand */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HAND,anyjoint);
Skeletons[i]["RightHand"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightHand"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightHand"]["Z"] = anyjoint.position.position.Z;
/* Left Knee */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_KNEE,anyjoint);
Skeletons[i]["LeftKnee"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftKnee"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftKnee"]["Z"] = anyjoint.position.position.Z;
/* Right Knee */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_KNEE,anyjoint);
Skeletons[i]["RightKnee"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightKnee"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightKnee"]["Z"] = anyjoint.position.position.Z;
/* Left Foot */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_FOOT,anyjoint);
Skeletons[i]["LeftFoot"]["X"] = anyjoint.position.position.X;
Skeletons[i]["LeftFoot"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["LeftFoot"]["Z"] = anyjoint.position.position.Z;
/* Right Foot */
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_FOOT,anyjoint);
Skeletons[i]["RightFoot"]["X"] = anyjoint.position.position.X;
Skeletons[i]["RightFoot"]["Y"] = anyjoint.position.position.Y;
Skeletons[i]["RightFoot"]["Z"] = anyjoint.position.position.Z;
/*printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
Skeletons[i]["Head"]["X"],
Skeletons[i]["Head"]["Y"],
Skeletons[i]["Head"]["Z"]);*/
}
}
}
g_scriptNode.Release();
g_depth.Release();
g_image.Release();
g_UserGenerator.Release();
g_Context.Release();
}
int main() {
const unsigned int nBackgroundTrain = 30;
const unsigned short touchDepthMin = 10;
const unsigned short touchDepthMax = 20;
const unsigned int touchMinArea = 50;
const bool localClientMode = true; // connect to a local client
const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization
const char* windowName = "Debug";
const Scalar debugColor0(0,0,128);
const Scalar debugColor1(255,0,0);
const Scalar debugColor2(255,255,255);
int xMin = 110;
int xMax = 560;
int yMin = 120;
int yMax = 320;
Mat1s depth(480, 640); // 16 bit depth (in millimeters)
Mat1b depth8(480, 640); // 8 bit depth
Mat3b rgb(480, 640); // 8 bit depth
Mat3b debug(480, 640); // debug visualization
Mat1s foreground(640, 480);
Mat1b foreground8(640, 480);
Mat1b touch(640, 480); // touch mask
Mat1s background(480, 640);
vector<Mat1s> buffer(nBackgroundTrain);
CHECK_RC(initOpenNI("niConfig.xml"), "initOpenNI");
// TUIO server object
TuioServer* tuio;
if (localClientMode) {
tuio = new TuioServer();
} else {
tuio = new TuioServer("192.168.0.1",3333,false);
}
TuioTime time;
// create some sliders
namedWindow(windowName);
createTrackbar("xMin", windowName, &xMin, 640);
createTrackbar("xMax", windowName, &xMax, 640);
createTrackbar("yMin", windowName, &yMin, 480);
createTrackbar("yMax", windowName, &yMax, 480);
// create background model (average depth)
for (unsigned int i=0; i<nBackgroundTrain; i++) {
CHECK_RC(xnContext.WaitAndUpdateAll(), "xnContext.WaitAndUpdateAll()");
depth.data = (uchar*) xnDepthGenerator.GetDepthMap();
buffer[i] = depth;
}
average(buffer, background);
while ( waitKey(1) != 27 ) {
// read available data
xnContext.WaitAndUpdateAll();
// update 16 bit depth matrix
depth.data = (uchar*) xnDepthGenerator.GetDepthMap();
//xnImgeGenertor.GetGrayscale8ImageMap()
// update rgb image
//rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here
//cvtColor(rgb, rgb, CV_RGB2BGR);
// extract foreground by simple subtraction of very basic background model
foreground = background - depth;
// find touch mask by thresholding (points that are close to background = touch points)
touch = (foreground > touchDepthMin) & (foreground < touchDepthMax);
// extract ROI
Rect roi(xMin, yMin, xMax - xMin, yMax - yMin);
Mat touchRoi = touch(roi);
// find touch points
vector< vector<Point2i> > contours;
vector<Point2f> touchPoints;
findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, Point2i(xMin, yMin));
for (unsigned int i=0; i<contours.size(); i++) {
Mat contourMat(contours[i]);
// find touch points by area thresholding
if ( contourArea(contourMat) > touchMinArea ) {
Scalar center = mean(contourMat);
Point2i touchPoint(center[0], center[1]);
touchPoints.push_back(touchPoint);
}
}
// send TUIO cursors
time = TuioTime::getSessionTime();
tuio->initFrame(time);
for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points
float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin);
float cursorY = 1 - (touchPoints[i].y - yMin)/(yMax - yMin);
TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX,cursorY);
// TODO improve tracking (don't move cursors away, that might be closer to another touch point)
if (cursor == NULL || cursor->getTuioTime() == time) {
tuio->addTuioCursor(cursorX,cursorY);
} else {
tuio->updateTuioCursor(cursor, cursorX, cursorY);
}
}
tuio->stopUntouchedMovingCursors();
tuio->removeUntouchedStoppedCursors();
tuio->commitFrame();
// draw debug frame
depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame
cvtColor(depth8, debug, CV_GRAY2BGR);
debug.setTo(debugColor0, touch); // touch mask
rectangle(debug, roi, debugColor1, 2); // surface boundaries
for (unsigned int i=0; i<touchPoints.size(); i++) { // touch points
circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED);
}
// render debug frame (with sliders)
imshow(windowName, debug);
//imshow("image", rgb);
}
return 0;
}
int main() {
const unsigned int nBackgroundTrain = 30;
const unsigned short touchDepthMin = 10;
const unsigned short touchDepthMax = 20;
const unsigned int touchMinArea = 50;
const bool localClientMode = true; // connect to a local client
const double debugFrameMaxDepth = 4000; // maximal distance (in millimeters) for 8 bit debug depth frame quantization
const char* windowName = "Debug";
const char* colorWindowName = "image";
const Scalar debugColor0(0, 0, 128);
const Scalar debugColor1(255, 0, 0);
const Scalar debugColor2(255, 255, 255);
const Scalar debugColor3(0, 255, 255);
const Scalar debugColor4(255, 0, 255);
int xMin = 50;
int xMax = 550;
int yMin = 50;
int yMax = 300;
Mat1s depth(480, 640); // 16 bit depth (in millimeters)
Mat1b depth8(480, 640); // 8 bit depth
Mat3b rgb(480, 640); // 8 bit depth
Mat3b debug(480, 640); // debug visualization
Mat1s foreground(640, 480);
Mat1b foreground8(640, 480);
Mat1b touch(640, 480); // touch mask
Mat1s background(480, 640);
vector<Mat1s> buffer(nBackgroundTrain);
IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3);
IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3);
initOpenNI("niConfig.xml");
// TUIO server object
TuioServer* tuio;
if (localClientMode) {
tuio = new TuioServer();
} else {
tuio = new TuioServer("192.168.0.2", 3333, false);
}
TuioTime time;
namedWindow(colorWindowName);
createTrackbar("xMin", colorWindowName, &xMin, 640);
createTrackbar("xMax", colorWindowName, &xMax, 640);
createTrackbar("yMin", colorWindowName, &yMin, 480);
createTrackbar("yMax", colorWindowName, &yMax, 480);
// create some sliders
namedWindow(windowName);
createTrackbar("xMin", windowName, &xMin, 640);
createTrackbar("xMax", windowName, &xMax, 640);
createTrackbar("yMin", windowName, &yMin, 480);
createTrackbar("yMax", windowName, &yMax, 480);
Keyboard * piano = new Keyboard();
(*piano).initKeyMap();
system("qjackctl &");
sleep(4);
JackByTheNotes * notesJack = new JackByTheNotes();
notesJack->connect();
sleep(2);
system("sudo jack_connect Piano:Rubinstein system:playback_1 &");
map<double, timeval> keys;
// create background model (average depth)
for (unsigned int i = 0; i < nBackgroundTrain; i++) {
xnContext.WaitAndUpdateAll();
depth.data = (uchar*) xnDepthGenerator.GetDepthMap();
buffer[i] = depth;
}
average(buffer, background);
while (waitKey(1) != 27) {
// read available data
xnContext.WaitAndUpdateAll();
// update 16 bit depth matrix
depth.data = (uchar*) xnDepthGenerator.GetDepthMap();
//xnImgeGenertor.GetGrayscale8ImageMap()
XnRGB24Pixel* xnRgb =
const_cast<XnRGB24Pixel*>(xnImgeGenertor.GetRGB24ImageMap());
// IplImage * image = cvCreateImage(cvSize(640, 480), 8, 3);
// IplImage * convertedImage = cvCreateImage(cvSize(640, 480), 8, 3);
cvSetData (image, xnRgb, 640 * 3);
cvConvertImage(image, convertedImage, CV_CVTIMG_SWAP_RB);
bool color = true;
rgb = convertedImage;
// cvtColor(rgb,rgb,CV_RGB2BGR);
// update rgb image
// rgb.data = (uchar*) xnImgeGenertor.GetRGB24ImageMap(); // segmentation fault here
// cvCvtColor(rgb, rgb, CV_BGR2RGB);
// extract foreground by simple subtraction of very basic background model
foreground = background - depth;
// find touch mask by thresholding (points that are close to background = touch points)
touch = (foreground > touchDepthMin) & (foreground < touchDepthMax);
// extract ROI
Rect roi(xMin, yMin, xMax - xMin, yMax - yMin);
Mat touchRoi = touch(roi);
// find touch points
vector<vector<Point2i> > contours;
vector<Point2f> touchPoints;
findContours(touchRoi, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE,
Point2i(xMin, yMin));
for (unsigned int i = 0; i < contours.size(); i++) {
Mat contourMat(contours[i]);
// find touch points by area thresholding
if (contourArea(contourMat) > touchMinArea) {
Scalar center = mean(contourMat);
Point2i touchPoint(center[0], center[1]);
touchPoints.push_back(touchPoint);
}
}
// send TUIO cursors
time = TuioTime::getSessionTime();
tuio->initFrame(time);
for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points
float cursorX = (touchPoints[i].x - xMin) / (xMax - xMin);
float cursorY = 1 - (touchPoints[i].y - yMin) / (yMax - yMin);
TuioCursor* cursor = tuio->getClosestTuioCursor(cursorX, cursorY);
// TODO improve tracking (don't move cursors away, that might be closer to another touch point)
if (cursor == NULL || cursor->getTuioTime() == time) {
tuio->addTuioCursor(cursorX, cursorY);
} else {
tuio->updateTuioCursor(cursor, cursorX, cursorY);
}
}
tuio->stopUntouchedMovingCursors();
tuio->removeUntouchedStoppedCursors();
tuio->commitFrame();
// draw debug frame
depth.convertTo(depth8, CV_8U, 255 / debugFrameMaxDepth); // render depth to debug frame
cvtColor(depth8, debug, CV_GRAY2BGR);
debug.setTo(debugColor0, touch); // touch mask
rectangle(debug, roi, debugColor1, 2); // surface boundaries
if (color)
rectangle(rgb, roi, debugColor1, 2); // surface boundaries
// draw 10 white keys within the roi
int stride = (xMax - xMin) / 10;
for (int keys = 1; keys < 10; keys++) {
Point lower(xMin + keys * stride, yMax);
if (keys == 3 || keys == 7) {
Point upper(xMin + keys * stride, yMin);
line(debug, upper, lower, debugColor3, 2, 0);
if (color)
line(rgb, upper, lower, debugColor3, 2, 0);
continue;
} else {
Point upper(xMin + keys * stride, (yMin + yMax) / 2);
line(debug, upper, lower, debugColor3, 2, 0);
if (color)
line(rgb, upper, lower, debugColor3, 2, 0);
}
Point blkUpper(xMin + keys * stride - stride / 3, yMin);
Point blkLower(xMin + keys * stride + stride / 3,
(yMin + yMax) / 2);
rectangle(debug, blkUpper, blkLower, debugColor4, 2);
if (color)
rectangle(rgb, blkUpper, blkLower, debugColor4, 2);
}
for (unsigned int i = 0; i < touchPoints.size(); i++) { // touch points
circle(debug, touchPoints[i], 5, debugColor2, CV_FILLED);
if (color)
circle(rgb, touchPoints[i], 5, debugColor2, CV_FILLED);
double frequency = piano->keyFrequency(touchPoints[i].y - 50,
touchPoints[i].x - 50);
cout << frequency << " " << touchPoints[i].y - 50 << " "
<< touchPoints[i].x - 50 << endl;
if (keys.find(frequency) == keys.end()) {
Note * note = new Note(frequency, 2, 4000);
notesJack->playNote(*note);
timeval now;
gettimeofday(&now, NULL);
keys[frequency] = now;
} else {
timeval now;
gettimeofday(&now, NULL);
if ((now.tv_sec - keys[frequency].tv_sec) * 1000
+ (now.tv_usec - keys[frequency].tv_usec) / 1000
> 1000) {
Note * note = new Note(frequency, 2, 4000);
notesJack->playNote(*note);
keys[frequency] = now;
}
}
}
// render debug frame (with sliders)
// IplImage grayScale = debug;
// cvFlip(&grayScale, NULL, 1);
// Mat gray(&grayScale);
// imshow(windowName, gray);
//
// IplImage colorful = rgb;
// cvFlip(&colorful, NULL, 1);
// Mat real(&colorful);
// imshow("image", real);
imshow(windowName, debug);
imshow("image", rgb);
// cvShowImage("image", image);
}
return 0;
}
ushort* getKinnectDepthMap() {
return (uchar*) xnDepthGenerator.GetDepthMap();
}
