|
Kiretu
|
The usage of the Kiretu-classes is demonstrated with the help of OpenKinect’s C++-OpenGL-wrapper example [1].
The code sections, where code of Kiretu was added are surrounded by comments:
// ###### Kiretu / xxxxx / start ###### ... // ###### Kiretu / xxxxx / start ######
/* This file is part of the Kiretu, a Kinect reconstruction tutor. * * http://pille.iwr.uni-heidelberg.de/~kinect01/ * * This example bases upon the C++-OpenGL-Wrapper example of OpenKinect. * * Copyright (c) 2010 * - Daniel Wunderlich (d.wunderlich@stud.uni-heidelberg.de) and * - Individual OpenKinect contributors. * See CONTRIB file for details. * * The code ist licensed to you under the terms of the GNU General Public * License, version 2.0. See the GPL2 file for the text of the license or the * following URL: * * http://www.gnu.org/licenses/gpl-2.0.txt * * If you redistribute this file in source form, modified or unmodified, you * have to leave this header intact and distribute it under the same terms with * the GPL2 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 * the License. * */ #include <stdio.h> #include <stdlib.h> #include <string.h> #include <assert.h> #include "libfreenect.h" #include <fstream> #include <pthread.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> #include <vector> // ###### Kiretu / includes / start ###### #include "Hud.h" #include "YMLParser.h" #include "FrameGrabber.h" #include "KinectCloud.h" #include "CloudWriter.h" #include "Util.h" // ###### Kiretu / includes / end ###### 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; // ###### Kiretu / global variables / start ###### Hud hud; // Hud YMLParser yml("./calibration/calib.yml"); // YML-Parser FrameGrabber fg(50, 5); // Frame-Grabber KinectCloud kinectCloud; // Kinect-Cloud CloudWriter cw("./ply/cloud"); // Cloud-Writer bool scanning = false; // Indicates if program is scanning bool showInfo = false; // Indicates if info-box is shown bool showHelp = false; // Indicates if help-box is shown /* * Flags for enabling/disabling the reconstruction-steps. Only used here for * initializing the steps in a readable way. After starting the program, values * are stored in reconstructionSteps[5] in the same order. * * See documentation for detailed explaination. */ bool enableRawToMeters = true; bool enableDepthToCloud = true; bool enableExtrinsics = true; bool enableRGBMapping = true; bool enableNormalComputation = false; bool reconstructionSteps[5]; // Flags of reconstruction-steps as decribed above. std::string kinectFormat = "RGB"; // Format of the video-stream. Used at HUD. // ###### Kiretu / global variables / end ###### 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(); // ###### Kiretu / printing HUD / start ###### glDisable(GL_TEXTURE_2D); hud.printGLLabel(); hud.printGLScanning(scanning); hud.printGLHelp(showHelp); hud.printGLState(showInfo, freenect_angle, kinectFormat, reconstructionSteps); glEnable(GL_TEXTURE_2D); /* * Setting the Kinect's LEDs. Not the best place to do this, but in depth_cb * the program hangs after calling freenect_set_led. */ if (scanning) { freenect_set_led(f_dev,LED_RED); } else { freenect_set_led(f_dev,LED_YELLOW); } // ###### Kiretu / printing HUD / end ###### glutSwapBuffers(); } void keyPressed(unsigned char key, int x, int y) { if (key == 27) { freenect_set_led(f_dev,LED_GREEN); die = 1; pthread_join(freenect_thread, NULL); glutDestroyWindow(window); free(depth_mid); free(depth_front); free(rgb_back); free(rgb_mid); free(rgb_front); // Not pthread_exit because OSX leaves a thread lying around and doesn't exit exit(0); } 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; kinectFormat = "RGB"; // Kiretu: Setting HUD video-format } else if (requested_format == FREENECT_VIDEO_RGB) { requested_format = FREENECT_VIDEO_YUV_RGB; kinectFormat = "YUV-RGB"; // Kiretu: Setting HUD video-format } else { requested_format = FREENECT_VIDEO_IR_8BIT; kinectFormat = "IR"; // Kiretu: Setting HUD video-format } } if (key == 'x') { freenect_angle--; if (freenect_angle < -30) { freenect_angle = -30; } } // ###### Kiretu / keyboard-shortcuts / start ###### // Toggle help-box if (key == 'h') { showHelp = !showHelp; } // Toggle info-box if (key == 'i') { showInfo = !showInfo; } // Toggle reconstruction-step: raw-to-meter if (key == 'm') { reconstructionSteps[0] = !reconstructionSteps[0]; } // Toggle reconstruction-step: depth-to-cloud if (key == 'd') { reconstructionSteps[1] = !reconstructionSteps[1]; } // Toggle reconstruction-step: apply extrisics if (key == 'e') { reconstructionSteps[2] = !reconstructionSteps[2]; } // Toggle reconstruction-step: rgb-mapping if (key == 'r') { reconstructionSteps[3] = !reconstructionSteps[3]; } // Toggle reconstruction-step: compute normals //~ if (key == 'n') { //~ reconstructionSteps[4] = !reconstructionSteps[4]; //~ } // Start scanning if (key == 32) { if (!scanning) { scanning = true; freenect_set_led(f_dev,LED_RED); } } // ###### Kiretu / keyboard-shortcuts / end ###### 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); glEnable(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("Kiretu-example"); glutDisplayFunc(&DrawGLScene); glutIdleFunc(&DrawGLScene); glutReshapeFunc(&ReSizeGLScene); glutKeyboardFunc(&keyPressed); InitGL(1280, 480); glutMainLoop(); return NULL; } uint16_t t_gamma[2048]; void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp) { uint16_t *depth = (uint16_t*)v_depth; pthread_mutex_lock(&gl_backbuf_mutex); // ###### Kiretu / scanning / start ###### bool addFrame; // Indicates if frame was added to Frame-Grabber if (scanning) { addFrame = fg.addFrame(depth); // Add frame to Frame-Grabber /* * If all frames are added (as specified at the Frame-Grabber's * constructor), the reconstruction of the pointcloud starts. */ if (!addFrame) { fg.addRgb(rgb_front); // Add rgb-image to Frame-Grabber fg.computeZValues(); // Compute the z-values using all frames kinectCloud.addZValues(fg.getZValues()); // z-values to cloud kinectCloud.addRgbValues(fg.getRgbValues()); // rgb to cloud // Reconstruction step: Raw-to-meter if (reconstructionSteps[0]) { kinectCloud.rawToMeter(); } //kinectCloud.undistortDepthImage(yml.getDepthDist()); // Reconstruction step: Depth-to-cloud if (reconstructionSteps[1]) { kinectCloud.depthToCloud(yml.getDepthCam()); } // Reconstruction step: Apply Extrinsics if (reconstructionSteps[2]) { kinectCloud.applyExtrinsics(yml.getRot(), yml.getTrans()); } // Reconstruction step: Depth-RGB-Mapping if (reconstructionSteps[3]) { kinectCloud.computeRgbMapping(yml.getRgbCam()); } // Reconstruction step: Compute normals //~ if (reconstructionSteps[4]) { //~ kinectCloud.computeNormals(); //~ } // Add all important values to the Cloud-Writer: cw.addCloud(kinectCloud.getCloud()); cw.addCloudRgb(kinectCloud.getCloudRgb()); cw.addCloudRgbMapping(kinectCloud.getCloudRgbMapping()); //~ cw.addNormals(kinectCloud.getNormals()); cw.addValidValues(kinectCloud.getValidValues()); cw.addReconstructionSteps(kinectCloud.getReconstructionSteps()); // Print points without an equivalent color at the rgb-image cw.setUndefined(true); // Color of points without an equivalent color at the rgb-image int col[] = {0, 255, 144}; cw.setUndefinedColor(col); cw.writeCloud(); // Write (save) cloud scanning = false; // Disabling scanning-flag } } // ###### Kiretu / scanning / end ###### for (int 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_YELLOW); 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\n\n", 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 main(int argc, char **argv) { // ###### Kiretu / initializing steps / start ###### // Set initializing reconstruction steps as described above reconstructionSteps[0] = enableRawToMeters; // raw-to-depth reconstructionSteps[1] = enableDepthToCloud; // depth-to-cloud reconstructionSteps[2] = enableExtrinsics; // apply extrinsics reconstructionSteps[3] = enableRGBMapping; // rgb-mapping reconstructionSteps[4] = enableNormalComputation; // compute normals yml.parseFile(); // Parse YML-calibration-file cw.setMirror(CloudWriter::MIRROR_Z); // Mirrors the cloud along z-axis cw.setMirror(CloudWriter::MIRROR_Y); // Mirrors the cloud along y-axis // ###### Kiretu / initializing steps / end ###### 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; } 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); freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); 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; if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) { printf("Could not open device\n"); return 1; } res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL); if (res) { printf("pthread_create failed\n"); return 1; } // OS X requires GLUT to run on the main thread gl_threadfunc(NULL); return 0; }
CC = g++ LD = g++ LDFLAGS = CFLAGS=-g -lfreenect -lopengl32 -lglut32 INCLUDES = -I /usr/local/include/libfreenect LIBS = -lfreenect -lGL -lGLU -lglut OBJECTS = kiretu.o Hud.o YMLParser.o FrameGrabber.o KinectCloud.o CloudWriter.o Util.o PROG = kiretu all:$(PROG) $(PROG): $(OBJECTS) $(LD) $(LDFLAGS) $(LIBS) $(OBJECTS) -o $(PROG) %.o: %.cpp $(CC) $(INCLUDES) $(CFLAGS) $(LIBS) -c $< clean: rm -rf *.o $(PROG)
Generated on Sat Jan 28 2012 13:51:49 for Kiretu by version 1.7.3 of
