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