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 
31 namespace firevision {
32 
33 /** Color image */
34 const unsigned int KinectCamera::RGB_IMAGE = 0;
35 
36 /** False color depth image */
37 const 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  */
53 FvFreenectDevice::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  */
72 void
73 FvFreenectDevice::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  */
85 void
86 FvFreenectDevice::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  */
95 unsigned char *
97 {
98  return m_rgb_buffer;
99 }
100 
101 /** Access the depth buffer.
102  * @return pointer to the depth buffer
103  */
104 uint16_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 
137 void
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 
151 void
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 
165 void
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 
179 void
181 {
182  m_freenect_ctx->deleteDevice(0);
183  free(m_false_color_depth_buffer);
184 }
185 
186 void
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 
245 void
247 {
248 }
249 
250 bool
252 {
253  return m_started;
254 }
255 
256 void
258 {
259 }
260 
261 unsigned char *
263 {
264  return m_buffer;
265 }
266 
267 unsigned int
269 {
270  return FREENECT_RGB_SIZE;
271 }
272 
273 void
275 {
276 }
277 
278 unsigned int
280 {
281  return FREENECT_FRAME_W;
282 }
283 
284 unsigned int
286 {
287  return FREENECT_FRAME_H;
288 }
289 
290 colorspace_t
292 {
293  return RGB;
294 }
295 
296 void
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
firevision::FvFreenectDevice::DepthCallback
void DepthCallback(void *depth, uint32_t timestamp)
Callback function for the freenect driver.
Definition: kinect.cpp:92
firevision::KinectCamera::ready
virtual bool ready()
Definition: kinect.cpp:257
firevision::KinectCamera::pixel_height
virtual unsigned int pixel_height()
Definition: kinect.cpp:291
firevision::KinectCamera::open
virtual void open()
Definition: kinect.cpp:144
firevision::KinectCamera::~KinectCamera
~KinectCamera()
Destructor.
Definition: kinect.cpp:138
firevision::FvFreenectDevice::rgb_buffer
unsigned char * rgb_buffer()
Access the RGB buffer.
Definition: kinect.cpp:102
firevision::KinectCamera::FALSE_COLOR_DEPTH_IMAGE
static const unsigned int FALSE_COLOR_DEPTH_IMAGE
False color depth image.
Definition: kinect.h:90
firevision::KinectCamera::colorspace
virtual colorspace_t colorspace()
Definition: kinect.cpp:297
firevision::KinectCamera::print_info
virtual void print_info()
Definition: kinect.cpp:263
firevision::KinectCamera::start
virtual void start()
Definition: kinect.cpp:158
firevision::KinectCamera::close
virtual void close()
Definition: kinect.cpp:186
firevision::KinectCamera::dispose_buffer
virtual void dispose_buffer()
Definition: kinect.cpp:280
firevision::KinectCamera::KinectCamera
KinectCamera(const CameraArgumentParser *cap=NULL)
Constructor.
Definition: kinect.cpp:119
firevision::KinectCamera::buffer
virtual unsigned char * buffer()
Definition: kinect.cpp:268
firevision::KinectCamera::buffer_size
virtual unsigned int buffer_size()
Definition: kinect.cpp:274
firevision::FvFreenectDevice::RGBCallback
void RGBCallback(freenect_pixel *rgb, uint32_t timestamp)
Callback function for the freenect driver.
Definition: kinect.cpp:79
firevision::CameraArgumentParser
Definition: camargp.h:41
firevision::KinectCamera::set_image_number
virtual void set_image_number(unsigned int n)
Definition: kinect.cpp:303
firevision::FvFreenectDevice::~FvFreenectDevice
~FvFreenectDevice()
Destructor.
Definition: kinect.cpp:66
firevision::KinectCamera::RGB_IMAGE
static const unsigned int RGB_IMAGE
Color image.
Definition: kinect.h:89
firevision::KinectCamera::capture
virtual void capture()
Definition: kinect.cpp:193
firevision::KinectCamera::stop
virtual void stop()
Definition: kinect.cpp:172
firevision::KinectCamera::flush
virtual void flush()
Definition: kinect.cpp:252
firevision::FvFreenectDevice::FvFreenectDevice
FvFreenectDevice(freenect_context *ctx, int index)
Constructor.
Definition: kinect.cpp:59
firevision::KinectCamera::pixel_width
virtual unsigned int pixel_width()
Definition: kinect.cpp:285
firevision::FvFreenectDevice::depth_buffer
uint16_t * depth_buffer()
Access the depth buffer.
Definition: kinect.cpp:111