eleni@5: #include <GL/gl.h>
eleni@4: #include <cxcore.h>
eleni@4: #include <highgui.h> //TODO remove
eleni@6: #include <cv.h> //TODO remove
eleni@4: 
eleni@2: #include <pthread.h>
eleni@1: #include <stdio.h>
eleni@2: #include <string.h>
eleni@1: 
eleni@1: #include "frame.h"
eleni@2: #include "kinect.h"
eleni@1: 
eleni@2: extern Frame *frame;
eleni@2: extern KinectParams *kin_params;
eleni@1: 
eleni@2: extern bool has_video;
eleni@2: extern bool has_depth;
eleni@2: 
eleni@2: Frame::Frame()
eleni@1: {
eleni@5:     video_buf = cv::Mat::zeros(KINECT_VIDEO_HEIGHT, KINECT_VIDEO_WIDTH, CV_8UC3);
eleni@6:     depth_buf = cv::Mat::zeros(KINECT_DEPTH_HEIGHT, KINECT_DEPTH_WIDTH, CV_16UC1);
eleni@5: 
eleni@5: 	tex_setup();
eleni@5: }
eleni@5: 
eleni@5: void Frame::tex_setup()
eleni@5: {
eleni@5: 	glGenTextures(1, &video_tex);
eleni@5: 	glGenTextures(1, &depth_tex);
eleni@5: 
eleni@5: 	glBindTexture(GL_TEXTURE_2D, video_tex);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
eleni@5: 	glTexImage2D(GL_TEXTURE_2D, 0, GL_BGRA, video_buf.size().width, video_buf.size().height,
eleni@5: 			0, GL_BGRA, GL_UNSIGNED_BYTE, &video_buf);
eleni@5: 
eleni@5: 	glBindTexture(GL_TEXTURE_2D, depth_tex);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
eleni@5: 	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
eleni@5: 	glTexImage2D(GL_TEXTURE_2D, 0, GL_BGRA, depth_buf.size().width, depth_buf.size().height,
eleni@5: 			0, GL_BGRA, GL_UNSIGNED_BYTE, &depth_buf);
eleni@5: 
eleni@1: }
eleni@1: 
eleni@2: void Frame::process()
eleni@1: {
eleni@2: 	if(has_video && has_depth)
eleni@2: 		printf("Depth and Rgb\n");
eleni@2: }
eleni@2: 
eleni@2: void video_cb(freenect_device *kin_dev, void *video, uint32_t time)
eleni@2: {
eleni@4: 	if(!video || !frame) {
eleni@2: 		has_video = false;
eleni@2: 		return;
eleni@2: 	}
eleni@2: 
eleni@5: 	/* freenect video data to cv mat: */
eleni@5: 	unsigned char* src = (unsigned char*)video;
eleni@5: 	unsigned char* dest = frame->video_buf.data;
eleni@5: 
eleni@5: 	for(int i=0; i<KINECT_VIDEO_HEIGHT * KINECT_VIDEO_WIDTH; i++) {
eleni@5: 		dest[0] = src[2];
eleni@5: 		dest[1] = src[1];
eleni@5: 		dest[2] = src[0];
eleni@5: 		dest += 3;
eleni@5: 		src += 3;
eleni@5: 	}
eleni@2: 	has_video = true;
eleni@1: }
eleni@1: 
eleni@1: void depth_cb(freenect_device *kin_dev, void *depth, uint32_t time)
eleni@1: {
eleni@5: 	if(!depth || !frame) {
eleni@2: 		has_depth = false;
eleni@2: 		return;
eleni@2: 	}
eleni@2: 
eleni@5: 	/* freenect depth data to cv mat: */
eleni@5: 
eleni@6: 	memcpy(frame->depth_buf.data, depth, KINECT_DEPTH_WIDTH * KINECT_DEPTH_HEIGHT * 2);
eleni@5: 
eleni@6: 	save_depth_ppm(depth, KINECT_DEPTH_WIDTH, KINECT_DEPTH_HEIGHT);
eleni@6: 	cv::imshow("foo", frame->depth_buf);
eleni@6: 	cv::waitKey(100);
eleni@2: 	has_depth = true;
eleni@1: }
eleni@5: 
eleni@5: bool save_video_ppm(void *video, int w, int h)
eleni@5: {
eleni@5: 	FILE *fp;
eleni@5: 	if(!(fp = fopen("test_video.ppm", "wb"))) {
eleni@5: 		fprintf(stderr, "Failed to open video file for writing.\n");
eleni@5: 		return false;
eleni@5: 	}
eleni@6: 	fprintf(fp, "P6\n%d %d\n255\n", w, h);
eleni@6: 	unsigned char *ptr = (unsigned char*)video;
eleni@5: 	for(int i=0; i<w * h * 3; i++) {
eleni@5: 		fputc(*ptr, fp);
eleni@5: 		ptr++;
eleni@5: 	}
eleni@5: 	fclose(fp);
eleni@5: 	return true;
eleni@5: }
eleni@5: 
eleni@5: bool save_depth_ppm(void *depth, int w, int h)
eleni@5: {
eleni@5: 	FILE *fp;
eleni@5: 	if(!(fp = fopen("test_depth.ppm", "wb"))) {
eleni@5: 		fprintf(stderr, "Failed to open depth file for writing.\n");
eleni@5: 		return false;
eleni@5: 	}
eleni@6: 	fprintf(fp, "P6\n%d %d\n255\n", w, h);
eleni@6: 	uint16_t *ptr = (uint16_t*)depth;
eleni@6: 	for(int i=0; i<w * h; i++) {
eleni@6: 		unsigned char c = *ptr++ >> 8;
eleni@6: 		fputc(c, fp);
eleni@6: 		fputc(c, fp);
eleni@6: 		fputc(c, fp);
eleni@5: 	}
eleni@5: 	fclose(fp);
eleni@5: 	return true;
eleni@5: }