this is the code i have written. can help me whether this is correct?
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "libfreenect.h"
#include "Vec3f.h"
#include <fstream>
#include <pthread.h>
#include <iostream>
#include "libfreenect_sync.h"
#if defined(__APPLE__)
#include <GLUT/glut.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#else
#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#endif
#include <math.h>
pthread_t freenect_thread;
volatile int die = 0;
int g_argc;
char **g_argv;
int window;
pthread_mutex_t gl_backbuf_mutex = PTHREAD_MUTEX_INITIALIZER;
// back: owned by libfreenect (implicit for depth)
// mid: owned by callbacks, "latest frame ready"
// front: owned by GL, "currently being drawn"
uint8_t *depth_mid, *depth_front;
uint8_t *rgb_back, *rgb_mid, *rgb_front;
GLuint gl_depth_tex;
GLuint gl_rgb_tex;
freenect_context *f_ctx;
freenect_device *f_dev;
int freenect_angle = 0;
int freenect_led;
freenect_video_format requested_format = FREENECT_VIDEO_RGB;
freenect_video_format current_format = FREENECT_VIDEO_RGB;
pthread_cond_t gl_frame_cond = PTHREAD_COND_INITIALIZER;
int got_rgb = 0;
int got_depth = 0;
uint16_t t_gamma[2048];
static uint16_t depth[640*480];
//static float xrgb;
//freenect_sync_get_depth((void**)&depth, &ts, 0,
FREENECT_DEPTH_11BIT);
void DrawGLScene()
{
pthread_mutex_lock(&gl_backbuf_mutex);
// When using YUV_RGB mode, RGB frames only arrive at 15Hz, so we
shouldn't force them to draw in lock-step.
// However, this is CPU/GPU intensive when we are receiving frames in
lockstep.
if (current_format == FREENECT_VIDEO_YUV_RGB) {
while (!got_depth && !got_rgb) {
pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
}
} else {
while ((!got_depth || !got_rgb) && requested_format !=
current_format) {
pthread_cond_wait(&gl_frame_cond, &gl_backbuf_mutex);
}
}
if (requested_format != current_format) {
pthread_mutex_unlock(&gl_backbuf_mutex);
return;
}
uint8_t *tmp;
if (got_depth) {
tmp = depth_front;
depth_front = depth_mid;
depth_mid = tmp;
got_depth = 0;
}
if (got_rgb) {
tmp = rgb_front;
rgb_front = rgb_mid;
rgb_mid = tmp;
got_rgb = 0;
}
pthread_mutex_unlock(&gl_backbuf_mutex);
glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB,
GL_UNSIGNED_BYTE, depth_front);
glBegin(GL_TRIANGLE_FAN);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glTexCoord2f(0, 0); glVertex3f(0,0,0);
glTexCoord2f(1, 0); glVertex3f(640,0,0);
glTexCoord2f(1, 1); glVertex3f(640,480,0);
glTexCoord2f(0, 1); glVertex3f(0,480,0);
glEnd();
glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
if (current_format == FREENECT_VIDEO_RGB || current_format ==
FREENECT_VIDEO_YUV_RGB)
glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB,
GL_UNSIGNED_BYTE, rgb_front);
else
glTexImage2D(GL_TEXTURE_2D, 0, 1, 640, 480, 0, GL_LUMINANCE,
GL_UNSIGNED_BYTE, rgb_front+640*4);
glBegin(GL_TRIANGLE_FAN);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glTexCoord2f(0, 0); glVertex3f(640,0,0);
glTexCoord2f(1, 0); glVertex3f(1280,0,0);
glTexCoord2f(1, 1); glVertex3f(1280,480,0);
glTexCoord2f(0, 1); glVertex3f(640,480,0);
glEnd();
glutSwapBuffers();
}
void keyPressed(unsigned char key, int x, int y)
{
if (key == 27) {
die = 1;
pthread_join(freenect_thread, NULL);
glutDestroyWindow(window);
free(depth_mid);
free(depth_front);
free(rgb_back);
free(rgb_mid);
free(rgb_front);
pthread_exit(NULL);
}
if (key == 'w') {
freenect_angle++;
if (freenect_angle > 30) {
freenect_angle = 30;
}
}
if (key == 's') {
freenect_angle = 0;
}
if (key == 'f') {
if (requested_format == FREENECT_VIDEO_IR_8BIT)
requested_format = FREENECT_VIDEO_RGB;
else if (requested_format == FREENECT_VIDEO_RGB)
requested_format = FREENECT_VIDEO_YUV_RGB;
else
requested_format = FREENECT_VIDEO_IR_8BIT;
}
if (key == 'x') {
freenect_angle--;
if (freenect_angle < -30) {
freenect_angle = -30;
}
}
if (key == '1') {
freenect_set_led(f_dev,LED_GREEN);
}
if (key == '2') {
freenect_set_led(f_dev,LED_RED);
}
if (key == '3') {
freenect_set_led(f_dev,LED_YELLOW);
}
if (key == '4') {
freenect_set_led(f_dev,LED_BLINK_GREEN);
}
if (key == '5') {
// 5 is the same as 4
freenect_set_led(f_dev,LED_BLINK_GREEN);
}
if (key == '6') {
freenect_set_led(f_dev,LED_BLINK_RED_YELLOW);
}
if (key == '0') {
freenect_set_led(f_dev,LED_OFF);
}
freenect_set_tilt_degs(f_dev,freenect_angle);
}
void ReSizeGLScene(int Width, int Height)
{
glViewport(0,0,Width,Height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho (0, 1280, 480, 0, -1.0f, 1.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void InitGL(int Width, int Height)
{
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glClearDepth(1.0);
glDepthFunc(GL_LESS);
glDepthMask(GL_FALSE);
glDisable(GL_DEPTH_TEST);
glDisable(GL_BLEND);
glDisable(GL_ALPHA_TEST);
glEnable(GL_TEXTURE_2D);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glShadeModel(GL_FLAT);
glGenTextures(1, &gl_depth_tex);
glBindTexture(GL_TEXTURE_2D, gl_depth_tex);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glGenTextures(1, &gl_rgb_tex);
glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
ReSizeGLScene(Width, Height);
}
void *gl_threadfunc(void *arg)
{
printf("GL thread\n");
glutInit(&g_argc, g_argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA |
GLUT_DEPTH);
glutInitWindowSize(1280, 480);
glutInitWindowPosition(0, 0);
window = glutCreateWindow("LibFreenect");
glutDisplayFunc(&DrawGLScene);
glutIdleFunc(&DrawGLScene);
glutReshapeFunc(&ReSizeGLScene);
glutKeyboardFunc(&keyPressed);
InitGL(1280, 480);
// int depth[640*480];
// short *depth = 0;
uint32_t ts;
freenect_sync_get_depth((void**)&depth, &ts, 0,
FREENECT_DEPTH_11BIT);
FILE *pfLog = fopen("Sample.dat", "wb");
int w =480;
int h = 640;
for(int x = 0; x < w; x ++) {
for(int y = 0,n = 0; y < h; y++, n++) {
float rawDepth = t_gamma[depth[n]];
if (x >= 100 && x <= 120 && y >= 100 && y <= 102) {
printf("%d\t",depth[n]);
}
fprintf(pfLog, "%f\t", rawDepth);
}
fprintf(pfLog, "\r\n");
}
fclose(pfLog);
glutMainLoop();
return NULL;
}
void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
{
int i;
uint16_t *depth = (uint16_t*)v_depth;
pthread_mutex_lock(&gl_backbuf_mutex);
//double fx_d = 5.9421434211923247e+02;
//double fy_d = 5.9104053696870778e+02;
//double cx_d = 3.3930780975300314e+02;
//double cy_d = 2.4273913761751615e+02;
/*
float cx = 320.0;
float cy = 240.0;
float minDistance = -10;
float scaleFactor = 0.0021;
for(int u = 0; u < 480;u++) {
for(int v= 0; v < 640; v++) {
// float rawDepth = t_gamma[depth[depth_offset]];
float xrgb = float(((u - cx) * t_gamma[(void**)&depth]+
minDistance) * scaleFactor * (cx/cy));
//float y = float(((v - cy) * t_gamma[depth[depth_offset]]+
minDistance) * scaleFactor);
/*double xrgb = ((((double)x - cx_d)*dxy)+(ox*fx_d)) * (fx_rgb/((dxy
+oz)*fx_d)) + cx_rgb;
double yrgb = ((((double)y - cy_d)*dxy)+(oy*fy_d)) * (fy_rgb/((dxy
+oz)*fy_d)) + cy_rgb;
*/
/* if (v>= 100 && v<= 120 && u>= 100 && u <= 102) {
printf("%f\t",xrgb);
}
}
}
*/
for (i=0; i<640*480; i++) {
int pval = t_gamma[depth[i]];
int lb = pval & 0xff;
switch (pval>>8) {
case 0:
depth_mid[3*i+0] = 255;
depth_mid[3*i+1] = 255-lb;
depth_mid[3*i+2] = 255-lb;
break;
case 1:
depth_mid[3*i+0] = 255;
depth_mid[3*i+1] = lb;
depth_mid[3*i+2] = 0;
break;
case 2:
depth_mid[3*i+0] = 255-lb;
depth_mid[3*i+1] = 255;
depth_mid[3*i+2] = 0;
break;
case 3:
depth_mid[3*i+0] = 0;
depth_mid[3*i+1] = 255;
depth_mid[3*i+2] = lb;
break;
case 4:
depth_mid[3*i+0] = 0;
depth_mid[3*i+1] = 255-lb;
depth_mid[3*i+2] = 255;
break;
case 5:
depth_mid[3*i+0] = 0;
depth_mid[3*i+1] = 0;
depth_mid[3*i+2] = 255-lb;
break;
default:
depth_mid[3*i+0] = 0;
depth_mid[3*i+1] = 0;
depth_mid[3*i+2] = 0;
break;
}
}
got_depth++;
pthread_cond_signal(&gl_frame_cond);
pthread_mutex_unlock(&gl_backbuf_mutex);
}
void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
{
pthread_mutex_lock(&gl_backbuf_mutex);
// swap buffers
assert (rgb_back == rgb);
rgb_back = rgb_mid;
freenect_set_video_buffer(dev, rgb_back);
rgb_mid = (uint8_t*)rgb;
got_rgb++;
pthread_cond_signal(&gl_frame_cond);
pthread_mutex_unlock(&gl_backbuf_mutex);
}
void *freenect_threadfunc(void *arg)
{
int accelCount = 0;
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev,
freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev,
freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,
FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED
mode, 'f'-video format\n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f
%4f", state->accelerometer_x, state->accelerometer_y, state-
>accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev,
freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM,
requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int
index, freenect_depth_format fmt);
/*double raw2depth( int raw_d ) {
//int raw_d = buf[row*width+col];
if (raw_d < 2047) {
return t_gamma(i);
}
return 0.0;
}
*/
int main(int argc, char **argv)
{
using namespace std;
int res;
depth_mid = (uint8_t*)malloc(640*480*3);
depth_front = (uint8_t*)malloc(640*480*3);
rgb_back = (uint8_t*)malloc(640*480*3);
rgb_mid = (uint8_t*)malloc(640*480*3);
rgb_front = (uint8_t*)malloc(640*480*3);
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;
}
/*float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;*/
/* const float k1 = 1.1863;
const float k2 = 2842.5;
const float k3 = 0.1236;
t_gamma[i] = k3 * tanf(i/k2 + k1);
*/
/*for (int i = 0; i < 2048; i++) {
t_gamma[i] = (float)(1.0 / ((double)(i) * -0.0030711016 +
3.
3309495161));
}*/
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
printf ("Open device ...\n");
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
printf ("Create thread...\n");
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc,
NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
printf ("gl_threadfunc...\n");
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
printf ("Get depth...\n");
// int depth[640*480];
// short *depth = 0;
uint32_t ts;
freenect_sync_get_depth((void**)&depth, &ts, 0,
FREENECT_DEPTH_11BIT);
FILE *pfLog = fopen("Sample.dat", "wb");
int w =480;
int h = 640;
for(int x = 0; x < w; x ++) {
for(int y = 0,n = 0; y < h; y++, n++) {
float rawDepth = t_gamma[depth[n]];
if (x >= 100 && x <= 120 && y >= 100 && y <= 102) {
printf("%d\t",depth[n]);
}
fprintf(pfLog, "%f\t", rawDepth);
}
fprintf(pfLog, "\r\n");
}
fclose(pfLog);
return 0;