Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * depth_drawer.cpp - Skeleton Visualization GUI: depth drawer 00004 * 00005 * Created: Tue Mar 29 17:17:47 2011 (on the way to Magdeburg for GO2011) 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "depth_drawer.h" 00024 #include <plugins/openni/utils/colors.h> 00025 00026 #include <fvcams/camera.h> 00027 #include <fvutils/color/colorspaces.h> 00028 #include <fvutils/color/conversions.h> 00029 00030 #include <cstdlib> 00031 #include <cstdio> 00032 #include <algorithm> 00033 #include <GL/glut.h> 00034 00035 using namespace fawkes; 00036 using namespace fawkes::openni; 00037 using namespace firevision; 00038 00039 /** @class SkelGuiDepthDrawer "image_drawer.h" 00040 * Draw images from camera in texture. 00041 * Uses texture mapping to show an image acquired from a camera in the 00042 * background. 00043 * @author Tim Niemueller 00044 */ 00045 00046 /** Constructor. 00047 * @param depth_cam camera to capture depth image 00048 * @param label_cam label to capture label frame 00049 * @param max_depth maximum depth value to expect 00050 */ 00051 SkelGuiDepthDrawer::SkelGuiDepthDrawer(firevision::Camera *depth_cam, 00052 firevision::Camera *label_cam, 00053 unsigned int max_depth) 00054 : SkelGuiTextureDrawer(depth_cam->pixel_width(), depth_cam->pixel_height()), 00055 __max_depth(max_depth) 00056 { 00057 __depth_cam = depth_cam; 00058 __label_cam = label_cam; 00059 __rgb_buf = malloc_buffer(RGB, __width, __height); 00060 __histogram = (float *)malloc(__max_depth * sizeof(float)); 00061 __show_labels = true; 00062 } 00063 00064 /** Destructor. */ 00065 SkelGuiDepthDrawer::~SkelGuiDepthDrawer() 00066 { 00067 free(__rgb_buf); 00068 free(__histogram); 00069 } 00070 00071 /** Toggle label state. 00072 * Turns on or off the label coloring of the depth map. 00073 */ 00074 void 00075 SkelGuiDepthDrawer::toggle_show_labels() 00076 { 00077 __show_labels = ! __show_labels; 00078 } 00079 00080 /** Fill texture. */ 00081 void 00082 SkelGuiDepthDrawer::fill_texture() 00083 { 00084 try { 00085 __depth_cam->capture(); 00086 } catch (Exception &e) { 00087 printf("Capturing depth image failed, exception follows\n"); 00088 e.print_trace(); 00089 throw; 00090 } 00091 00092 uint16_t *depth = (uint16_t *)__depth_cam->buffer(); 00093 unsigned int num_points = 0; 00094 memset(__histogram, 0, __max_depth * sizeof(float)); 00095 00096 // base histogram 00097 for (unsigned int i = 0; i < __width * __height; ++i) { 00098 if (depth[i] != 0) { 00099 ++__histogram[depth[i]]; 00100 ++num_points; 00101 } 00102 } 00103 00104 // accumulative histogram 00105 for (unsigned int i = 1; i < __max_depth; ++i) { 00106 __histogram[i] += __histogram[i-1]; 00107 } 00108 00109 // set gray value in histogram 00110 if (num_points > 0) { 00111 for (unsigned int i = 1; i < __max_depth; ++i) { 00112 __histogram[i] = truncf(256. * (1.f - (__histogram[i] / num_points))); 00113 } 00114 } 00115 00116 if (__label_cam) { 00117 try { 00118 __label_cam->capture(); 00119 } catch (Exception &e) { 00120 printf("Capturing label image failed, exception follows\n"); 00121 e.print_trace(); 00122 throw; 00123 } 00124 uint16_t *l = (uint16_t *)__label_cam->buffer(); 00125 uint16_t *d = depth; 00126 unsigned char *r = __rgb_buf; 00127 for (unsigned int i = 0; i < __width * __height; ++i, ++l, ++d, r += 3) { 00128 r[0] = 0; r[1] = 0; r[2] = 0; 00129 unsigned int color = *l % NUM_USER_COLORS; 00130 if (!__show_labels || (*l == 0)) color = NUM_USER_COLORS; 00131 00132 if (*d != 0) { 00133 float hv = __histogram[*d]; 00134 r[0] = hv * USER_COLORS[color][0]; 00135 r[1] = hv * USER_COLORS[color][1]; 00136 r[2] = hv * USER_COLORS[color][2]; 00137 } 00138 } 00139 __label_cam->dispose_buffer(); 00140 } else { 00141 uint16_t *d = depth; 00142 unsigned char *r = __rgb_buf; 00143 for (unsigned int i = 0; i < __width * __height; ++i, ++d, r += 3) { 00144 r[0] = 0; r[1] = 0; r[2] = 0; 00145 if (*d != 0) { 00146 float hv = __histogram[*d]; 00147 r[0] = hv; 00148 r[1] = hv; 00149 r[2] = hv; 00150 } 00151 } 00152 } 00153 00154 copy_rgb_to_texture(__rgb_buf); 00155 00156 __depth_cam->dispose_buffer(); 00157 } 00158