Fawkes API  Fawkes Development Version
bumblebee2.cpp
00001 
00002 /***************************************************************************
00003  *  bumblebee2.cpp - Point Grey Bumblebee 2 camera
00004  *
00005  *  Generated: Sat Apr 14 20:51:19 2007 (watching Ghostbusters)
00006  *  Copyright  2005-2007  Tim Niemueller [www.niemueller.de]
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 <fvcams/bumblebee2.h>
00025 
00026 #include <fvcams/cam_exceptions.h>
00027 #include <core/exception.h>
00028 #include <fvutils/system/camargp.h>
00029 #include <fvutils/color/conversions.h>
00030 // include <fvutils/writers/pnm.h>
00031 
00032 #include <stdlib.h>
00033 #include <unistd.h>
00034 #include <string>
00035 #ifdef __FreeBSD__
00036 #  include <sys/endian.h>
00037 #elif defined(__MACH__) && defined(__APPLE__)
00038 #  include <sys/_endian.h>
00039 #else
00040 #  include <endian.h>
00041 #endif
00042 
00043 #include <utils/math/angle.h>
00044 
00045 #include <cstdio>
00046 
00047 #include <dc1394/utils.h>
00048 #include <dc1394/register.h>
00049 #include <dc1394/conversions.h>
00050 
00051 using namespace fawkes;
00052 
00053 namespace firevision {
00054 #if 0 /* just to make Emacs auto-indent happy */
00055 }
00056 #endif
00057 
00058 /** @class Bumblebee2Camera <fvcams/bumblebee2.h>
00059  * Bumblebee2 camera.
00060  * Camera implementation that allows fo access to the PointGrey Research Bumblebee2
00061  * camera. It uses libdc1394 to access the camera for fast image transfers (as recommended
00062  * by PTGrey) and can be used in conjunction with the TriclopsStereoProcessor in the
00063  * stereo utilities library.
00064  *
00065  * and the Triclops SDK by PTGrey for calculation of the stereo image.
00066  * This implementation is based on the Firewire implementation and extends it. The
00067  * capture() method implicitly does all the stereo processing needed. This cannot
00068  * be turned off. The video modes is appropriately configured for the camera. You can
00069  * get access to the left and right images where necessary using the set_image_number()
00070  * method and the constants LEFT_ORIGINAL and RIGHT_ORIGINAL. The disparity image buffer
00071  * can be retrieved via buffer_disparity().
00072  *
00073  * Currently only the low resolution version (640x480) of the Bumblebee2 is supported,
00074  * an extension for the hires version may follow if we get one of these cameras.
00075  *
00076  * This class also encapsulates a coordinate system transformation that you can use to
00077  * transform the coordinates from the camera system to another right-handed system like
00078  * the robot system.
00079  *
00080  * The camera coordinate system has the X-axis pointing to the right,
00081  * Y-axis to the floor and Z-axis forward, if the camera is placed parallel to the ground
00082  * and you look in the direction of the camera. The origin of the system is in the right
00083  * lens system of the Bumblebee.
00084  *
00085  * @author Tim Niemueller
00086  */
00087 
00088 
00089 /** Original image in RAW16 */
00090 const unsigned int Bumblebee2Camera::ORIGINAL = 0;
00091 
00092 /** Deinterlaced image */
00093 const unsigned int Bumblebee2Camera::DEINTERLACED = 1;
00094 
00095 /** From bayer tile decoded RGB image */
00096 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2;
00097 
00098 
00099 /// PGR specific registers
00100 /** PTGrey proprietary register: Bayer tile mapping information */
00101 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040)
00102 
00103 /** PTGrey proprietary: config data length */
00104 #define PGR_REG_CONFIG_LENGTH           (0x1FFC) 
00105 
00106 /** PTGrey proprietary register: config register */
00107 #define PGR_REG_CONFIG_DATA             (0x2000) 
00108 
00109 /** PTGrey proprietary register: unit directory offset */
00110 #define PGR_REG_UNIT_DIRECTORY_OFFSET   (0x0424)
00111 
00112 /** PTGrey proprietary register: image data format */
00113 #define PGR_REG_IMAGE_DATA_FORMAT       (0x1048)
00114 /** PTGrey image data format: PGR-specific (little endian) mode */
00115 #define PTG_Y16_Data_Format_PGR_specific  (0xFFFFFFFE)
00116 
00117 /** PTGrey proprietary register: serial number */
00118 #define PGR_REG_SERIAL_NUMBER           (0x1F20)
00119 
00120 /** PTGrey image data format: PGR-specific (little endian) mode */
00121 /** Constructor.
00122  * Initialize and take parameters from camera argument parser. The following
00123  * arguments are supported:
00124  * - nbufs=NBUFS, number of DMA buffers, integer, 0 < n <= 32
00125  * - width=WIDTH, width in pixels of Format7 ROI
00126  * - height=HEIGHT, height in pixels of Format7 ROI
00127  * - startx=STARTX, X start of Format7 ROI
00128  * - starty=STARTY, Y start of Format7 ROI
00129  * @param cap camera argument parser
00130  */
00131 Bumblebee2Camera::Bumblebee2Camera(const CameraArgumentParser *cap)
00132   : FirewireCamera(DC1394_FRAMERATE_30,
00133                    DC1394_VIDEO_MODE_FORMAT7_3,
00134                    DC1394_ISO_SPEED_400,
00135                    /* num buffers */ 8)
00136 {
00137   // Defaults
00138 
00139   _model = strdup(cap->cam_id().c_str());
00140   // num_buffers set in constructor call
00141   _format7_coding = DC1394_COLOR_CODING_RAW16;
00142   _format7_width  = 640;
00143   _format7_height = 480;
00144   _format7_startx = _format7_starty = 0;
00145 
00146   if ( cap->has("nbufs") ) {
00147     _num_buffers = atoi(cap->get("nbufs").c_str());
00148   }
00149   if ( cap->has("width") ) {
00150     _format7_width = atoi(cap->get("width").c_str());
00151   }
00152   if ( cap->has("height") ) {
00153     _format7_height = atoi(cap->get("height").c_str());
00154   }
00155   if ( cap->has("startx") ) {
00156     _format7_startx = atoi(cap->get("startx").c_str());
00157   }
00158   if ( cap->has("starty") ) {
00159     _format7_starty = atoi(cap->get("starty").c_str());
00160   }
00161   if ( cap->has("focus") ) {
00162     parse_set_focus(cap->get("focus").c_str());
00163   }
00164   if ( cap->has("white_balance") ) {
00165     parse_set_white_balance(cap->get("white_balance").c_str());
00166   }
00167   if ( cap->has("shutter") ) {
00168     parse_set_shutter(cap->get("shutter").c_str());
00169   }
00170 
00171   __buffer_deinterlaced = NULL;
00172   __buffer_rgb = NULL;
00173 }
00174 
00175 
00176 /** Destructor. */
00177 Bumblebee2Camera::~Bumblebee2Camera()
00178 {
00179   if (__buffer_deinterlaced != NULL)  free(__buffer_deinterlaced);
00180   if (__buffer_rgb != NULL)           free(__buffer_rgb);
00181 }
00182 
00183 
00184 /** Get BB2 serial no.
00185  * @return BB2 serial number.
00186  */
00187 uint32_t
00188 Bumblebee2Camera::serial_no() const
00189 {
00190   if ( ! _opened )  throw Exception("Camera not opened");
00191 
00192   uint32_t value = 0;
00193   dc1394error_t err = dc1394_get_control_register( _camera, PGR_REG_SERIAL_NUMBER, &value );
00194   if ( err != DC1394_SUCCESS ) {
00195     throw Exception("Bumblebee2::serial_no: dc1394_get_control_register(PGR_REG_SERIAL_NUMBER) failed\n");
00196   }
00197   return value;
00198 }
00199 
00200 
00201 /** Verify GUID validity.
00202  * Compares the given GUID with the GUID of the camera. The GUID may be of two
00203  * forms. If the first four bytes are all 0xFF then it is assumed that the
00204  * GUID was created from the BB2-specific serial number. For example if a
00205  * rectification LUT was generated with the context file only but without
00206  * access to the real camera. Otherwise the GUID is matched against the
00207  * Firewire GUID.
00208  * @param ver_guid GUID to verify
00209  * @return true if the given GUID matches the current camera, false otherwise
00210  */
00211 bool
00212 Bumblebee2Camera::verify_guid(uint64_t ver_guid) const
00213 {
00214   if ( ! _opened )  throw Exception("Camera not opened");
00215 
00216   uint64_t tguid = ver_guid;
00217   tguid >>= 32;
00218   tguid &= 0xFFFFFFFF;
00219   if ( tguid == 0xFFFFFFFF ) {
00220     // serial number!
00221     ver_guid &= 0xFFFFFFFF;
00222     return (serial_no() == ver_guid);
00223   } else {
00224     return (guid() == ver_guid);
00225   }
00226 }
00227 
00228 
00229 void
00230 Bumblebee2Camera::print_info()
00231 {
00232   FirewireCamera::print_info();
00233 
00234   printf("Serial: %u\n", serial_no());
00235 #if (defined(__WORDSIZE) && __WORDSIZE == 64) || (defined(LONG_BIT) && LONG_BIT == 64)
00236   printf("GUID:   0x%016lx\n", (long unsigned int)guid());
00237 #else
00238   printf("GUID:   0x%016llx\n", guid());
00239 #endif
00240 }
00241 
00242 void
00243 Bumblebee2Camera::open()
00244 {
00245   try {
00246     FirewireCamera::open();
00247   } catch (Exception &e) {
00248     throw;
00249   }
00250 
00251   if ( ! _opened ) {
00252     throw Exception("Bumblebee2Camera::open: FirewireCamera::open dit not suceed");
00253   }
00254 
00255   __buffer_deinterlaced = (unsigned char *)malloc(pixel_width() * pixel_height() * 2);
00256   __buffer_rgb = malloc_buffer(RGB, pixel_width(), pixel_height() * 2);
00257   __buffer = NULL;
00258 
00259 #if __BYTE_ORDER == __LITTLE_ENDIAN
00260   dc1394error_t err;
00261   typedef union {
00262     uint32_t value;
00263     struct {
00264       uint32_t   presence   :  1;
00265       uint32_t   reserved1  : 21;
00266       uint32_t   mirror     :  1;
00267       uint32_t   bayer_mono :  1;
00268       uint32_t   reserved2  :  7;
00269       uint32_t   data_format:  1;
00270     } idf;
00271   } idf_u;
00272   idf_u value;
00273   err = dc1394_get_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, &(value.value) );
00274   if ( err != DC1394_SUCCESS ) {
00275     throw Exception("Bumblebee2::open: dc1394_get_control_register(PGR_REG_DATA_FORMAT) failed\n");
00276   }
00277   value.value &= PTG_Y16_Data_Format_PGR_specific;
00278   value.idf.data_format = 0;
00279   err = dc1394_set_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, value.value );
00280   if ( err != DC1394_SUCCESS ) {
00281     throw Exception("Bumblebee2::open: Setting PGR-specific mode on little-endian system failed\n");
00282   }
00283 #endif
00284 
00285   get_bayer_tile();
00286 }
00287 
00288 
00289 void
00290 Bumblebee2Camera::close()
00291 {
00292   if ( _opened ) {
00293     FirewireCamera::close();  
00294     if (__buffer_deinterlaced != NULL) {
00295       free(__buffer_deinterlaced);
00296       __buffer_deinterlaced = NULL;
00297     }
00298     if (__buffer_rgb != NULL) {
00299       free(__buffer_rgb);
00300       __buffer_rgb = NULL;
00301     }
00302   }
00303 }
00304 
00305 void
00306 Bumblebee2Camera::capture()
00307 {
00308   try {
00309     FirewireCamera::capture();
00310   } catch (CaptureException &e) {
00311     e.append("Bumblebee2Camera::capture: failed to retrieve image");
00312     if ( ORIGINAL == __image_num )  __buffer = NULL;
00313     throw;
00314   }
00315   if ( ORIGINAL == __image_num ) {
00316     __buffer = _frame->image;
00317   }
00318 }
00319 
00320 
00321 unsigned char *
00322 Bumblebee2Camera::buffer()
00323 {
00324   return __buffer;
00325 }
00326 
00327 
00328 void
00329 Bumblebee2Camera::set_image_number(unsigned int image_num)
00330 {
00331   __image_num = image_num;
00332   switch ( image_num ) {
00333   case DEINTERLACED: __buffer = __buffer_deinterlaced; break;
00334   case RGB_IMAGE: __buffer = __buffer_rgb; break;
00335   default:  __buffer = NULL; break;
00336   }
00337 }
00338 
00339 
00340 /** Check if connected camera is a Bumblebee2.
00341  * @return true, if the connected camera is a Bumblebee2, false otherwise
00342  */
00343 bool
00344 Bumblebee2Camera::is_bumblebee2()
00345 {
00346   if ( ! _opened ) throw CameraNotOpenedException();
00347 
00348   return( strncmp( _camera->model, "Bumblebee2", strlen("Bumblebee2") ) == 0);
00349 }
00350 
00351 
00352 /** De-interlace the 16 bit data into 2 bayer tile pattern images. */
00353 void
00354 Bumblebee2Camera::deinterlace_stereo()
00355 {
00356   dc1394_deinterlace_stereo( _frame->image, __buffer_deinterlaced,
00357                              pixel_width(), 2 * pixel_height() ); 
00358 }
00359 
00360 
00361 /** Extract RGB color image from the bayer tile image.
00362  * This will transform the bayer tile image to an RGB image using the
00363  * nearest neighbour method.
00364  * Note: this will alias colors on the top and bottom rows
00365  */
00366 void
00367 Bumblebee2Camera::decode_bayer()
00368 {
00369   dc1394_bayer_decoding_8bit( __buffer_deinterlaced, __buffer_rgb,
00370                               pixel_width(), 2 * pixel_height(), 
00371                               __bayer_pattern, DC1394_BAYER_METHOD_NEAREST ); 
00372 }
00373 
00374 
00375 
00376 
00377 
00378 /** De-interlace the 16 bit data into 2 bayer tile pattern images.
00379  * Can be used for offline de-interlacing.
00380  * @param raw16 In-buffer RAW16-encoded
00381  * @param deinterlaced upon return contains the deinterlaced image
00382  * @param width width of image in pixels
00383  * @param height height of image in pixels
00384  */
00385 void
00386 Bumblebee2Camera::deinterlace_stereo(unsigned char *raw16, unsigned char *deinterlaced,
00387                                      unsigned int width, unsigned int height)
00388 {
00389   dc1394_deinterlace_stereo( raw16, deinterlaced, width, 2 * height ); 
00390 }
00391 
00392 
00393 /** Extract RGB color image from the bayer tile image.
00394  * This will transform the bayer tile image to an RGB image using the
00395  * nearest neighbour method.
00396  * Note: this will alias colors on the top and bottom rows
00397  * @param deinterlaced in-buffer with deinterlaced image
00398  * @param rgb upon return contains RGB image
00399  * @param width width of image in pixels
00400  * @param height height of image in pixels
00401  * @param bayer_pattern bayer pattern, one of
00402  *  - 0x59595959 (YYYY, no pattern)
00403  *  - 0x52474742 (RGGB)
00404  *  - 0x47524247 (GRBG)
00405  *  - 0x42474752 (BGGR)
00406  * This depends on the used camera.
00407  */
00408 void
00409 Bumblebee2Camera::decode_bayer(unsigned char *deinterlaced, unsigned char *rgb,
00410                                unsigned int width, unsigned int height,
00411                                bayer_pattern_t bayer_pattern)
00412 {
00413   dc1394color_filter_t dc_bayer_pattern;
00414 
00415   switch (bayer_pattern) {
00416   default:
00417   case BAYER_PATTERN_YYYY:
00418     dc_bayer_pattern = (dc1394color_filter_t) 0;
00419     break;
00420   case BAYER_PATTERN_RGGB:
00421     dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB;
00422     break;
00423   case BAYER_PATTERN_GBRG:
00424     dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG;
00425     break;
00426   case BAYER_PATTERN_GRBG:
00427     dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG;
00428     break;
00429   case BAYER_PATTERN_BGGR:
00430     dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR;
00431     break;
00432   }
00433 
00434   dc1394_bayer_decoding_8bit( deinterlaced, rgb, width, 2 * height, 
00435                               dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST ); 
00436 }
00437 
00438 
00439 /** Retrieve bayer tile.
00440  * This is an internal method that access a special PTGrey register in the camera to
00441  * determine the bayer tile mode.
00442  */
00443 void
00444 Bumblebee2Camera::get_bayer_tile()
00445 {
00446   uint32_t value;
00447   if (dc1394_get_control_register( _camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value) != DC1394_SUCCESS ) {
00448     throw Exception("Could not query bayer tile register");
00449   }
00450 
00451   // Magic numbers are specific to PTGrey cameras
00452   switch (value) {
00453   default:
00454   case 0x59595959:      // YYYY
00455     // no bayer
00456     __bayer_pattern = (dc1394color_filter_t) 0;
00457     break;
00458   case 0x52474742:      // RGGB
00459     __bayer_pattern = DC1394_COLOR_FILTER_RGGB;
00460     break;
00461   case 0x47425247:      // GBRG
00462     __bayer_pattern = DC1394_COLOR_FILTER_GBRG;
00463     break;
00464   case 0x47524247:      // GRBG
00465     __bayer_pattern = DC1394_COLOR_FILTER_GRBG;
00466     break;
00467   case 0x42474752:      // BGGR
00468     __bayer_pattern = DC1394_COLOR_FILTER_BGGR;
00469     break;
00470   }
00471 }
00472 
00473 
00474 /** Retrieve config from camera.
00475  * This method retrieves the config from the camera and writes it to a file such that
00476  * the Triclops SDK can use it for context initialization.
00477  * @param filename filename to write the config to
00478  * @exception Exception thrown if there is an error when trying to retrieve the config
00479  * or writing it to a file.
00480  */
00481 void
00482 Bumblebee2Camera::write_triclops_config_from_camera_to_file(const char *filename)
00483 {
00484   dc1394error_t err;
00485   uint32_t value;
00486   
00487   err = dc1394_get_control_register( _camera, PGR_REG_CONFIG_LENGTH, &value );
00488   if ( err != DC1394_SUCCESS ) {
00489     throw Exception("dc1394_get_control_register(PGR_REG_CONFIG_LENGTH) failed\n");
00490   }
00491    
00492   // the length of the config file
00493   unsigned long file_size_bytes = value;
00494   if( file_size_bytes == 0 ) {
00495     throw Exception("File size == 0!\n" );
00496   }
00497    
00498   FILE* file = fopen( filename, "w" );
00499   if ( !file ) {
00500     throw Exception("Can't open temporary file\n" );
00501   }
00502 
00503   // Read the config file, and save it to the output file,
00504   // while fixing endianness.
00505   for( unsigned long offset = 0 ; offset < file_size_bytes; offset += 4 ) {
00506     err = dc1394_get_control_register( _camera,
00507                                        PGR_REG_CONFIG_DATA + offset, 
00508                                        &value );
00509      
00510     if( err != DC1394_SUCCESS ) {
00511       Exception e("Failed to get control register");
00512       e.append("Can't get control register 0x%x\n",
00513                (int) (PGR_REG_CONFIG_DATA+offset) );
00514       fclose( file );
00515       throw e;
00516     }
00517     
00518     for( int i = 24; i >= 0; i -= 8 ) {
00519       fputc( ( (value>>i) & 0xFF ), file );
00520     }
00521   }
00522   fclose( file );  
00523 }
00524 
00525 } // end namespace firevision