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