Fawkes API  Fawkes Development Version
depth_drawer.cpp
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