GTK+ Viewer for Microsoft Kinect (uses libfreeconect)

gtkview.c

/*
 * Gtk example to show depth map from Micrsoft Kinect
 * Copyright (c) 2010 John Stowers
 *
 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
 * for details.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 * In all cases you must keep the copyright notice intact and include a copy
 * of the CONTRIB file.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 */


#include <stdio.h>
#include <string.h>
#include <math.h>

#include <pthread.h>

#include <libusb.h>
#include <gtk/gtk.h>

#include "libfreenect.h"

#define WINDOW_RGB 0
#define WINDOW_DEPTH 1

volatile freenect_context *f_ctx = NULL;

pthread_t gl_thread;
volatile int die = 0;

int g_argc;
char **g_argv;

int window;

pthread_mutex_t gl_backbuf_mutex = PTHREAD_MUTEX_INITIALIZER;

uint8_t gl_depth_front[640*480*4];
uint8_t gl_depth_back[640*480*4];

uint8_t gl_rgb_front[640*480*4];
uint8_t gl_rgb_back[640*480*4];

pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
int got_frames = 0;

uint16_t t_gamma[2048];

void depth_cb(freenect_device *dev, freenect_depth *depth, uint32_t timestamp)
{
	int i;

	pthread_mutex_lock(&gl_backbuf_mutex);
	for (i=0; i<FREENECT_FRAME_PIX; i++) {
		int pval = t_gamma[depth[i]];
		int lb = pval & 0xff;
		switch (pval>>8) {
			case 0:
				gl_depth_back[3*i+0] = 255;
				gl_depth_back[3*i+1] = 255-lb;
				gl_depth_back[3*i+2] = 255-lb;
				break;
			case 1:
				gl_depth_back[3*i+0] = 255;
				gl_depth_back[3*i+1] = lb;
				gl_depth_back[3*i+2] = 0;
				break;
			case 2:
				gl_depth_back[3*i+0] = 255-lb;
				gl_depth_back[3*i+1] = 255;
				gl_depth_back[3*i+2] = 0;
				break;
			case 3:
				gl_depth_back[3*i+0] = 0;
				gl_depth_back[3*i+1] = 255;
				gl_depth_back[3*i+2] = lb;
				break;
			case 4:
				gl_depth_back[3*i+0] = 0;
				gl_depth_back[3*i+1] = 255-lb;
				gl_depth_back[3*i+2] = 255;
				break;
			case 5:
				gl_depth_back[3*i+0] = 0;
				gl_depth_back[3*i+1] = 0;
				gl_depth_back[3*i+2] = 255-lb;
				break;
			default:
				gl_depth_back[3*i+0] = 0;
				gl_depth_back[3*i+1] = 0;
				gl_depth_back[3*i+2] = 0;
				break;
		}
	}
	got_frames++;
	pthread_cond_signal(&gl_frame_cond);
	pthread_mutex_unlock(&gl_backbuf_mutex);
}

void rgb_cb(freenect_device *dev, freenect_pixel *rgb, uint32_t timestamp)
{
	pthread_mutex_lock(&gl_backbuf_mutex);
	got_frames++;
	memcpy(gl_rgb_back, rgb, FREENECT_RGB_SIZE);
	pthread_cond_signal(&gl_frame_cond);
	pthread_mutex_unlock(&gl_backbuf_mutex);
}

void *gl_threadfunc(void *arg)
{
	while(freenect_process_events(f_ctx) >= 0);

	printf("-- done!\n");

	return NULL;
}

static gboolean expose_event_callback_depth (GtkWidget *widget, GdkEventExpose *event, gpointer data)
{
    gdk_draw_rgb_image(
                        widget->window,
                        widget->style->fg_gc[GTK_STATE_NORMAL],
                        0, 0, 
                        640, 480, 
                        GDK_RGB_DITHER_NONE, 
                        gl_depth_front, 
                        640 * 3);

    return TRUE;
}

static gboolean expose_event_callback_rgb (GtkWidget *widget, GdkEventExpose *event, gpointer data)
{
    gdk_draw_rgb_image(
                        widget->window,
                        widget->style->fg_gc[GTK_STATE_NORMAL],
                        0, 0, 
                        640, 480, 
                        GDK_RGB_DITHER_NONE, 
                        gl_rgb_front, 
                        640 * 3);

    return TRUE;
}

static gboolean redraw(gpointer data)
{
    GtkWidget **windows = data;

	pthread_mutex_lock(&gl_backbuf_mutex);

	while (got_frames < 2) {
		pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
	}

	memcpy(gl_depth_front, gl_depth_back, sizeof(gl_depth_back));
	memcpy(gl_rgb_front, gl_rgb_back, sizeof(gl_rgb_back));
	got_frames = 0;
	pthread_mutex_unlock(&gl_backbuf_mutex);

    gtk_widget_queue_draw(GTK_WIDGET(windows[WINDOW_RGB]));
    gtk_widget_queue_draw(GTK_WIDGET(windows[WINDOW_DEPTH]));

    return TRUE;
}

#define FPS_TO_MS(x) ((1.0/x)*1000.0)

int main(int argc, char **argv)
{
	int res;
	freenect_device *f_dev;
    GtkWidget *canvases[2];

    gtk_init(&argc, &argv);
    g_thread_init(NULL);

	printf("Kinect camera test\n");

	int i;
	for (i=0; i<2048; i++) {
		float v = i/2048.0;
		v = powf(v, 3)* 6;
		t_gamma[i] = v*6*256;
	}
	
	g_argc = argc;
	g_argv = argv;

	if (freenect_init(&f_ctx, NULL) < 0) {
		printf("freenect_init() failed\n");
		return 1;
	}

	if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
		printf("Could not open device\n");
		return 1;
	}

	freenect_set_depth_callback(f_dev, depth_cb);
	freenect_set_rgb_callback(f_dev, rgb_cb);
	freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);

	res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
	if (res) {
		printf("pthread_create failed\n");
		return 1;
	}
	
	freenect_start_depth(f_dev);
	freenect_start_rgb(f_dev);

    GtkWidget *window = gtk_window_new( GTK_WINDOW_TOPLEVEL );
    GtkWidget *canvas = gtk_drawing_area_new();
    gtk_widget_set_size_request(canvas, 640, 480);
    g_signal_connect (G_OBJECT (canvas), "expose_event",  
            G_CALLBACK (expose_event_callback_depth), NULL);
    gtk_container_add( GTK_CONTAINER(window), canvas );

    GtkWidget *window2 = gtk_window_new( GTK_WINDOW_TOPLEVEL );
    GtkWidget *canvas2 = gtk_drawing_area_new();
    gtk_widget_set_size_request(canvas2, 640, 480);
    g_signal_connect (G_OBJECT (canvas2), "expose_event",  
            G_CALLBACK (expose_event_callback_rgb), NULL);
    gtk_container_add( GTK_CONTAINER(window2), canvas2 );

    // setup refresh rate
    canvases[WINDOW_DEPTH] = canvas;
    canvases[WINDOW_RGB] = canvas2;

    g_timeout_add (FPS_TO_MS(20), redraw, canvases);

    // go
    gtk_widget_show_all( window );
    gtk_widget_show_all( window2 );
    gtk_main();

	printf("-- exit!\n");

	pthread_exit(NULL);
}