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 
25 #include <GL/glut.h>
26 #include <core/exception.h>
27 #include <fvcams/camera.h>
28 #include <fvutils/color/colorspaces.h>
29 #include <fvutils/color/conversions.h>
30 #include <plugins/openni/utils/colors.h>
31 
32 #include <algorithm>
33 #include <cstdio>
34 #include <cstdlib>
35 #include <cstring>
36 
37 using namespace fawkes;
38 using namespace fawkes::openni;
39 using namespace firevision;
40 
41 /** @class SkelGuiDepthDrawer "image_drawer.h"
42  * Draw images from camera in texture.
43  * Uses texture mapping to show an image acquired from a camera in the
44  * background.
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor.
49  * @param depth_cam camera to capture depth image
50  * @param label_cam label to capture label frame
51  * @param max_depth maximum depth value to expect
52  */
54  firevision::Camera *label_cam,
55  unsigned int max_depth)
56 : SkelGuiTextureDrawer(depth_cam->pixel_width(), depth_cam->pixel_height()),
57  depth_cam_(depth_cam),
58  label_cam_(label_cam),
59  rgb_buf_raii_(malloc_buffer(RGB, width_, height_)),
60  rgb_buf_((unsigned char *)*rgb_buf_raii_),
61  max_depth_(max_depth),
62  histogram_raii_(malloc(max_depth_ * sizeof(float))),
63  histogram_((float *)*histogram_raii_),
64  show_labels_(true)
65 {
66 }
67 
68 /** Destructor. */
70 {
71 }
72 
73 /** Toggle label state.
74  * Turns on or off the label coloring of the depth map.
75  */
76 void
78 {
79  show_labels_ = !show_labels_;
80 }
81 
82 /** Fill texture. */
83 void
85 {
86  try {
87  depth_cam_->capture();
88  } catch (Exception &e) {
89  printf("Capturing depth image failed, exception follows\n");
90  e.print_trace();
91  throw;
92  }
93 
94  uint16_t * depth = (uint16_t *)depth_cam_->buffer();
95  unsigned int num_points = 0;
96  memset(histogram_, 0, max_depth_ * sizeof(float));
97 
98  // base histogram
99  for (unsigned int i = 0; i < width_ * height_; ++i) {
100  if (depth[i] != 0) {
101  ++histogram_[depth[i]];
102  ++num_points;
103  }
104  }
105 
106  // accumulative histogram
107  for (unsigned int i = 1; i < max_depth_; ++i) {
108  histogram_[i] += histogram_[i - 1];
109  }
110 
111  // set gray value in histogram
112  if (num_points > 0) {
113  for (unsigned int i = 1; i < max_depth_; ++i) {
114  histogram_[i] = truncf(256. * (1.f - (histogram_[i] / num_points)));
115  }
116  }
117 
118  if (label_cam_) {
119  try {
120  label_cam_->capture();
121  } catch (Exception &e) {
122  printf("Capturing label image failed, exception follows\n");
123  e.print_trace();
124  throw;
125  }
126  uint16_t * l = (uint16_t *)label_cam_->buffer();
127  uint16_t * d = depth;
128  unsigned char *r = rgb_buf_;
129  for (unsigned int i = 0; i < width_ * height_; ++i, ++l, ++d, r += 3) {
130  r[0] = 0;
131  r[1] = 0;
132  r[2] = 0;
133  unsigned int color = *l % NUM_USER_COLORS;
134  if (!show_labels_ || (*l == 0))
135  color = NUM_USER_COLORS;
136 
137  if (*d != 0) {
138  float hv = histogram_[*d];
139  r[0] = hv * USER_COLORS[color][0];
140  r[1] = hv * USER_COLORS[color][1];
141  r[2] = hv * USER_COLORS[color][2];
142  }
143  }
144  label_cam_->dispose_buffer();
145  } else {
146  uint16_t * d = depth;
147  unsigned char *r = rgb_buf_;
148  for (unsigned int i = 0; i < width_ * height_; ++i, ++d, r += 3) {
149  r[0] = 0;
150  r[1] = 0;
151  r[2] = 0;
152  if (*d != 0) {
153  float hv = histogram_[*d];
154  r[0] = hv;
155  r[1] = hv;
156  r[2] = hv;
157  }
158  }
159  }
160 
161  copy_rgb_to_texture(rgb_buf_);
162 
163  depth_cam_->dispose_buffer();
164 }
SkelGuiDepthDrawer(firevision::Camera *depth_cam, firevision::Camera *label_cam, unsigned int max_depth)
Constructor.
virtual void fill_texture()
Fill texture.
void toggle_show_labels()
Toggle label state.
~SkelGuiDepthDrawer()
Destructor.
Draw images from camera in texture.
const unsigned int height_
Height of visible area from texture.
const unsigned int width_
Width of visible area from texture.
void copy_rgb_to_texture(const unsigned char *rgb_buf)
Copy an RGB buffer to texture.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
Camera interface for image aquiring devices in FireVision.
Definition: camera.h:33
virtual void dispose_buffer()=0
Dispose current buffer.
virtual void capture()=0
Capture an image.
virtual unsigned char * buffer()=0
Get access to current image buffer.
Fawkes library namespace.