chunkview.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) 2013 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 
36 #if defined(__APPLE__)
37 #include <GLUT/glut.h>
38 #else
39 #include <GL/glut.h>
40 #endif
41 
42 #include <math.h>
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"
58 
59 GLuint gl_depth_tex;
60 
65 
66 
67 pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
68 int got_depth = 0;
69 
70 /*
71 Downsample and unpack pixels to 320*240 on the fly. Mind that even though
72 we setup libfreenect to return packed data, When we process them with the
73 chunk callback they will end up in unpacked form. Converting to mm is also
74 an option. The best way to do it is by passing a lookup table for raw->mm
75 as the argument to freenect_set_user. You can then access it here as the ud.
76 */
77 
78 void chunk_cb(void *buffer, void *pkt_data, int pkt_num, int pkt_size,void *ud)
79 {
80  if(pkt_num == 73 || pkt_num == 146) return;
81  uint8_t *raw = (uint8_t *) pkt_data;
82  uint16_t *frame=(uint16_t *)buffer;
83  if(pkt_num > 219){
84  raw += (pkt_num-220) * 12;
85  frame += 320 * (pkt_num-2);
86  }else if(pkt_num > 146){
87  raw += (pkt_num-147) * 12 + 4;
88  frame += 320 * (pkt_num-2);
89  }else if(pkt_num > 73){
90  raw += (pkt_num-74) * 12 + 8;
91  frame += 320 * (pkt_num-1);
92  }else{
93  raw += pkt_num * 12;
94  frame += 320 * pkt_num;
95  }
96 
97  int n = 0;
98  while(n != 40){
99  frame[0] = (raw[0]<<3) | (raw[1]>>5);
100  frame[1] = ((raw[2]<<9) | (raw[3]<<1) | (raw[4]>>7) ) & 2047;
101  frame[2] = ((raw[5]<<7) | (raw[6]>>1) ) & 2047;
102  frame[3] = ((raw[8]<<5) | (raw[9]>>3) ) & 2047;
103  frame[4] = (raw[11]<<3) | (raw[12]>>5);
104  frame[5] = ((raw[13]<<9) | (raw[14]<<1) | (raw[15]>>7) ) & 2047;
105  frame[6] = ((raw[16]<<7) | (raw[17]>>1) ) & 2047;
106  frame[7] = ((raw[19]<<5) | (raw[20]>>3) ) & 2047;
107  frame+=8;
108  raw+=22;
109  n++;
110  }
111 
112 }
113 
115 {
116  pthread_mutex_lock(&gl_backbuf_mutex);
117 
118  while (!got_depth) {
119  pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
120  }
121 
122 
123  uint8_t *tmp;
124 
125  if (got_depth) {
126  tmp = depth_front;
128  depth_mid = tmp;
129  got_depth = 0;
130  }
131 
132  pthread_mutex_unlock(&gl_backbuf_mutex);
133 
134  glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
135  glTexImage2D(GL_TEXTURE_2D, 0, 3, 320, 240, 0, GL_RGB, GL_UNSIGNED_BYTE, depth_front);
136 
137  glBegin(GL_TRIANGLE_FAN);
138  glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
139  glTexCoord2f(0, 0); glVertex3f(0,0,0);
140  glTexCoord2f(1, 0); glVertex3f(320,0,0);
141  glTexCoord2f(1, 1); glVertex3f(320,240,0);
142  glTexCoord2f(0, 1); glVertex3f(0,240,0);
143  glEnd();
144 
145  glutSwapBuffers();
146 }
147 
148 void keyPressed(unsigned char key, int x, int y)
149 {
150  if (key == 27) {
151  die = 1;
152  pthread_join(freenect_thread, NULL);
153  glutDestroyWindow(window);
154  free(depth_mid);
155  free(depth_front);
156  // Not pthread_exit because OSX leaves a thread lying around and doesn't exit
157  exit(0);
158  }
159  if (key == 'w') {
160  freenect_angle++;
161  if (freenect_angle > 30) {
162  freenect_angle = 30;
163  }
164  }
165  if (key == 's') {
166  freenect_angle = 0;
167  }
168  if (key == 'x') {
169  freenect_angle--;
170  if (freenect_angle < -30) {
171  freenect_angle = -30;
172  }
173  }
174  if (key == '1') {
176  }
177  if (key == '2') {
178  freenect_set_led(f_dev,LED_RED);
179  }
180  if (key == '3') {
182  }
183  if (key == '4') {
185  }
186  if (key == '5') {
187  // 5 is the same as 4
189  }
190  if (key == '6') {
192  }
193  if (key == '0') {
194  freenect_set_led(f_dev,LED_OFF);
195  }
197 }
198 
199 void ReSizeGLScene(int Width, int Height)
200 {
201  glViewport(0,0,Width,Height);
202  glMatrixMode(GL_PROJECTION);
203  glLoadIdentity();
204  glOrtho (0, 320, 240, 0, -1.0f, 1.0f);
205  glMatrixMode(GL_MODELVIEW);
206  glLoadIdentity();
207 }
208 
209 void InitGL(int Width, int Height)
210 {
211  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
212  glClearDepth(1.0);
213  glDepthFunc(GL_LESS);
214  glDepthMask(GL_FALSE);
215  glDisable(GL_DEPTH_TEST);
216  glDisable(GL_BLEND);
217  glDisable(GL_ALPHA_TEST);
218  glEnable(GL_TEXTURE_2D);
219  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
220  glShadeModel(GL_FLAT);
221 
222  glGenTextures(1, &gl_depth_tex);
223  glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
224  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
225  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
226 
227  ReSizeGLScene(Width, Height);
228 }
229 
230 void *gl_threadfunc(void *arg)
231 {
232  printf("GL thread\n");
233 
234  glutInit(&g_argc, g_argv);
235 
236  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
237  glutInitWindowSize(320, 240);
238  glutInitWindowPosition(0, 0);
239 
240  window = glutCreateWindow("LibFreenect");
241 
242  glutDisplayFunc(&DrawGLScene);
243  glutIdleFunc(&DrawGLScene);
244  glutReshapeFunc(&ReSizeGLScene);
245  glutKeyboardFunc(&keyPressed);
246 
247  InitGL(320, 240);
248 
249  glutMainLoop();
250 
251  return NULL;
252 }
253 
255 
256 
257 void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
258 {
259  int i;
260  uint16_t *depth = (uint16_t*)v_depth;
261  pthread_mutex_lock(&gl_backbuf_mutex);
262  for (i=0; i<320*240; i++) {
263  int pval = t_gamma[depth[i]];
264  int lb = pval & 0xff;
265  switch (pval>>8) {
266  case 0:
267  depth_mid[3*i+0] = 255;
268  depth_mid[3*i+1] = 255-lb;
269  depth_mid[3*i+2] = 255-lb;
270  break;
271  case 1:
272  depth_mid[3*i+0] = 255;
273  depth_mid[3*i+1] = lb;
274  depth_mid[3*i+2] = 0;
275  break;
276  case 2:
277  depth_mid[3*i+0] = 255-lb;
278  depth_mid[3*i+1] = 255;
279  depth_mid[3*i+2] = 0;
280  break;
281  case 3:
282  depth_mid[3*i+0] = 0;
283  depth_mid[3*i+1] = 255;
284  depth_mid[3*i+2] = lb;
285  break;
286  case 4:
287  depth_mid[3*i+0] = 0;
288  depth_mid[3*i+1] = 255-lb;
289  depth_mid[3*i+2] = 255;
290  break;
291  case 5:
292  depth_mid[3*i+0] = 0;
293  depth_mid[3*i+1] = 0;
294  depth_mid[3*i+2] = 255-lb;
295  break;
296  default:
297  depth_mid[3*i+0] = 0;
298  depth_mid[3*i+1] = 0;
299  depth_mid[3*i+2] = 0;
300  break;
301  }
302  }
303  got_depth++;
304  pthread_cond_signal(&gl_frame_cond);
305  pthread_mutex_unlock(&gl_backbuf_mutex);
306 }
307 
308 void *freenect_threadfunc(void *arg)
309 {
310  int accelCount = 0;
311 
313  freenect_set_led(f_dev,LED_RED);
317  freenect_start_depth(f_dev);
318 
319  printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
320 
321  while (!die && freenect_process_events(f_ctx) >= 0) {
322  //Throttle the text output
323  if (accelCount++ >= 2000)
324  {
325  accelCount = 0;
328  state = freenect_get_tilt_state(f_dev);
329  double dx,dy,dz;
330  freenect_get_mks_accel(state, &dx, &dy, &dz);
331  printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
332  fflush(stdout);
333  }
334 
335  }
336 
337  printf("\nshutting down streams...\n");
338 
339  freenect_stop_depth(f_dev);
340 
341  freenect_close_device(f_dev);
342  freenect_shutdown(f_ctx);
343 
344  printf("-- done!\n");
345  return NULL;
346 }
347 
348 int main(int argc, char **argv)
349 {
350  int res;
351 
352  depth_mid = (uint8_t*)malloc(320*240*3);
353  depth_front = (uint8_t*)malloc(320*240*3);
354 
355  printf("Kinect camera test\n");
356 
357  int i;
358  for (i=0; i<2048; i++) {
359  float v = i/2048.0;
360  v = powf(v, 3)* 6;
361  t_gamma[i] = v*6*256;
362  }
363 
364  g_argc = argc;
365  g_argv = argv;
366 
367  if (freenect_init(&f_ctx, NULL) < 0) {
368  printf("freenect_init() failed\n");
369  return 1;
370  }
371 
374 
375  int nr_devices = freenect_num_devices (f_ctx);
376  printf ("Number of devices found: %d\n", nr_devices);
377 
378  int user_device_number = 0;
379  if (argc > 1)
380  user_device_number = atoi(argv[1]);
381 
382  if (nr_devices < 1) {
383  freenect_shutdown(f_ctx);
384  return 1;
385  }
386 
387  if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
388  printf("Could not open device\n");
389  freenect_shutdown(f_ctx);
390  return 1;
391  }
392 
393  res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
394  if (res) {
395  printf("pthread_create failed\n");
396  freenect_shutdown(f_ctx);
397  return 1;
398  }
399 
400  // OS X requires GLUT to run on the main thread
401  gl_threadfunc(NULL);
402 
403  return 0;
404 }
int got_depth
Definition: chunkview.c:68
void * freenect_threadfunc(void *arg)
Definition: chunkview.c:308
void freenect_select_subdevices(freenect_context *ctx, freenect_device_flags subdevs)
Definition: fakenect.c:338
void freenect_get_mks_accel(freenect_raw_tilt_state *state, double *x, double *y, double *z)
Definition: fakenect.c:227
void chunk_cb(void *buffer, void *pkt_data, int pkt_num, int pkt_size, void *ud)
Definition: chunkview.c:78
pthread_t freenect_thread
Definition: chunkview.c:44
unsigned short uint16_t
uint8_t * depth_mid
Definition: chunkview.c:57
freenect_context * f_ctx
Definition: chunkview.c:61
pthread_mutex_t gl_backbuf_mutex
Definition: chunkview.c:52
volatile int die
Definition: chunkview.c:45
int freenect_num_devices(freenect_context *ctx)
Definition: fakenect.c:308
int freenect_led
Definition: chunkview.c:64
int freenect_start_depth(freenect_device *dev)
Definition: fakenect.c:365
int freenect_angle
Definition: chunkview.c:63
int freenect_set_led(freenect_device *dev, freenect_led_options option)
Definition: fakenect.c:414
unsigned char uint8_t
void * gl_threadfunc(void *arg)
Definition: chunkview.c:230
int freenect_shutdown(freenect_context *ctx)
Definition: fakenect.c:402
int frame
Definition: regview.c:72
pthread_cond_t gl_frame_cond
Definition: chunkview.c:67
int g_argc
Definition: chunkview.c:47
FREENECTAPI void freenect_set_depth_chunk_callback(freenect_device *dev, freenect_chunk_cb cb)
Definition: cameras.c:1102
int freenect_set_tilt_degs(freenect_device *dev, double angle)
Definition: fakenect.c:410
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
GLuint gl_depth_tex
Definition: chunkview.c:59
freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_depth_format fmt)
Definition: fakenect.c:284
char ** g_argv
Definition: chunkview.c:48
void ReSizeGLScene(int Width, int Height)
Definition: chunkview.c:199
uint8_t * depth_front
Definition: chunkview.c:57
int window
Definition: chunkview.c:50
void DrawGLScene()
Definition: chunkview.c:114
freenect_raw_tilt_state * freenect_get_tilt_state(freenect_device *dev)
Definition: fakenect.c:217
int freenect_update_tilt_state(freenect_device *dev)
Definition: fakenect.c:418
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 freenect_open_device(freenect_context *ctx, freenect_device **dev, int index)
Definition: fakenect.c:314
void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
Definition: chunkview.c:257
Data from the tilt motor and accelerometer.
Definition: libfreenect.h:167
unsigned int uint32_t
capture state
Definition: micview.c:53
freenect_device_flags
Definition: libfreenect.h:58
int freenect_stop_depth(freenect_device *dev)
Definition: fakenect.c:377
void InitGL(int Width, int Height)
Definition: chunkview.c:209
void keyPressed(unsigned char key, int x, int y)
Definition: chunkview.c:148
freenect_device * f_dev
Definition: chunkview.c:62
int main(int argc, char **argv)
Definition: chunkview.c:348
int freenect_process_events(freenect_context *ctx)
Definition: fakenect.c:140
uint16_t t_gamma[2048]
Definition: chunkview.c:254
int freenect_init(freenect_context **ctx, freenect_usb_context *usb_ctx)
Definition: fakenect.c:327


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