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
37using namespace fawkes;
38using namespace fawkes::openni;
39using 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 */
76void
78{
79 show_labels_ = !show_labels_;
80}
81
82/** Fill texture. */
83void
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() noexcept
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.