Fawkes API Fawkes Development Version
kinect.cpp
1
2/***************************************************************************
3 * kinect.cpp - Microsoft Kinect 3D Camera using the freenect driver
4 *
5 * Created: Fri Nov 26 11:03:24 2010
6 * Copyright 2010 Daniel Beck
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. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#include "kinect.h"
25
26#include <cmath>
27#include <cstdio>
28#include <cstdlib>
29#include <cstring>
30
31namespace firevision {
32
33/** Color image */
34const unsigned int KinectCamera::RGB_IMAGE = 0;
35
36/** False color depth image */
37const unsigned int KinectCamera::FALSE_COLOR_DEPTH_IMAGE = 1;
38
39/** @class KinectCamera <fvcams/kinect.h>
40 * Access the Microsoft Kinect camera using the freenect driver.
41 * @author Daniel Beck
42 */
43
44/** @class FvFreenectDevice <fvcams/kinect.h>
45 * Implementation of the FreenectDevice interface of the driver.
46 * @author Daniel Beck
47 */
48
49/** Constructor.
50 * @param ctx the freenet context
51 * @param index the index of the new device
52 */
53FvFreenectDevice::FvFreenectDevice(freenect_context *ctx, int index) : FreenectDevice(ctx, index)
54{
55 m_rgb_buffer = (unsigned char *)malloc(FREENECT_RGB_SIZE);
56 m_depth_buffer = (uint16_t *)malloc(FREENECT_DEPTH_SIZE);
57}
58
59/** Destructor. */
61{
62 free(m_rgb_buffer);
63 free(m_depth_buffer);
64}
65
66/** Callback function for the freenect driver.
67 * This function is called with a pointer to the RGB image and the
68 * timestamp of the frame.
69 * @param rgb pointer to the RGB image
70 * @param timestamp timestamp of the image
71 */
72void
73FvFreenectDevice::RGBCallback(freenect_pixel *rgb, uint32_t timestamp)
74{
75 memcpy((void *)m_rgb_buffer, (void *)rgb, FREENECT_RGB_SIZE);
76 m_depth_timestamp = timestamp;
77}
78
79/** Callback function for the freenect driver.
80 * This function is called with a pointer to the depth image and the
81 * timestamp of the frame.
82 * @param depth pointer to the depth image
83 * @param timestamp timestamp of the image
84 */
85void
86FvFreenectDevice::DepthCallback(void *depth, uint32_t timestamp)
87{
88 memcpy((void *)m_depth_buffer, (void *)depth, FREENECT_DEPTH_SIZE);
89 m_depth_timestamp = timestamp;
90}
91
92/** Access the RGB buffer.
93 * @return pointer to the RGB buffer
94 */
95unsigned char *
97{
98 return m_rgb_buffer;
99}
100
101/** Access the depth buffer.
102 * @return pointer to the depth buffer
103 */
104uint16_t *
106{
107 return m_depth_buffer;
108}
109
110/** Constructor.
111 * @param cap camera argument parser
112 */
114: m_freenect_dev(0),
115 m_opened(false),
116 m_started(false),
117 m_image_num(FALSE_COLOR_DEPTH_IMAGE),
118 m_buffer(0),
119 m_false_color_depth_buffer(0)
120{
121 // init freenect context
122 m_freenect_ctx = new Freenect::Freenect<FvFreenectDevice>();
123
124 for (unsigned int i = 0; i < 2048; ++i) {
125 float v = i / 2048.0;
126 v = powf(v, 3) * 6;
127 m_gamma[i] = v * 6 * 256;
128 }
129}
130
131/** Destructor. */
133{
134 delete m_freenect_ctx;
135}
136
137void
139{
140 try {
141 m_freenect_dev = &(m_freenect_ctx->createDevice(0));
142 m_opened = true;
143 } catch (std::runtime_error &e) {
144 m_opened = false;
145 }
146
147 m_false_color_depth_buffer = (unsigned char *)malloc(FREENECT_RGB_SIZE);
148 set_image_number(m_image_num);
149}
150
151void
153{
154 if (!m_started) {
155 try {
156 m_freenect_dev->startRGB();
157 m_freenect_dev->startDepth();
158 m_started = true;
159 } catch (std::runtime_error &e) {
160 m_started = false;
161 }
162 }
163}
164
165void
167{
168 if (m_started) {
169 try {
170 m_freenect_dev->stopRGB();
171 m_freenect_dev->stopDepth();
172 m_started = false;
173 } catch (std::runtime_error &e) {
174 m_started = true;
175 }
176 }
177}
178
179void
181{
182 m_freenect_ctx->deleteDevice(0);
183 free(m_false_color_depth_buffer);
184}
185
186void
188{
189 if (!m_started || !m_opened) {
190 return;
191 }
192
193 if (FALSE_COLOR_DEPTH_IMAGE == m_image_num) {
194 for (unsigned int i = 0; i < FREENECT_FRAME_PIX; ++i) {
195 freenect_depth *depth = m_freenect_dev->depth_buffer();
196 int pval = m_gamma[depth[i]];
197 int lb = pval & 0xff;
198 switch (pval >> 8) {
199 case 0:
200 m_false_color_depth_buffer[3 * i + 0] = 255;
201 m_false_color_depth_buffer[3 * i + 1] = 255 - lb;
202 m_false_color_depth_buffer[3 * i + 2] = 255 - lb;
203 break;
204
205 case 1:
206 m_false_color_depth_buffer[3 * i + 0] = 255;
207 m_false_color_depth_buffer[3 * i + 1] = lb;
208 m_false_color_depth_buffer[3 * i + 2] = 0;
209 break;
210
211 case 2:
212 m_false_color_depth_buffer[3 * i + 0] = 255 - lb;
213 m_false_color_depth_buffer[3 * i + 1] = 255;
214 m_false_color_depth_buffer[3 * i + 2] = 0;
215 break;
216
217 case 3:
218 m_false_color_depth_buffer[3 * i + 0] = 0;
219 m_false_color_depth_buffer[3 * i + 1] = 255;
220 m_false_color_depth_buffer[3 * i + 2] = lb;
221 break;
222
223 case 4:
224 m_false_color_depth_buffer[3 * i + 0] = 0;
225 m_false_color_depth_buffer[3 * i + 1] = 255 - lb;
226 m_false_color_depth_buffer[3 * i + 2] = 255;
227 break;
228
229 case 5:
230 m_false_color_depth_buffer[3 * i + 0] = 0;
231 m_false_color_depth_buffer[3 * i + 1] = 0;
232 m_false_color_depth_buffer[3 * i + 2] = 255 - lb;
233 break;
234
235 default:
236 m_false_color_depth_buffer[3 * i + 0] = 0;
237 m_false_color_depth_buffer[3 * i + 1] = 0;
238 m_false_color_depth_buffer[3 * i + 2] = 0;
239 break;
240 }
241 }
242 }
243}
244
245void
247{
248}
249
250bool
252{
253 return m_started;
254}
255
256void
258{
259}
260
261unsigned char *
263{
264 return m_buffer;
265}
266
267unsigned int
269{
270 return FREENECT_RGB_SIZE;
271}
272
273void
275{
276}
277
278unsigned int
280{
281 return FREENECT_FRAME_W;
282}
283
284unsigned int
286{
287 return FREENECT_FRAME_H;
288}
289
290colorspace_t
292{
293 return RGB;
294}
295
296void
298{
299 m_image_num = n;
300 switch (m_image_num) {
301 case RGB_IMAGE:
302 m_buffer = m_freenect_dev->rgb_buffer();
303 printf("Selected RGB buffer\n");
304 break;
305
307 m_buffer = m_false_color_depth_buffer;
308 printf("Selected false color depth buffer\n");
309 break;
310
311 default: m_buffer = m_freenect_dev->rgb_buffer();
312 }
313}
314
315} // end namespace firevision
Camera argument parser.
Definition: camargp.h:36
FvFreenectDevice(freenect_context *ctx, int index)
Constructor.
Definition: kinect.cpp:53
~FvFreenectDevice()
Destructor.
Definition: kinect.cpp:60
void RGBCallback(freenect_pixel *rgb, uint32_t timestamp)
Callback function for the freenect driver.
Definition: kinect.cpp:73
unsigned char * rgb_buffer()
Access the RGB buffer.
Definition: kinect.cpp:96
uint16_t * depth_buffer()
Access the depth buffer.
Definition: kinect.cpp:105
void DepthCallback(void *depth, uint32_t timestamp)
Callback function for the freenect driver.
Definition: kinect.cpp:86
virtual unsigned int pixel_height()
Height of image in pixels.
Definition: kinect.cpp:285
KinectCamera(const CameraArgumentParser *cap=NULL)
Constructor.
Definition: kinect.cpp:113
static const unsigned int FALSE_COLOR_DEPTH_IMAGE
False color depth image.
Definition: kinect.h:84
virtual void set_image_number(unsigned int n)
Set image number to retrieve.
Definition: kinect.cpp:297
virtual void print_info()
Print out camera information.
Definition: kinect.cpp:257
virtual void start()
Start image transfer from the camera.
Definition: kinect.cpp:152
static const unsigned int RGB_IMAGE
Color image.
Definition: kinect.h:83
virtual void capture()
Capture an image.
Definition: kinect.cpp:187
virtual colorspace_t colorspace()
Colorspace of returned image.
Definition: kinect.cpp:291
~KinectCamera()
Destructor.
Definition: kinect.cpp:132
virtual void close()
Close camera.
Definition: kinect.cpp:180
virtual unsigned int pixel_width()
Width of image in pixels.
Definition: kinect.cpp:279
virtual bool ready()
Camera is ready for taking pictures.
Definition: kinect.cpp:251
virtual unsigned int buffer_size()
Size of buffer.
Definition: kinect.cpp:268
virtual void open()
Open the camera.
Definition: kinect.cpp:138
virtual void flush()
Flush image queue.
Definition: kinect.cpp:246
virtual void stop()
Stop image transfer from the camera.
Definition: kinect.cpp:166
virtual unsigned char * buffer()
Get access to current image buffer.
Definition: kinect.cpp:262
virtual void dispose_buffer()
Dispose current buffer.
Definition: kinect.cpp:274