Fawkes API  Fawkes Development Version
kinect.cpp
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