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