regview.c
Go to the documentation of this file.
1 /*
2  * This file is part of the OpenKinect Project. http://www.openkinect.org
3  *
4  * Copyright (c) 2011 individual OpenKinect contributors. See the CONTRIB file
5  * for details.
6  *
7  * This code is licensed to you under the terms of the Apache License, version
8  * 2.0, or, at your option, the terms of the GNU General Public License,
9  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
10  * or the following URLs:
11  * http://www.apache.org/licenses/LICENSE-2.0
12  * http://www.gnu.org/licenses/gpl-2.0.txt
13  *
14  * If you redistribute this file in source form, modified or unmodified, you
15  * may:
16  * 1) Leave this header intact and distribute it under the same terms,
17  * accompanying it with the APACHE20 and GPL20 files, or
18  * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
19  * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
20  * In all cases you must keep the copyright notice intact and include a copy
21  * of the CONTRIB file.
22  *
23  * Binary distributions must follow the binary distribution requirements of
24  * either License.
25  */
26 
27 
28 #include <stdio.h>
29 #include <stdlib.h>
30 #include <string.h>
31 #include <assert.h>
32 #include "libfreenect.h"
33 
34 #include <pthread.h>
35 #include <math.h>
36 
37 #if defined(__APPLE__)
38 #include <GLUT/glut.h>
39 #else
40 #include <GL/glut.h>
41 #endif
42 
43 
44 pthread_t freenect_thread;
45 volatile int die = 0;
46 
47 int g_argc;
48 char **g_argv;
49 
50 int window;
51 
52 pthread_mutex_t gl_backbuf_mutex = PTHREAD_MUTEX_INITIALIZER;
53 
54 // back: owned by libfreenect (implicit for depth)
55 // mid: owned by callbacks, "latest frame ready"
56 // front: owned by GL, "currently being drawn"
59 
60 GLuint gl_depth_tex;
61 GLuint gl_rgb_tex;
62 
67 
68 pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
69 int got_rgb = 0;
70 int got_depth = 0;
71 
72 int frame = 0;
73 int my_ftime = 0;
74 double fps = 0;
75 
76 void idle()
77 {
78  pthread_mutex_lock(&gl_backbuf_mutex);
79 
80  // When using YUV_RGB mode, RGB frames only arrive at 15Hz, so we shouldn't force them to draw in lock-step.
81  // However, this is CPU/GPU intensive when we are receiving frames in lockstep.
82  while (!got_depth && !got_rgb) {
83  pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
84  }
85 
86  if (!got_depth || !got_rgb) {
87  pthread_mutex_unlock(&gl_backbuf_mutex);
88  return;
89  }
90  pthread_mutex_unlock(&gl_backbuf_mutex);
91  glutPostRedisplay();
92 }
93 
94 void DrawGLScene() {
95  uint8_t *tmp;
96 
97  pthread_mutex_lock(&gl_backbuf_mutex);
98  if (got_depth) {
99  tmp = depth_front;
101  depth_mid = tmp;
102  got_depth = 0;
103  }
104  if (got_rgb) {
105  tmp = rgb_front;
106  rgb_front = rgb_mid;
107  rgb_mid = tmp;
108  got_rgb = 0;
109  }
110  pthread_mutex_unlock(&gl_backbuf_mutex);
111 
112  glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
113  glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_front);
114 
115  glBegin(GL_TRIANGLE_FAN);
116  glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
117  glTexCoord2f(0, 0); glVertex3f(0,0,0);
118  glTexCoord2f(1, 0); glVertex3f(640,0,0);
119  glTexCoord2f(1, 1); glVertex3f(640,480,0);
120  glTexCoord2f(0, 1); glVertex3f(0,480,0);
121  glEnd();
122 
123  glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
124  glTexImage2D(GL_TEXTURE_2D, 0, 4, 640, 480, 0, GL_RGBA, GL_UNSIGNED_BYTE, depth_front);
125 
126  glBegin(GL_TRIANGLE_FAN);
127  glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
128  glTexCoord2f(0, 0); glVertex3f(0,0,0);
129  glTexCoord2f(1, 0); glVertex3f(640,0,0);
130  glTexCoord2f(1, 1); glVertex3f(640,480,0);
131  glTexCoord2f(0, 1); glVertex3f(0,480,0);
132  glEnd();
133 
134  glutSwapBuffers();
135 
136  frame++;
137  if (frame % 30 == 0) {
138  int ms = glutGet(GLUT_ELAPSED_TIME);
139  fps = 30.0/((ms - my_ftime)/1000.0);
140  my_ftime = ms;
141  }
142 }
143 
144 void keyPressed(unsigned char key, int x, int y)
145 {
146  if (key == 27 || key == 'q') {
147  die = 1;
148  pthread_join(freenect_thread, NULL);
149  pthread_cond_signal(&gl_frame_cond);
150  glutDestroyWindow(window);
151  free(depth_mid);
152  free(depth_front);
153  free(rgb_back);
154  free(rgb_mid);
155  free(rgb_front);
156  // Not pthread_exit because OSX leaves a thread lying around and doesn't exit
157  exit(0);
158  }
159 }
160 
161 void ReSizeGLScene(int Width, int Height)
162 {
163  glViewport(0,0,Width,Height);
164  glMatrixMode(GL_PROJECTION);
165  glLoadIdentity();
166  glOrtho (0, 640, 480, 0, -1.0f, 1.0f);
167  glMatrixMode(GL_MODELVIEW);
168  glLoadIdentity();
169 }
170 
171 void InitGL(int Width, int Height)
172 {
173  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
174  glClearDepth(1.0);
175  glDepthFunc(GL_LESS);
176  glDepthMask(GL_FALSE);
177  glDisable(GL_DEPTH_TEST);
178  glEnable(GL_BLEND);
179  glDisable(GL_ALPHA_TEST);
180  glEnable(GL_TEXTURE_2D);
181  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
182  glShadeModel(GL_FLAT);
183 
184  glGenTextures(1, &gl_depth_tex);
185  glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
186  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
187  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
188 
189  glGenTextures(1, &gl_rgb_tex);
190  glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
191  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
192  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
193 
194  ReSizeGLScene(Width, Height);
195 }
196 
197 void *gl_threadfunc(void *arg)
198 {
199  printf("GL thread\n");
200  glutInit(&g_argc, g_argv);
201 
202  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
203  glutInitWindowSize(640, 480);
204  glutInitWindowPosition(0, 0);
205 
206  window = glutCreateWindow("libfreenect Registration viewer");
207 
208  glutDisplayFunc(&DrawGLScene);
209  glutIdleFunc(&idle);
210  glutReshapeFunc(&ReSizeGLScene);
211  glutKeyboardFunc(&keyPressed);
212 
213  InitGL(640, 480);
214 
215  glutMainLoop();
216 
217  return NULL;
218 }
219 
221 
222 void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
223 {
224  int i;
225  uint16_t *depth = (uint16_t*)v_depth;
226 
227  pthread_mutex_lock(&gl_backbuf_mutex);
228  for (i=0; i<640*480; i++) {
229  //if (depth[i] >= 2048) continue;
230  int pval = t_gamma[depth[i]] / 4;
231  int lb = pval & 0xff;
232  depth_mid[4*i+3] = 128; // default alpha value
233  if (depth[i] == 0) depth_mid[4*i+3] = 0; // remove anything without depth value
234  switch (pval>>8) {
235  case 0:
236  depth_mid[4*i+0] = 255;
237  depth_mid[4*i+1] = 255-lb;
238  depth_mid[4*i+2] = 255-lb;
239  break;
240  case 1:
241  depth_mid[4*i+0] = 255;
242  depth_mid[4*i+1] = lb;
243  depth_mid[4*i+2] = 0;
244  break;
245  case 2:
246  depth_mid[4*i+0] = 255-lb;
247  depth_mid[4*i+1] = 255;
248  depth_mid[4*i+2] = 0;
249  break;
250  case 3:
251  depth_mid[4*i+0] = 0;
252  depth_mid[4*i+1] = 255;
253  depth_mid[4*i+2] = lb;
254  break;
255  case 4:
256  depth_mid[4*i+0] = 0;
257  depth_mid[4*i+1] = 255-lb;
258  depth_mid[4*i+2] = 255;
259  break;
260  case 5:
261  depth_mid[4*i+0] = 0;
262  depth_mid[4*i+1] = 0;
263  depth_mid[4*i+2] = 255-lb;
264  break;
265  default:
266  depth_mid[4*i+0] = 0;
267  depth_mid[4*i+1] = 0;
268  depth_mid[4*i+2] = 0;
269  depth_mid[4*i+3] = 0;
270  break;
271  }
272  }
273  got_depth++;
274  pthread_cond_signal(&gl_frame_cond);
275  pthread_mutex_unlock(&gl_backbuf_mutex);
276 }
277 
278 void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
279 {
280  pthread_mutex_lock(&gl_backbuf_mutex);
281 
282  // swap buffers
283  assert (rgb_back == rgb);
284  rgb_back = rgb_mid;
286  rgb_mid = (uint8_t*)rgb;
287 
288  got_rgb++;
289  pthread_cond_signal(&gl_frame_cond);
290  pthread_mutex_unlock(&gl_backbuf_mutex);
291 }
292 
293 void *freenect_threadfunc(void *arg)
294 {
300 
301  freenect_start_depth(f_dev);
302  freenect_start_video(f_dev);
303 
304  printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode, 'f'-video format\n");
305 
306  while (!die) {
307  int res = freenect_process_events(f_ctx);
308  if (res < 0 && res != -10) {
309  printf("\nError %d received from libusb - aborting.\n",res);
310  break;
311  }
312  }
313  printf("\nshutting down streams...\n");
314 
315  freenect_stop_depth(f_dev);
316  freenect_stop_video(f_dev);
317 
318  freenect_close_device(f_dev);
319  freenect_shutdown(f_ctx);
320 
321  printf("-- done!\n");
322  return NULL;
323 }
324 
325 int main(int argc, char **argv)
326 {
327  int res;
328 
329  depth_mid = (uint8_t*)malloc(640*480*4);
330  depth_front = (uint8_t*)malloc(640*480*4);
331  rgb_back = (uint8_t*)malloc(640*480*3);
332  rgb_mid = (uint8_t*)malloc(640*480*3);
333  rgb_front = (uint8_t*)malloc(640*480*3);
334 
335  printf("Kinect camera test\n");
336 
337  int i;
338  for (i=0; i<10000; i++) {
339  float v = i/2048.0;
340  v = powf(v, 3)* 6;
341  t_gamma[i] = v*6*256;
342  }
343 
344  g_argc = argc;
345  g_argv = argv;
346 
347  if (freenect_init(&f_ctx, NULL) < 0) {
348  printf("freenect_init() failed\n");
349  return 1;
350  }
351 
354 
355  int nr_devices = freenect_num_devices (f_ctx);
356  printf ("Number of devices found: %d\n", nr_devices);
357 
358  int user_device_number = 0;
359  if (argc > 1)
360  user_device_number = atoi(argv[1]);
361 
362  if (nr_devices < 1) {
363  freenect_shutdown(f_ctx);
364  return 1;
365  }
366 
367  if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
368  printf("Could not open device\n");
369  freenect_shutdown(f_ctx);
370  return 1;
371  }
372 
373  res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
374  if (res) {
375  printf("pthread_create failed\n");
376  freenect_shutdown(f_ctx);
377  return 1;
378  }
379 
380  // OS X requires GLUT to run on the main thread
381  gl_threadfunc(NULL);
382 
383  return 0;
384 }
void DrawGLScene()
Definition: regview.c:94
void freenect_set_video_callback(freenect_device *dev, freenect_video_cb cb)
Definition: fakenect.c:241
void freenect_select_subdevices(freenect_context *ctx, freenect_device_flags subdevs)
Definition: fakenect.c:338
int freenect_stop_video(freenect_device *dev)
Definition: fakenect.c:383
unsigned short uint16_t
pthread_cond_t gl_frame_cond
Definition: regview.c:68
int freenect_num_devices(freenect_context *ctx)
Definition: fakenect.c:308
freenect_device * f_dev
Definition: regview.c:64
int freenect_start_depth(freenect_device *dev)
Definition: fakenect.c:365
int main(int argc, char **argv)
Definition: regview.c:325
GLuint gl_rgb_tex
Definition: regview.c:61
int freenect_set_video_buffer(freenect_device *dev, void *buf)
Definition: fakenect.c:349
unsigned char uint8_t
void * gl_threadfunc(void *arg)
Definition: regview.c:197
uint8_t * depth_front
Definition: regview.c:57
int freenect_shutdown(freenect_context *ctx)
Definition: fakenect.c:402
int frame
Definition: regview.c:72
int g_argc
Definition: regview.c:47
uint8_t * rgb_back
Definition: regview.c:58
int got_rgb
Definition: regview.c:69
int freenect_set_video_mode(freenect_device *dev, const freenect_frame_mode mode)
Definition: fakenect.c:246
void freenect_set_depth_callback(freenect_device *dev, freenect_depth_cb cb)
Definition: fakenect.c:236
void freenect_set_log_level(freenect_context *ctx, freenect_loglevel level)
Definition: fakenect.c:401
int freenect_start_video(freenect_device *dev)
Definition: fakenect.c:371
int freenect_led
Definition: regview.c:66
freenect_frame_mode freenect_find_video_mode(freenect_resolution res, freenect_video_format fmt)
Definition: fakenect.c:260
void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
Definition: regview.c:222
freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_depth_format fmt)
Definition: fakenect.c:284
uint16_t t_gamma[10000]
Definition: regview.c:220
int window
Definition: regview.c:50
GLuint gl_depth_tex
Definition: regview.c:60
uint8_t * rgb_front
Definition: regview.c:58
void keyPressed(unsigned char key, int x, int y)
Definition: regview.c:144
double fps
Definition: regview.c:74
void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
Definition: regview.c:278
int freenect_close_device(freenect_device *dev)
Definition: fakenect.c:406
int freenect_set_depth_mode(freenect_device *dev, const freenect_frame_mode mode)
Definition: fakenect.c:253
int my_ftime
Definition: regview.c:73
int freenect_open_device(freenect_context *ctx, freenect_device **dev, int index)
Definition: fakenect.c:314
freenect_context * f_ctx
Definition: regview.c:63
int got_depth
Definition: regview.c:70
void * freenect_threadfunc(void *arg)
Definition: regview.c:293
unsigned int uint32_t
void InitGL(int Width, int Height)
Definition: regview.c:171
freenect_device_flags
Definition: libfreenect.h:58
volatile int die
Definition: regview.c:45
int freenect_stop_depth(freenect_device *dev)
Definition: fakenect.c:377
char ** g_argv
Definition: regview.c:48
void idle()
Definition: regview.c:76
uint8_t * depth_mid
Definition: regview.c:57
int freenect_process_events(freenect_context *ctx)
Definition: fakenect.c:140
pthread_mutex_t gl_backbuf_mutex
Definition: regview.c:52
uint8_t * rgb_mid
Definition: regview.c:58
int freenect_angle
Definition: regview.c:65
int freenect_init(freenect_context **ctx, freenect_usb_context *usb_ctx)
Definition: fakenect.c:327
void ReSizeGLScene(int Width, int Height)
Definition: regview.c:161
pthread_t freenect_thread
Definition: regview.c:44


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Mon Jun 10 2019 13:46:42