Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * kinect.cpp - Microsoft Kinect 3D Camera using the freenect driver 00004 * 00005 * Created: Fri Nov 26 11:03:24 2010 00006 * Copyright 2010 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include "kinect.h" 00025 00026 #include <cstdlib> 00027 #include <cstring> 00028 #include <cmath> 00029 00030 #include <cstdio> 00031 00032 namespace firevision { 00033 #if 0 /* just to make Emacs auto-indent happy */ 00034 } 00035 #endif 00036 00037 /** Color image */ 00038 const unsigned int KinectCamera::RGB_IMAGE = 0; 00039 00040 /** False color depth image */ 00041 const unsigned int KinectCamera::FALSE_COLOR_DEPTH_IMAGE = 1; 00042 00043 /** @class KinectCamera <fvcams/kinect.h> 00044 * Access the Microsoft Kinect camera using the freenect driver. 00045 * @author Daniel Beck 00046 */ 00047 00048 /** @class FvFreenectDevice <fvcams/kinect.h> 00049 * Implementation of the FreenectDevice interface of the driver. 00050 * @author Daniel Beck 00051 */ 00052 00053 /** Constructor. 00054 * @param ctx the freenet context 00055 * @param index the index of the new device 00056 */ 00057 FvFreenectDevice::FvFreenectDevice( freenect_context* ctx, int index ) 00058 : FreenectDevice( ctx, index ) 00059 { 00060 m_rgb_buffer = (unsigned char *) malloc( FREENECT_RGB_SIZE ); 00061 m_depth_buffer = (uint16_t *) malloc( FREENECT_DEPTH_SIZE ); 00062 } 00063 00064 /** Destructor. */ 00065 FvFreenectDevice::~FvFreenectDevice() 00066 { 00067 free( m_rgb_buffer ); 00068 free( m_depth_buffer ); 00069 } 00070 00071 /** Callback function for the freenect driver. 00072 * This function is called with a pointer to the RGB image and the 00073 * timestamp of the frame. 00074 * @param rgb pointer to the RGB image 00075 * @param timestamp timestamp of the image 00076 */ 00077 void 00078 FvFreenectDevice::RGBCallback( freenect_pixel* rgb, uint32_t timestamp ) 00079 { 00080 memcpy( (void *) m_rgb_buffer, (void *) rgb, FREENECT_RGB_SIZE ); 00081 m_depth_timestamp = timestamp; 00082 } 00083 00084 /** Callback function for the freenect driver. 00085 * This function is called with a pointer to the depth image and the 00086 * timestamp of the frame. 00087 * @param depth pointer to the depth image 00088 * @param timestamp timestamp of the image 00089 */ 00090 void 00091 FvFreenectDevice::DepthCallback( void* depth, uint32_t timestamp ) 00092 { 00093 memcpy( (void *) m_depth_buffer, (void *) depth, FREENECT_DEPTH_SIZE ); 00094 m_depth_timestamp = timestamp; 00095 } 00096 00097 /** Access the RGB buffer. 00098 * @return pointer to the RGB buffer 00099 */ 00100 unsigned char* 00101 FvFreenectDevice::rgb_buffer() 00102 { 00103 return m_rgb_buffer; 00104 } 00105 00106 /** Access the depth buffer. 00107 * @return pointer to the depth buffer 00108 */ 00109 uint16_t* 00110 FvFreenectDevice::depth_buffer() 00111 { 00112 return m_depth_buffer; 00113 } 00114 00115 /** Constructor. 00116 * @param cap camera argument parser 00117 */ 00118 KinectCamera::KinectCamera( const CameraArgumentParser* cap ) 00119 : m_freenect_dev( 0 ), 00120 m_opened( false ), 00121 m_started( false ), 00122 m_image_num( FALSE_COLOR_DEPTH_IMAGE ), 00123 m_buffer( 0 ), 00124 m_false_color_depth_buffer( 0 ) 00125 { 00126 // init freenect context 00127 m_freenect_ctx = new Freenect::Freenect< FvFreenectDevice >(); 00128 00129 for ( unsigned int i = 0; i < 2048; ++i ) 00130 { 00131 float v = i / 2048.0; 00132 v = powf(v, 3) * 6; 00133 m_gamma[i] = v * 6 * 256; 00134 } 00135 } 00136 00137 /** Destructor. */ 00138 KinectCamera::~KinectCamera() 00139 { 00140 delete m_freenect_ctx; 00141 } 00142 00143 void 00144 KinectCamera::open() 00145 { 00146 try 00147 { 00148 m_freenect_dev = &( m_freenect_ctx->createDevice( 0 ) ); 00149 m_opened = true; 00150 } 00151 catch( std::runtime_error& e ) 00152 { 00153 m_opened = false; 00154 } 00155 00156 m_false_color_depth_buffer = (unsigned char*) malloc( FREENECT_RGB_SIZE ); 00157 set_image_number( m_image_num ); 00158 } 00159 00160 void 00161 KinectCamera::start() 00162 { 00163 if ( !m_started ) 00164 { 00165 try 00166 { 00167 m_freenect_dev->startRGB(); 00168 m_freenect_dev->startDepth(); 00169 m_started = true; 00170 } 00171 catch( std::runtime_error& e ) 00172 { 00173 m_started = false; 00174 } 00175 } 00176 } 00177 00178 void 00179 KinectCamera::stop() 00180 { 00181 if ( m_started ) 00182 { 00183 try 00184 { 00185 m_freenect_dev->stopRGB(); 00186 m_freenect_dev->stopDepth(); 00187 m_started = false; 00188 } 00189 catch( std::runtime_error& e ) 00190 { 00191 m_started = true; 00192 } 00193 } 00194 } 00195 00196 void 00197 KinectCamera::close() 00198 { 00199 m_freenect_ctx->deleteDevice( 0 ); 00200 free( m_false_color_depth_buffer ); 00201 } 00202 00203 void 00204 KinectCamera::capture() 00205 { 00206 if ( !m_started || !m_opened ) { return; } 00207 00208 if ( FALSE_COLOR_DEPTH_IMAGE == m_image_num ) 00209 { 00210 for ( unsigned int i = 0; i < FREENECT_FRAME_PIX; ++i ) 00211 { 00212 freenect_depth* depth = m_freenect_dev->depth_buffer(); 00213 int pval = m_gamma[ depth[i] ]; 00214 int lb = pval & 0xff; 00215 switch (pval>>8) { 00216 case 0: 00217 m_false_color_depth_buffer[3*i+0] = 255; 00218 m_false_color_depth_buffer[3*i+1] = 255-lb; 00219 m_false_color_depth_buffer[3*i+2] = 255-lb; 00220 break; 00221 00222 case 1: 00223 m_false_color_depth_buffer[3*i+0] = 255; 00224 m_false_color_depth_buffer[3*i+1] = lb; 00225 m_false_color_depth_buffer[3*i+2] = 0; 00226 break; 00227 00228 case 2: 00229 m_false_color_depth_buffer[3*i+0] = 255-lb; 00230 m_false_color_depth_buffer[3*i+1] = 255; 00231 m_false_color_depth_buffer[3*i+2] = 0; 00232 break; 00233 00234 case 3: 00235 m_false_color_depth_buffer[3*i+0] = 0; 00236 m_false_color_depth_buffer[3*i+1] = 255; 00237 m_false_color_depth_buffer[3*i+2] = lb; 00238 break; 00239 00240 case 4: 00241 m_false_color_depth_buffer[3*i+0] = 0; 00242 m_false_color_depth_buffer[3*i+1] = 255-lb; 00243 m_false_color_depth_buffer[3*i+2] = 255; 00244 break; 00245 00246 case 5: 00247 m_false_color_depth_buffer[3*i+0] = 0; 00248 m_false_color_depth_buffer[3*i+1] = 0; 00249 m_false_color_depth_buffer[3*i+2] = 255-lb; 00250 break; 00251 00252 default: 00253 m_false_color_depth_buffer[3*i+0] = 0; 00254 m_false_color_depth_buffer[3*i+1] = 0; 00255 m_false_color_depth_buffer[3*i+2] = 0; 00256 break; 00257 } 00258 } 00259 } 00260 } 00261 00262 void 00263 KinectCamera::flush() 00264 { 00265 } 00266 00267 bool 00268 KinectCamera::ready() 00269 { 00270 return m_started; 00271 } 00272 00273 void 00274 KinectCamera::print_info() 00275 { 00276 } 00277 00278 unsigned char* 00279 KinectCamera::buffer() 00280 { 00281 return m_buffer; 00282 } 00283 00284 unsigned int 00285 KinectCamera::buffer_size() 00286 { 00287 return FREENECT_RGB_SIZE; 00288 } 00289 00290 void 00291 KinectCamera::dispose_buffer() 00292 { 00293 } 00294 00295 unsigned int 00296 KinectCamera::pixel_width() 00297 { 00298 return FREENECT_FRAME_W; 00299 } 00300 00301 unsigned int 00302 KinectCamera::pixel_height() 00303 { 00304 return FREENECT_FRAME_H; 00305 } 00306 00307 colorspace_t 00308 KinectCamera::colorspace() 00309 { 00310 return RGB; 00311 } 00312 00313 void 00314 KinectCamera::set_image_number( unsigned int n ) 00315 { 00316 m_image_num = n; 00317 switch ( m_image_num ) 00318 { 00319 case RGB_IMAGE: 00320 m_buffer = m_freenect_dev->rgb_buffer(); 00321 printf( "Selected RGB buffer\n" ); 00322 break; 00323 00324 case FALSE_COLOR_DEPTH_IMAGE: 00325 m_buffer = m_false_color_depth_buffer; 00326 printf( "Selected false color depth buffer\n" ); 00327 break; 00328 00329 default: 00330 m_buffer = m_freenect_dev->rgb_buffer(); 00331 } 00332 } 00333 00334 } // end namespace firevision