23 #include "depth_drawer.h"
24 #include <plugins/openni/utils/colors.h>
26 #include <fvcams/camera.h>
27 #include <fvutils/color/colorspaces.h>
28 #include <fvutils/color/conversions.h>
35 using namespace fawkes;
36 using namespace fawkes::openni;
37 using namespace firevision;
53 unsigned int max_depth)
55 __max_depth(max_depth)
57 __depth_cam = depth_cam;
58 __label_cam = label_cam;
60 __histogram = (
float *)malloc(__max_depth *
sizeof(
float));
77 __show_labels = ! __show_labels;
87 printf(
"Capturing depth image failed, exception follows\n");
92 uint16_t *depth = (uint16_t *)__depth_cam->
buffer();
93 unsigned int num_points = 0;
94 memset(__histogram, 0, __max_depth *
sizeof(
float));
99 ++__histogram[depth[i]];
105 for (
unsigned int i = 1; i < __max_depth; ++i) {
106 __histogram[i] += __histogram[i-1];
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)));
120 printf(
"Capturing label image failed, exception follows\n");
124 uint16_t *l = (uint16_t *)__label_cam->
buffer();
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;
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];
142 unsigned char *r = __rgb_buf;
144 r[0] = 0; r[1] = 0; r[2] = 0;
146 float hv = __histogram[*d];