Fawkes API  Fawkes Development Version
depth_drawer.cpp
1 
2 /***************************************************************************
3  * depth_drawer.cpp - Skeleton Visualization GUI: depth drawer
4  *
5  * Created: Tue Mar 29 17:17:47 2011 (on the way to Magdeburg for GO2011)
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "depth_drawer.h"
24 #include <plugins/openni/utils/colors.h>
25 
26 #include <fvcams/camera.h>
27 #include <fvutils/color/colorspaces.h>
28 #include <fvutils/color/conversions.h>
29 
30 #include <cstdlib>
31 #include <cstdio>
32 #include <algorithm>
33 #include <GL/glut.h>
34 
35 using namespace fawkes;
36 using namespace fawkes::openni;
37 using namespace firevision;
38 
39 /** @class SkelGuiDepthDrawer "image_drawer.h"
40  * Draw images from camera in texture.
41  * Uses texture mapping to show an image acquired from a camera in the
42  * background.
43  * @author Tim Niemueller
44  */
45 
46 /** Constructor.
47  * @param depth_cam camera to capture depth image
48  * @param label_cam label to capture label frame
49  * @param max_depth maximum depth value to expect
50  */
52  firevision::Camera *label_cam,
53  unsigned int max_depth)
54  : SkelGuiTextureDrawer(depth_cam->pixel_width(), depth_cam->pixel_height()),
55  __max_depth(max_depth)
56 {
57  __depth_cam = depth_cam;
58  __label_cam = label_cam;
59  __rgb_buf = malloc_buffer(RGB, __width, __height);
60  __histogram = (float *)malloc(__max_depth * sizeof(float));
61  __show_labels = true;
62 }
63 
64 /** Destructor. */
66 {
67  free(__rgb_buf);
68  free(__histogram);
69 }
70 
71 /** Toggle label state.
72  * Turns on or off the label coloring of the depth map.
73  */
74 void
76 {
77  __show_labels = ! __show_labels;
78 }
79 
80 /** Fill texture. */
81 void
83 {
84  try {
85  __depth_cam->capture();
86  } catch (Exception &e) {
87  printf("Capturing depth image failed, exception follows\n");
88  e.print_trace();
89  throw;
90  }
91 
92  uint16_t *depth = (uint16_t *)__depth_cam->buffer();
93  unsigned int num_points = 0;
94  memset(__histogram, 0, __max_depth * sizeof(float));
95 
96  // base histogram
97  for (unsigned int i = 0; i < __width * __height; ++i) {
98  if (depth[i] != 0) {
99  ++__histogram[depth[i]];
100  ++num_points;
101  }
102  }
103 
104  // accumulative histogram
105  for (unsigned int i = 1; i < __max_depth; ++i) {
106  __histogram[i] += __histogram[i-1];
107  }
108 
109  // set gray value in histogram
110  if (num_points > 0) {
111  for (unsigned int i = 1; i < __max_depth; ++i) {
112  __histogram[i] = truncf(256. * (1.f - (__histogram[i] / num_points)));
113  }
114  }
115 
116  if (__label_cam) {
117  try {
118  __label_cam->capture();
119  } catch (Exception &e) {
120  printf("Capturing label image failed, exception follows\n");
121  e.print_trace();
122  throw;
123  }
124  uint16_t *l = (uint16_t *)__label_cam->buffer();
125  uint16_t *d = depth;
126  unsigned char *r = __rgb_buf;
127  for (unsigned int i = 0; i < __width * __height; ++i, ++l, ++d, r += 3) {
128  r[0] = 0; r[1] = 0; r[2] = 0;
129  unsigned int color = *l % NUM_USER_COLORS;
130  if (!__show_labels || (*l == 0)) color = NUM_USER_COLORS;
131 
132  if (*d != 0) {
133  float hv = __histogram[*d];
134  r[0] = hv * USER_COLORS[color][0];
135  r[1] = hv * USER_COLORS[color][1];
136  r[2] = hv * USER_COLORS[color][2];
137  }
138  }
139  __label_cam->dispose_buffer();
140  } else {
141  uint16_t *d = depth;
142  unsigned char *r = __rgb_buf;
143  for (unsigned int i = 0; i < __width * __height; ++i, ++d, r += 3) {
144  r[0] = 0; r[1] = 0; r[2] = 0;
145  if (*d != 0) {
146  float hv = __histogram[*d];
147  r[0] = hv;
148  r[1] = hv;
149  r[2] = hv;
150  }
151  }
152  }
153 
154  copy_rgb_to_texture(__rgb_buf);
155 
156  __depth_cam->dispose_buffer();
157 }
158 
Camera interface for image aquiring devices in FireVision.
Definition: camera.h:35
Fawkes library namespace.
void toggle_show_labels()
Toggle label state.
SkelGuiDepthDrawer(firevision::Camera *depth_cam, firevision::Camera *label_cam, unsigned int max_depth)
Constructor.
const unsigned int __width
Width of visible area from texture.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void capture()=0
Capture an image.
~SkelGuiDepthDrawer()
Destructor.
void copy_rgb_to_texture(const unsigned char *rgb_buf)
Copy an RGB buffer to texture.
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
virtual unsigned char * buffer()=0
Get access to current image buffer.
Draw images from camera in texture.
const unsigned int __height
Height of visible area from texture.
virtual void fill_texture()
Fill texture.
virtual void dispose_buffer()=0
Dispose current buffer.