freenect: glview.c Source File

00001 /*
00002  * This file is part of the OpenKinect Project. http://www.openkinect.org
00003  *
00004  * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
00005  * for details.
00006  *
00007  * This code is licensed to you under the terms of the Apache License, version
00008  * 2.0, or, at your option, the terms of the GNU General Public License,
00009  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
00010  * or the following URLs:
00011  * http://www.apache.org/licenses/LICENSE-2.0
00012  * http://www.gnu.org/licenses/gpl-2.0.txt
00013  *
00014  * If you redistribute this file in source form, modified or unmodified, you
00015  * may:
00016  *   1) Leave this header intact and distribute it under the same terms,
00017  *      accompanying it with the APACHE20 and GPL20 files, or
00018  *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
00019  *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
00020  * In all cases you must keep the copyright notice intact and include a copy
00021  * of the CONTRIB file.
00022  *
00023  * Binary distributions must follow the binary distribution requirements of
00024  * either License.
00025  */
00026 
00027 
00028 #include <stdio.h>
00029 #include <string.h>
00030 #include "libfreenect.h"
00031 
00032 #include <pthread.h>
00033 
00034 #if defined(__APPLE__)
00035 #include <GLUT/glut.h>
00036 #include <OpenGL/gl.h>
00037 #include <OpenGL/glu.h>
00038 #else
00039 #include <GL/glut.h>
00040 #include <GL/gl.h>
00041 #include <GL/glu.h>
00042 #endif
00043 
00044 #include <math.h>
00045 
00046 pthread_t freenect_thread;
00047 volatile int die = 0;
00048 
00049 int g_argc;
00050 char **g_argv;
00051 
00052 int window;
00053 int ir_mode = 0;
00054 
00055 pthread_mutex_t gl_backbuf_mutex = PTHREAD_MUTEX_INITIALIZER;
00056 
00057 uint8_t gl_depth_front[640 * 480 * 4];
00058 uint8_t gl_depth_back[640 * 480 * 4];
00059 
00060 uint8_t gl_rgb_front[640 * 480 * 4];
00061 uint8_t gl_rgb_back[640 * 480 * 4];
00062 
00063 GLuint gl_depth_tex;
00064 GLuint gl_rgb_tex;
00065 
00066 freenect_context *f_ctx;
00067 freenect_device *f_dev;
00068 int freenect_angle = 0;
00069 int freenect_led;
00070 
00071 
00072 pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
00073 int got_frames = 0;
00074 
00075 void
00076 DrawGLScene ()
00077 {
00078   pthread_mutex_lock (&gl_backbuf_mutex);
00079 
00080   while (got_frames < 2)
00081   {
00082     pthread_cond_wait (&gl_frame_cond, &gl_backbuf_mutex);
00083   }
00084 
00085   memcpy (gl_depth_front, gl_depth_back, sizeof (gl_depth_back));
00086   memcpy (gl_rgb_front, gl_rgb_back, sizeof (gl_rgb_back));
00087   got_frames = 0;
00088   pthread_mutex_unlock (&gl_backbuf_mutex);
00089 
00090   glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00091   glLoadIdentity ();
00092 
00093   glEnable (GL_TEXTURE_2D);
00094 
00095   glBindTexture (GL_TEXTURE_2D, gl_depth_tex);
00096   glTexImage2D (GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, gl_depth_front);
00097 
00098   glBegin (GL_TRIANGLE_FAN);
00099   glColor4f (255.0f, 255.0f, 255.0f, 255.0f);
00100   glTexCoord2f (0, 0);
00101   glVertex3f (0, 0, 0);
00102   glTexCoord2f (1, 0);
00103   glVertex3f (640, 0, 0);
00104   glTexCoord2f (1, 1);
00105   glVertex3f (640, 480, 0);
00106   glTexCoord2f (0, 1);
00107   glVertex3f (0, 480, 0);
00108   glEnd ();
00109 
00110   glBindTexture (GL_TEXTURE_2D, gl_rgb_tex);
00111   glTexImage2D (GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, gl_rgb_front);
00112 
00113   glBegin (GL_TRIANGLE_FAN);
00114   glColor4f (255.0f, 255.0f, 255.0f, 255.0f);
00115   glTexCoord2f (0, 0);
00116   glVertex3f (640, 0, 0);
00117   glTexCoord2f (1, 0);
00118   glVertex3f (1280, 0, 0);
00119   glTexCoord2f (1, 1);
00120   glVertex3f (1280, 480, 0);
00121   glTexCoord2f (0, 1);
00122   glVertex3f (640, 480, 0);
00123   glEnd ();
00124 
00125   glutSwapBuffers ();
00126 }
00127 
00128 void
00129 keyPressed (unsigned char key, int x, int y)
00130 {
00131   if (key == 27)
00132   {
00133     die = 1;
00134     pthread_join (freenect_thread, NULL);
00135     glutDestroyWindow (window);
00136     pthread_exit (NULL);
00137   }
00138   if (key == 'w')
00139   {
00140     freenect_angle++;
00141     if (freenect_angle > 30)
00142     {
00143       freenect_angle = 30;
00144     }
00145   }
00146   if (key == 's')
00147   {
00148     freenect_angle = 0;
00149   }
00150   if (key == 'x')
00151   {
00152     freenect_angle--;
00153     if (freenect_angle < -30)
00154     {
00155       freenect_angle = -30;
00156     }
00157   }
00158   // Toggle the IR mode on/off
00159   if (key == 'i')
00160   {
00161     if (ir_mode)
00162     {
00163       freenect_set_rgb_format (f_dev, FREENECT_FORMAT_RGB);
00164       freenect_start_rgb (f_dev);
00165     }
00166     else
00167     {
00168       freenect_set_rgb_format (f_dev, FREENECT_FORMAT_IR);
00169       freenect_start_ir (f_dev);
00170     }
00171     ir_mode = !ir_mode;
00172   }
00173   if (key == '1')
00174   {
00175     freenect_set_led (f_dev, LED_GREEN);
00176   }
00177   if (key == '2')
00178   {
00179     freenect_set_led (f_dev, LED_RED);
00180   }
00181   if (key == '3')
00182   {
00183     freenect_set_led (f_dev, LED_YELLOW);
00184   }
00185   if (key == '4')
00186   {
00187     freenect_set_led (f_dev, LED_BLINK_YELLOW);
00188   }
00189   if (key == '5')
00190   {
00191     freenect_set_led (f_dev, LED_BLINK_GREEN);
00192   }
00193   if (key == '6')
00194   {
00195     freenect_set_led (f_dev, LED_BLINK_RED_YELLOW);
00196   }
00197   if (key == '0')
00198   {
00199     freenect_set_led (f_dev, LED_OFF);
00200   }
00201   freenect_set_tilt_degs (f_dev, freenect_angle);
00202 }
00203 
00204 void
00205 ReSizeGLScene (int Width, int Height)
00206 {
00207   glViewport (0, 0, Width, Height);
00208   glMatrixMode (GL_PROJECTION);
00209   glLoadIdentity ();
00210   glOrtho (0, 1280, 480, 0, -1.0f, 1.0f);
00211   glMatrixMode (GL_MODELVIEW);
00212 }
00213 
00214 void
00215 InitGL (int Width, int Height)
00216 {
00217   glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
00218   glClearDepth (1.0);
00219   glDepthFunc (GL_LESS);
00220   glDisable (GL_DEPTH_TEST);
00221   glEnable (GL_BLEND);
00222   glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00223   glShadeModel (GL_SMOOTH);
00224   glGenTextures (1, &gl_depth_tex);
00225   glBindTexture (GL_TEXTURE_2D, gl_depth_tex);
00226   glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00227   glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00228   glGenTextures (1, &gl_rgb_tex);
00229   glBindTexture (GL_TEXTURE_2D, gl_rgb_tex);
00230   glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00231   glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00232   ReSizeGLScene (Width, Height);
00233 }
00234 
00235 void *
00236 gl_threadfunc (void *arg)
00237 {
00238   printf ("GL thread\n");
00239 
00240   glutInit (&g_argc, g_argv);
00241 
00242   glutInitDisplayMode (GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
00243   glutInitWindowSize (1280, 480);
00244   glutInitWindowPosition (0, 0);
00245 
00246   window = glutCreateWindow ("Kinect Viewer Demo");
00247 
00248   glutDisplayFunc (&DrawGLScene);
00249   glutIdleFunc (&DrawGLScene);
00250   glutReshapeFunc (&ReSizeGLScene);
00251   glutKeyboardFunc (&keyPressed);
00252 
00253   InitGL (1280, 480);
00254 
00255   glutMainLoop ();
00256 
00257   return NULL;
00258 }
00259 
00260 uint16_t t_gamma[2048];
00261 
00262 void
00263 depth_cb (freenect_device * dev, void *v_depth, uint32_t timestamp)
00264 {
00265   int i;
00266   freenect_depth *depth = v_depth;
00267 
00268   pthread_mutex_lock (&gl_backbuf_mutex);
00269   for (i = 0; i < FREENECT_FRAME_PIX; i++)
00270   {
00271     int pval = t_gamma[depth[i]];
00272     int lb = pval & 0xff;
00273     switch (pval >> 8)
00274     {
00275     case 0:
00276       gl_depth_back[3 * i + 0] = 255;
00277       gl_depth_back[3 * i + 1] = 255 - lb;
00278       gl_depth_back[3 * i + 2] = 255 - lb;
00279       break;
00280     case 1:
00281       gl_depth_back[3 * i + 0] = 255;
00282       gl_depth_back[3 * i + 1] = lb;
00283       gl_depth_back[3 * i + 2] = 0;
00284       break;
00285     case 2:
00286       gl_depth_back[3 * i + 0] = 255 - lb;
00287       gl_depth_back[3 * i + 1] = 255;
00288       gl_depth_back[3 * i + 2] = 0;
00289       break;
00290     case 3:
00291       gl_depth_back[3 * i + 0] = 0;
00292       gl_depth_back[3 * i + 1] = 255;
00293       gl_depth_back[3 * i + 2] = lb;
00294       break;
00295     case 4:
00296       gl_depth_back[3 * i + 0] = 0;
00297       gl_depth_back[3 * i + 1] = 255 - lb;
00298       gl_depth_back[3 * i + 2] = 255;
00299       break;
00300     case 5:
00301       gl_depth_back[3 * i + 0] = 0;
00302       gl_depth_back[3 * i + 1] = 0;
00303       gl_depth_back[3 * i + 2] = 255 - lb;
00304       break;
00305     default:
00306       gl_depth_back[3 * i + 0] = 0;
00307       gl_depth_back[3 * i + 1] = 0;
00308       gl_depth_back[3 * i + 2] = 0;
00309       break;
00310     }
00311   }
00312   got_frames++;
00313   pthread_cond_signal (&gl_frame_cond);
00314   pthread_mutex_unlock (&gl_backbuf_mutex);
00315 }
00316 
00317 void
00318 rgb_cb (freenect_device * dev, freenect_pixel * rgb, uint32_t timestamp)
00319 {
00320   pthread_mutex_lock (&gl_backbuf_mutex);
00321   got_frames++;
00322   memcpy (gl_rgb_back, rgb, FREENECT_RGB_SIZE);
00323   pthread_cond_signal (&gl_frame_cond);
00324   pthread_mutex_unlock (&gl_backbuf_mutex);
00325 }
00326 
00327 void
00328 ir_cb (freenect_device * dev, freenect_pixel_ir * rgb, uint32_t timestamp)
00329 {
00330   pthread_mutex_lock (&gl_backbuf_mutex);
00331   got_frames++;
00332 
00333   int i;
00334   for (i = 0; i < FREENECT_FRAME_PIX; i++)
00335   {
00336     int pval = rgb[i];
00337     pval = pval >> 2;
00338     int lb = pval & 0xff;
00339 
00340     gl_rgb_back[3 * i + 0] = lb;
00341     gl_rgb_back[3 * i + 1] = lb;
00342     gl_rgb_back[3 * i + 2] = lb;
00343 
00344   }
00345   pthread_cond_signal (&gl_frame_cond);
00346   pthread_mutex_unlock (&gl_backbuf_mutex);
00347 }
00348 
00349 void *
00350 freenect_threadfunc (void *arg)
00351 {
00352   freenect_set_tilt_degs (f_dev, freenect_angle);
00353   freenect_set_led (f_dev, LED_RED);
00354   freenect_set_depth_callback (f_dev, depth_cb);
00355   freenect_set_rgb_callback (f_dev, rgb_cb);
00356   freenect_set_ir_callback (f_dev, ir_cb);
00357   freenect_set_rgb_format (f_dev, FREENECT_FORMAT_RGB);
00358   freenect_set_depth_format (f_dev, FREENECT_FORMAT_11_BIT);
00359 
00360   freenect_start_depth (f_dev);
00361   freenect_start_rgb (f_dev);
00362 
00363   printf ("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
00364 
00365   while (!die && freenect_process_events (f_ctx) >= 0)
00366   {
00367     freenect_raw_device_state *state;
00368     freenect_update_device_state (f_dev);
00369     state = freenect_get_device_state (f_dev);
00370     double dx, dy, dz;
00371     freenect_get_mks_accel (state, &dx, &dy, &dz);
00372     printf ("\r raw acceleration: %4d %4d %4d  mks acceleration: %4f %4f %4f", state->accelerometer_x,
00373             state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
00374     fflush (stdout);
00375   }
00376 
00377   printf ("\nshutting down streams...\n");
00378 
00379   freenect_stop_depth (f_dev);
00380   freenect_stop_rgb (f_dev);
00381   freenect_stop_ir (f_dev);
00382 
00383   printf ("-- done!\n");
00384   return NULL;
00385 }
00386 
00387 int
00388 main (int argc, char **argv)
00389 {
00390   int res;
00391 
00392   printf ("Kinect camera test\n");
00393 
00394   int i;
00395   for (i = 0; i < 2048; i++)
00396   {
00397     float v = i / 2048.0;
00398     v = powf (v, 3) * 6;
00399     t_gamma[i] = v * 6 * 256;
00400   }
00401 
00402   g_argc = argc;
00403   g_argv = argv;
00404 
00405   if (freenect_init (&f_ctx, NULL) < 0)
00406   {
00407     printf ("freenect_init() failed\n");
00408     return 1;
00409   }
00410 
00411   freenect_set_log_level (f_ctx, FREENECT_LOG_DEBUG);
00412 
00413   int nr_devices = freenect_num_devices (f_ctx);
00414   printf ("Number of devices found: %d\n", nr_devices);
00415 
00416   int user_device_number = 0;
00417   if (argc > 1)
00418     user_device_number = atoi (argv[1]);
00419 
00420   if (nr_devices < 1)
00421     return 1;
00422 
00423   if (freenect_open_device (f_ctx, &f_dev, user_device_number) < 0)
00424   {
00425     printf ("Could not open device\n");
00426     return 1;
00427   }
00428 
00429   res = pthread_create (&freenect_thread, NULL, freenect_threadfunc, NULL);
00430   if (res)
00431   {
00432     printf ("pthread_create failed\n");
00433     return 1;
00434   }
00435 
00436   // OS X requires GLUT to run on the main thread
00437   gl_threadfunc (NULL);
00438 
00439   return 0;
00440 }