triclops.cpp

00001 
00002 /***************************************************************************
00003  *  triclops.cpp - Stereo processor using the TriclopsSDK
00004  *
00005  *  Created: Fri May 18 17:20:31 2007
00006  *  Copyright  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 <stereo/triclops.h>
00025 
00026 #include <core/exceptions/software.h>
00027 #include <core/exceptions/system.h>
00028 #include <cams/bumblebee2.h>
00029 #include <fvutils/base/roi.h>
00030 #include <utils/math/angle.h>
00031 #include <fvutils/color/conversions.h>
00032 #include <fvutils/rectification/rectfile.h>
00033 #include <fvutils/rectification/rectinfo_lut_block.h>
00034 
00035 // PGR Triclops SDK
00036 #include <triclops.h>
00037 
00038 #include <cstdlib>
00039 #include <unistd.h>
00040 #include <errno.h>
00041 
00042 #include <math.h>
00043 
00044 using namespace fawkes;
00045 
00046 namespace firevision {
00047 #if 0 /* just to make Emacs auto-indent happy */
00048 }
00049 #endif
00050 
00051 /// @cond INTERNALS
00052 /** Data internal to Triclops stereo processor
00053  * This class exists to be able to hide the triclops stuff from the camera
00054  * user and not to expose the Triclops SDK headers.
00055  */
00056 class TriclopsStereoProcessorData
00057 {
00058  public:
00059   TriclopsContext triclops;
00060   TriclopsInput   input;
00061   TriclopsError   err;
00062   TriclopsImage   rectified_image;
00063   TriclopsImage16 disparity_image_hires;
00064   TriclopsImage   disparity_image_lores;
00065   bool            enable_subpixel_interpolation;
00066 };
00067 /// @endcond
00068 
00069 /** @class TriclopsStereoProcessor <stereo/triclops.h>
00070  * Stereo processing using PGR Triclops SDK.
00071  * This class uses the Triclops SDK provided by Point Grey Research along with
00072  * the Bumblebee2 cameras.
00073  * @author Tim Niemueller
00074  */
00075 
00076 
00077 /** Constructor.
00078  * This constructor initializes this triclops wrapper to work on a real camera.
00079  * @param camera Must be of type Bumblebee2Camera
00080  */
00081 TriclopsStereoProcessor::TriclopsStereoProcessor(Camera *camera)
00082 {
00083   _context_file = NULL;
00084   buffer_deinterlaced = NULL;
00085 
00086   bb2 = dynamic_cast<Bumblebee2Camera *>(camera);
00087   if ( ! bb2 ) {
00088     throw TypeMismatchException("Camera is not of type Bumblebee2Camera");
00089   }
00090   if ( ! bb2->is_bumblebee2() ) {
00091     throw TypeMismatchException("Camera is not a Bumblebee 2");
00092   }
00093 
00094   bb2->set_image_number(Bumblebee2Camera::RGB_IMAGE);
00095 
00096   _width      = bb2->pixel_width();
00097   _height     = bb2->pixel_height();
00098 
00099   _output_image_width  = _width;
00100   _output_image_height = _height;
00101 
00102   create_buffers();
00103   try {
00104     setup_triclops();
00105   } catch (...) {
00106     throw;
00107   }
00108 
00109   buffer_rgb          = bb2->buffer();
00110   buffer_rgb_right    = buffer_rgb; 
00111   buffer_rgb_left     = buffer_rgb + colorspace_buffer_size(RGB, _width, _height); 
00112   buffer_rgb_center   = buffer_rgb_left; // wtf? Done so in pgr code
00113 }
00114 
00115 
00116 /** Constructor.
00117  * With this ctor you can make the triclops wrapper to work on saved images given
00118  * the expected image size (of a single image) and the path to the Triclops
00119  * context from the used camera.
00120  * @param width image width in pixels
00121  * @param height image height in pixels
00122  * @param context_file Triclops context file
00123  */
00124 TriclopsStereoProcessor::TriclopsStereoProcessor(unsigned int width, unsigned int height,
00125                                                  const char *context_file)
00126 {
00127   _width = width;
00128   _height = height;
00129   _output_image_width  = _width;
00130   _output_image_height = _height;
00131   _context_file = strdup(context_file);
00132 
00133   bb2 = NULL;
00134 
00135   create_buffers();
00136   try {
00137     setup_triclops();
00138   } catch (Exception &e) {
00139     throw;
00140   }
00141 }
00142 
00143 
00144 /** Create working buffers.
00145  * buffer size calculated as: we have RAW16 format, which means two bytes per
00146  * pixel, thus total buffer size must be w * h * 2
00147  */
00148 void
00149 TriclopsStereoProcessor::create_buffers()
00150 {
00151   buffer_green        = (unsigned char *)malloc(_width * _height * 2);
00152   buffer_yuv_right    = malloc_buffer(YUV422_PLANAR, _width, _height);
00153   buffer_yuv_left     = malloc_buffer(YUV422_PLANAR, _width, _height);
00154 
00155   if ( bb2 ) {
00156     buffer_rgb          = bb2->buffer();
00157   } else {
00158     buffer_rgb = (unsigned char *)malloc(colorspace_buffer_size(RGB, _width, _height) * 2);
00159     buffer_deinterlaced = (unsigned char *)malloc(_width * _height * 2);
00160   }
00161   buffer_rgb_right    = buffer_rgb; 
00162   buffer_rgb_left     = buffer_rgb + colorspace_buffer_size(RGB, _width, _height); 
00163   buffer_rgb_center   = buffer_rgb_left; // wtf? Done so in pgr code
00164 
00165 }
00166 
00167 
00168 void
00169 TriclopsStereoProcessor::setup_triclops()
00170 {
00171   // Internal data
00172   data = new TriclopsStereoProcessorData();
00173   // Always the same
00174   data->input.inputType   = TriInp_RGB;
00175   data->input.nrows       = _height;
00176   data->input.ncols       = _width;
00177   data->input.rowinc      = data->input.ncols;
00178   /*
00179   data->input.u.rgb.red   = buffer_yuv_right;
00180   data->input.u.rgb.green = buffer_yuv_left;
00181   data->input.u.rgb.blue  = buffer_yuv_left;
00182   */
00183   data->input.u.rgb.red   = buffer_green;
00184   data->input.u.rgb.green = buffer_green + _width * _height;
00185   data->input.u.rgb.blue  = data->input.u.rgb.green;
00186 
00187   if ( bb2 ) {
00188     try {
00189       get_triclops_context_from_camera();
00190     } catch (Exception &e) {
00191       free(data);
00192       throw;
00193     }
00194   } else {
00195 
00196     if ( ! _context_file ) {
00197       free(data);
00198       throw NullPointerException("TriclopsStereoProcessor: You must supply the path "
00199                                  "to a valid BB2 context file");
00200     }
00201 
00202     if ( access(_context_file, F_OK | R_OK) != 0 ) {
00203       free(data);
00204       throw CouldNotOpenFileException("TriclopsStereoProcessor: Cannot access context file");
00205     }
00206     data->err = triclopsGetDefaultContextFromFile(&(data->triclops), _context_file);
00207     if ( data->err != TriclopsErrorOk ) {
00208       free(data);
00209       throw Exception("Fetching Triclops context from camera failed");
00210     }
00211   }
00212 
00213   // Set defaults
00214   data->enable_subpixel_interpolation = false;
00215 
00216   triclopsSetSubpixelInterpolation( data->triclops, 0);
00217 
00218   triclopsSetResolutionAndPrepare( data->triclops,
00219                                    _height, _width,
00220                                    _height, _width);
00221 
00222   triclopsSetEdgeCorrelation( data->triclops, 1 );
00223   triclopsSetLowpass( data->triclops, 1 );
00224   triclopsSetDisparity( data->triclops, 5, 100);
00225   triclopsSetEdgeMask(data->triclops, 11);
00226   triclopsSetStereoMask(data->triclops, 23);
00227   triclopsSetSurfaceValidation(data->triclops, 1);
00228   triclopsSetTextureValidation(data->triclops, 0);
00229 
00230   triclopsSetDisparityMapping( data->triclops, 10, 85 );
00231   triclopsSetDisparityMappingOn( data->triclops, 1 );
00232 }
00233 
00234 
00235 /** Destructor. */
00236 TriclopsStereoProcessor::~TriclopsStereoProcessor()
00237 {
00238   // segfaults :-/
00239   // triclopsDestroyContext( data->triclops );
00240 
00241   if ( buffer_green != NULL )         free(buffer_green);
00242   if ( buffer_yuv_right != NULL )     free(buffer_yuv_right);
00243   if ( buffer_yuv_left != NULL )      free(buffer_yuv_left);
00244   if ( _context_file != NULL)         free(_context_file);
00245 
00246   if ( ! bb2 ) {
00247     if ( buffer_rgb)                  free(buffer_rgb);
00248     if ( buffer_deinterlaced)         free(buffer_deinterlaced);
00249   }
00250 
00251   buffer_green = NULL;
00252   buffer_rgb = NULL;
00253   buffer_yuv_right = NULL;
00254   buffer_yuv_left = NULL;
00255   _buffer = NULL;
00256 
00257   delete data;
00258 }
00259 
00260 
00261 /** Set raw buffer.
00262  * @param raw16_buffer buffer containing the stereo image encoded as BB2 RAW16
00263  */
00264 void
00265 TriclopsStereoProcessor::set_raw_buffer(unsigned char *raw16_buffer)
00266 {
00267   buffer_raw16 = raw16_buffer;
00268 }
00269 
00270 
00271 /** Set the resolution of the output images.
00272  * @param width the width of the output images
00273  * @param height the height of the output images
00274  */
00275 void
00276 TriclopsStereoProcessor::set_output_image_size(unsigned int width, unsigned int height)
00277 {
00278   data->err = triclopsSetResolutionAndPrepare(data->triclops, height, width, _height, _width);
00279 
00280   if ( data->err == TriclopsErrorOk )
00281   {
00282     _output_image_width  = width;
00283     _output_image_height = height;
00284   }
00285 }
00286 
00287 
00288 /** Enable or disable subpixel interpolation
00289  * @param enabled true to enable, false to disable
00290  */
00291 void
00292 TriclopsStereoProcessor::set_subpixel_interpolation(bool enabled)
00293 {
00294   data->enable_subpixel_interpolation = enabled;
00295   triclopsSetSubpixelInterpolation(data->triclops, enabled);
00296 }
00297 
00298 
00299 /** Enable or disable edge correlation.
00300  * @param enabled true to enable, false to disable
00301  */
00302 void
00303 TriclopsStereoProcessor::set_edge_correlation(bool enabled)
00304 {
00305   triclopsSetEdgeCorrelation(data->triclops, enabled);
00306 }
00307 
00308 
00309 /** Enable or disable lowpass filtering before rectification.
00310  * @param enabled true to enable, false to disable
00311  */
00312 void
00313 TriclopsStereoProcessor::set_lowpass(bool enabled)
00314 {
00315   triclopsSetLowpass(data->triclops, enabled);
00316 }
00317 
00318 
00319 /** Set disparity range.
00320  * @param min minimum disparity
00321  * @param max maximum disparity
00322  */
00323 void
00324 TriclopsStereoProcessor::set_disparity_range(int min, int max)
00325 {
00326   triclopsSetDisparity(data->triclops, min, max);
00327 }
00328 
00329 
00330 /** Set edge mask.
00331  * Size of the kernel used for edge detection.
00332  * This value must be in the range [3..13].
00333  * @param mask_size mask size
00334  */
00335 void
00336 TriclopsStereoProcessor::set_edge_masksize(unsigned int mask_size)
00337 {
00338   triclopsSetEdgeMask(data->triclops, mask_size);
00339 }
00340 
00341 
00342 /** Set stereo mask.
00343  * Size of the mask used for stereo correlation.
00344  * @param mask_size mask size
00345  */
00346 void
00347 TriclopsStereoProcessor::set_stereo_masksize(unsigned int mask_size)
00348 {
00349   triclopsSetStereoMask(data->triclops, mask_size);
00350 }
00351 
00352 
00353 /** Enable or disable surface validation.
00354  * @param enabled true to enable, false to disable
00355  */
00356 void
00357 TriclopsStereoProcessor::set_surface_validation(bool enabled)
00358 {
00359   triclopsSetSurfaceValidation(data->triclops, enabled);
00360 }
00361 
00362 
00363 /** Enable or disable texture validation.
00364  * @param enabled true to enable, false to disable
00365  */
00366 void
00367 TriclopsStereoProcessor::set_texture_validation(bool enabled)
00368 {
00369   triclopsSetTextureValidation(data->triclops, enabled);  
00370 }
00371 
00372 
00373 /** Set disparity mapping range.
00374  * @param min minimum disparity
00375  * @param max maximum disparity
00376  */
00377 void
00378 TriclopsStereoProcessor::set_disparity_mapping_range(unsigned char min, unsigned char max)
00379 {
00380   triclopsSetDisparityMapping(data->triclops, min, max);
00381 }
00382 
00383 
00384 /** Enable or disable disparity mapping.
00385  * @param enabled true to enable, false to disable
00386  */
00387 void
00388 TriclopsStereoProcessor::set_disparity_mapping(bool enabled)
00389 {
00390   triclopsSetDisparityMappingOn(data->triclops, enabled);
00391 }
00392 
00393 /** Check state of subpixel interpolation
00394  * @return true if enabled, false otherwise
00395  */
00396 bool
00397 TriclopsStereoProcessor::subpixel_interpolation()
00398 {
00399   TriclopsBool on;
00400   triclopsGetSubpixelInterpolation(data->triclops, &on);
00401   return on;
00402 }
00403 
00404 
00405 /** Get width of ouput images.
00406  * @return width of output images
00407  */
00408 unsigned int
00409 TriclopsStereoProcessor::output_image_width()
00410 {
00411   return _output_image_width;
00412 }
00413 
00414 
00415 /** Get height of ouput images.
00416  * @return height of output images
00417  */
00418 unsigned int
00419 TriclopsStereoProcessor::output_image_height()
00420 {
00421   return _output_image_height;
00422 }
00423 
00424 
00425 /** Check state of edge correlation.
00426  * @return true if enabled, false otherwise
00427  */
00428 bool
00429 TriclopsStereoProcessor::edge_correlation()
00430 {
00431   TriclopsBool on;
00432   triclopsGetEdgeCorrelation(data->triclops, &on);
00433   return on;
00434 }
00435 
00436 
00437 /** Check state of lowpass filtering.
00438  * @return true if enabled, false otherwise
00439  */
00440 bool
00441 TriclopsStereoProcessor::lowpass()
00442 {
00443   TriclopsBool on;
00444   triclopsGetLowpass(data->triclops, &on);
00445   return on;
00446 }
00447 
00448 
00449 /** Get disparity range min value.
00450  * @return disparity range min value
00451  */
00452 int
00453 TriclopsStereoProcessor::disparity_range_min()
00454 {
00455   int min, max;
00456   triclopsGetDisparity( data->triclops, &min, &max );
00457   return min;
00458 }
00459 
00460 
00461 /** Get disparity range max value.
00462  * @return disparity range max value
00463  */
00464 int
00465 TriclopsStereoProcessor::disparity_range_max()
00466 {
00467   int min, max;
00468   triclopsGetDisparity( data->triclops, &min, &max );
00469   return max;
00470 }
00471 
00472 
00473 /** Get edge mask size.
00474  * @return size of the edge mask
00475  */
00476 unsigned int
00477 TriclopsStereoProcessor::edge_masksize()
00478 {
00479   int mask_size = 0;
00480   triclopsGetEdgeMask( data->triclops, &mask_size );
00481   return mask_size;
00482 }
00483 
00484 
00485 /** Get stereo mask size.
00486  * @return size of the stereo mask
00487  */
00488 unsigned int
00489 TriclopsStereoProcessor::stereo_masksize()
00490 {
00491   int mask_size = 0;
00492   triclopsGetStereoMask( data->triclops, &mask_size );
00493   return mask_size;
00494 }
00495 
00496 
00497 /** Check state of surface validation.
00498  * @return true if enabled, false otherwise
00499  */
00500 bool
00501 TriclopsStereoProcessor::surface_validation()
00502 {
00503   TriclopsBool on;
00504   triclopsGetSurfaceValidation(data->triclops, &on);
00505   return on;
00506 }
00507 
00508 
00509 /** Check state of texture validation.
00510  * @return true if enabled, false otherwise
00511  */
00512 bool
00513 TriclopsStereoProcessor::texture_validation()
00514 {
00515   TriclopsBool on;
00516   triclopsGetTextureValidation(data->triclops, &on);
00517   return on;
00518 }
00519 
00520 
00521 /** Get disparity mapping min value.
00522  * @return min value for disparity mapping
00523  */
00524 unsigned char
00525 TriclopsStereoProcessor::disparity_mapping_min()
00526 {
00527   unsigned char min, max;
00528   triclopsGetDisparityMapping( data->triclops, &min, &max );
00529   return min;
00530 }
00531 
00532 
00533 /** Get disparity mapping max value.
00534  * @return max value for disparity mapping
00535  */
00536 unsigned char
00537 TriclopsStereoProcessor::disparity_mapping_max()
00538 {
00539   unsigned char min, max;
00540   triclopsGetDisparityMapping( data->triclops, &min, &max );
00541   return max;
00542 }
00543 
00544 
00545 /** Check state of disparity mapping
00546  * @return true if enabled, false otherwise
00547  */
00548 bool
00549 TriclopsStereoProcessor::disparity_mapping()
00550 {
00551   TriclopsBool on;
00552   triclopsGetDisparityMappingOn(data->triclops, &on);
00553   return on;
00554 }
00555 
00556 
00557 void
00558 TriclopsStereoProcessor::preprocess_stereo()
00559 {
00560   if ( bb2 ) {
00561     bb2->deinterlace_stereo();
00562     bb2->decode_bayer();
00563   } else {
00564     Bumblebee2Camera::deinterlace_stereo(buffer_raw16, buffer_deinterlaced,
00565                                          _width, _height);
00566     Bumblebee2Camera::decode_bayer(buffer_deinterlaced, buffer_rgb,
00567                                    _width, _height, BAYER_PATTERN_BGGR);
00568   }
00569 }
00570 
00571 void
00572 TriclopsStereoProcessor::calculate_yuv(bool both)
00573 {
00574   // RGB -> YUV
00575   convert(RGB, YUV422_PLANAR, buffer_rgb_right, buffer_yuv_right, _width, _height);
00576   if ( both ) {
00577     convert(RGB, YUV422_PLANAR, buffer_rgb_left, buffer_yuv_left, _width, _height);
00578   }
00579 }
00580 
00581 
00582 void
00583 TriclopsStereoProcessor::calculate_disparity(ROI *roi)
00584 {
00585   TriclopsROI *rois;
00586   int          max_rois;
00587 
00588   if ( NULL != roi ) {
00589     if ( TriclopsErrorOk == triclopsGetROIs( data->triclops, &rois, &max_rois ) ) {
00590       // assume there is always at least one ROI
00591       rois[0].col = roi->start.x;
00592       rois[0].row = roi->start.y;
00593       rois[0].ncols = roi->width;
00594       rois[0].nrows = roi->height;
00595 
00596       triclopsSetNumberOfROIs( data->triclops, 1 );
00597     } else {
00598       triclopsSetNumberOfROIs( data->triclops, 0 );
00599     }
00600   } else {
00601     triclopsSetNumberOfROIs( data->triclops, 0 );
00602   }
00603 
00604   // now deinterlace the RGB Buffer
00605   deinterlace_green( buffer_rgb, buffer_green, _width, 6 * _height );
00606 
00607   // rectify
00608   if ( (data->err = triclopsRectify( data->triclops, &(data->input))) != TriclopsErrorOk ) {
00609     throw Exception("Rectifying the image failed");
00610   }
00611 
00612   // disparity
00613   if ( (data->err = triclopsStereo( data->triclops )) != TriclopsErrorOk ) {
00614     throw Exception("Calculating the disparity image failed");
00615   }
00616 
00617   triclopsGetImage(data->triclops, TriImg_RECTIFIED, TriCam_REFERENCE, &(data->rectified_image));
00618 
00619   if ( data->enable_subpixel_interpolation ) {
00620     triclopsGetImage16( data->triclops, TriImg16_DISPARITY, TriCam_REFERENCE, &(data->disparity_image_hires) );
00621   } else {
00622     triclopsGetImage( data->triclops, TriImg_DISPARITY, TriCam_REFERENCE, &(data->disparity_image_lores) );
00623   }
00624   /*
00625   if ( data->enable_subpixel_interpolation ) {
00626     if ( data->disparity_image_hires.data[640 * 240 + 320] < 0xFF00 ) {
00627       float x, y, z;
00628       triclopsRCD16ToXYZ(data->triclops, _height, _width, data->disparity_image_hires.data[640 * 240 + 320], &x, &y, &z);
00629       cout << "Pixel coordinate: (x, y, z) = (" << x << "," << y << "," << z << ")" << endl;
00630     } else {
00631       cout << "Pixel has invalid disparity value" << endl;
00632     }
00633     if ( ! done ) {
00634       triclopsSaveImage16( &(data->disparity_image_hires), "disparity.pgm" );
00635       triclopsSetDisparityMappingOn( data->triclops, 0 );
00636       done = true;
00637     }
00638   } else {
00639     if ( (data->disparity_image_lores.data + data->disparity_image_lores.rowinc * 240 + 320) != 0 ) {
00640       float x, y, z;
00641       triclopsRCD8ToXYZ(data->triclops, 240, 320, *(data->disparity_image_lores.data + data->disparity_image_lores.rowinc * 240 + 320), &x, &y, &z);
00642       cout << "Pixel coordinate: (x, y, z) = (" << x << "," << y << "," << z << ")" << endl;
00643     } else {
00644       cout << "Pixel has invalid disparity value" << endl;
00645     }
00646     if ( ! done ) {
00647       PNMWriter pnm(PNM_PGM, "disparity_own.pgm", _width, _height);
00648       pnm.set_buffer(YUV422_PLANAR, data->disparity_image_lores.data);
00649       pnm.write();
00650       triclopsSaveImage( &(data->disparity_image_lores), "disparity_lores.pgm" );
00651       triclopsSetDisparityMappingOn( data->triclops, 0 );
00652       done = true;
00653     }
00654   }
00655   */
00656 }
00657 
00658 
00659 /** Get the disparity image buffer.
00660  * Access method to the disparity image buffer.
00661  * @return pointer to the internal disparity image buffer
00662  */
00663 unsigned char *
00664 TriclopsStereoProcessor::disparity_buffer()
00665 {
00666   if ( data->enable_subpixel_interpolation ) {
00667     return (unsigned char *)data->disparity_image_hires.data;
00668   } else {
00669     return data->disparity_image_lores.data;
00670   }
00671 }
00672 
00673 
00674 size_t
00675 TriclopsStereoProcessor::disparity_buffer_size() const
00676 {
00677   if ( data->enable_subpixel_interpolation ) {
00678     return _width * _height * 2;
00679   } else {
00680     return _width * _height;
00681   }
00682 }
00683 
00684 
00685 unsigned char *
00686 TriclopsStereoProcessor::yuv_buffer()
00687 {
00688   return buffer_yuv_right;
00689 }
00690 
00691 
00692 unsigned char *
00693 TriclopsStereoProcessor::auxiliary_yuv_buffer()
00694 {
00695   return buffer_yuv_left;
00696 }
00697 
00698 
00699 /** Get Triclops context.
00700  * This retrieves calibration information from the camera and stores it into a
00701  * temporary file. With this file the Triclops context is initialized. Afterwards
00702  * the temporary file is removed.
00703  */
00704 void
00705 TriclopsStereoProcessor::get_triclops_context_from_camera()
00706 {
00707   char *tmpname = (char *)malloc(strlen("triclops_cal_XXXXXX") + 1);
00708   strcpy(tmpname, "triclops_cal_XXXXXX");
00709   char *tmpfile = mktemp(tmpname);
00710   bb2->write_triclops_config_from_camera_to_file(tmpfile);
00711 
00712   data->err = triclopsGetDefaultContextFromFile(&(data->triclops), tmpfile);
00713   if ( data->err != TriclopsErrorOk ) {
00714     free(tmpfile);
00715     throw Exception("Fetching Triclops context from camera failed");
00716   }
00717   unlink(tmpfile);
00718   free(tmpfile);
00719 }
00720 
00721 
00722 /** Deinterlace green buffer.
00723  * Method used in stereo processing. Following the PTGrey example, seems useless
00724  * if we have YUV planar and thus grey images anyway.
00725  * @param src source buffer
00726  * @param dest destination buffer
00727  * @param width width of the image
00728  * @param height height of the image
00729  */
00730 void
00731 TriclopsStereoProcessor::deinterlace_green( unsigned char* src, 
00732                                      unsigned char* dest, 
00733                                      unsigned int width, 
00734                                      unsigned int height)
00735 {
00736   register int i = (width*height)-2;
00737   register int g = ((width*height)/3)-1;
00738 
00739   while (i >= 0) {
00740     dest[g--] = src[i-=3];
00741   }
00742 }
00743 
00744 
00745 
00746 
00747 /** Get camera-relative coordinates of a point.
00748  * Use this method to get the coordinates in the camera coordinate system of a given
00749  * point in the image. It may not be possible to provide such a coordinate if no valid
00750  * disparity information could be calculated for the given point.
00751  * @param px x coordinate in image
00752  * @param py y coordinate in image
00753  * @param x contains the x coordinate in the camera-relative coordinate system upon
00754  * successful return
00755  * @param y contains the y coordinate in the camera-relative coordinate system upon
00756  * successful return
00757  * @param z contains the z coordinate in the camera-relative coordinate system upon
00758  * successful return
00759  * @return true, if valid information could be retrieved. In that case (x, y, z) is filled
00760  * with the coordinates, false otherwise (x, y, and z are not modified).
00761  */
00762 bool
00763 TriclopsStereoProcessor::get_xyz(unsigned int px, unsigned int py, float *x, float *y, float *z)
00764 {
00765   if ( data->enable_subpixel_interpolation ) {
00766     unsigned short disparity = data->disparity_image_hires.data[_width * py + px];
00767     if ( disparity < 0xFF00 ) {
00768       triclopsRCD16ToXYZ(data->triclops, py, px, disparity, x, y, z);
00769       return true;
00770     }
00771   } else {
00772     unsigned char disparity = data->disparity_image_lores.data[_width * py + px];
00773     if ( disparity != 0 ) {
00774       triclopsRCD8ToXYZ(data->triclops, py, px, disparity, x, y, z);
00775       return true;
00776     }
00777   }
00778 
00779   return false;
00780 }
00781 
00782 
00783 /** Get transformed coordinates of a point.
00784  * Use this method to get the coordinates in the transformed coordinate system of a given
00785  * point in the image. It may not be possible to provide such a coordinate if no valid
00786  * disparity information could be calculated for the given point.
00787  * @param px x coordinate in image
00788  * @param py y coordinate in image
00789  * @param x contains the x coordinate in the camera-relative coordinate system upon
00790  * successful return
00791  * @param y contains the y coordinate in the camera-relative coordinate system upon
00792  * successful return
00793  * @param z contains the z coordinate in the camera-relative coordinate system upon
00794  * successful return
00795  * @return true, if valid information could be retrieved. In that case (x, y, z) is filled
00796  * with the coordinates, false otherwise (x, y, and z are not modified).
00797  */
00798 bool
00799 TriclopsStereoProcessor::get_world_xyz(unsigned int px, unsigned int py, float *x, float *y, float *z)
00800 {
00801   float cam_angle = deg2rad(57);
00802   float trans_x = -0.1;
00803   float trans_y =  0.05;
00804   float trans_z = -0.78;
00805   float tx, ty, tz;
00806   if ( get_xyz(px, py, &tx, &ty, &tz) ) {
00807     /* transform around x axis
00808     *x = tx;
00809     *y = cos(cam_angle) * ty  + sin(cam_angle) * tz;
00810     *z = -sin(cam_angle) * ty + cos(cam_angle) * tz;
00811     */
00812     float x1, y1, z1, x2, y2, z2, x3, y3, z3;
00813     x1 = tx;
00814     y1 = cos(cam_angle) * ty  + sin(cam_angle) * tz;
00815     z1 = -sin(cam_angle) * ty + cos(cam_angle) * tz;
00816     // cout << "Transform 1: (" << tx << "," << ty << "," << tz << ") -> (" << x1 << "," << y1 << "," << z1 << ")" << endl;
00817     // *x = y1;
00818     // *y =  cos(cam_angle) * x1 + sin(cam_angle) * z1;
00819     // *z = -sin(cam_angle) * x1 + cos(cam_angle) * z1;
00820     x2 = y1;
00821     y2 = x1;
00822     z2 = z1;
00823     // cout << "Transform 2: (" << x1 << "," << y1 << "," << z1 << ") -> (" << x2 << "," << y2 << "," << z2 << ")" << endl;
00824 
00825     x3 = z2;
00826     y3 = y2;
00827     z3 = x2;
00828     // cout << "Transform 3: (" << x2 << "," << y2 << "," << z2 << ") -> (" << x3 << "," << y3 << "," << z3 << ")" << endl;
00829 
00830     *x = x3 + trans_x;
00831     *y = y3 + trans_y;
00832     *z = z3 + trans_z; 
00833 
00834     // cout << "Transform 4: (" << x3 << "," << y3 << "," << z3 << ") -> (" << *x << "," << *y << "," << *z << ")" << endl;
00835 
00836     /*
00837     *x = ty + trans_x;
00838     *y = -sin(cam_angle) * tx + cos(cam_angle) * tz + trans_y;
00839     *z = cos(cam_angle) * tx + sin(cam_angle) * tz + trans_z;
00840     */
00841     return true;
00842   } else {
00843     return false;
00844   }
00845 }
00846 
00847 
00848 /** Generate rectification LUT.
00849  * This will generate a lookup table that can be used for fast rectification of
00850  * an image. The lut will be of the dimensions of the currently specified image
00851  * (either given to the offline context file constructor or as defined by the supplied
00852  * BB2 camera). The file will use RectificationFile to write two RectificationLookupTable
00853  * entities, one for each image.
00854  * @param lut_file name of the file to write to. The file will be created if
00855  * it does not exist and truncated otherwise. The directory where the file has to
00856  * be stored has to exist.
00857  */
00858 void
00859 TriclopsStereoProcessor::generate_rectification_lut(const char *lut_file)
00860 {
00861   uint64_t guid = 0;
00862   const char *model;
00863 
00864   if ( bb2 ) {
00865     guid  = bb2->guid();
00866     model = bb2->model();
00867   } else {
00868     int serial_no;
00869     triclopsGetSerialNumber(data->triclops, &serial_no);
00870     guid = 0xFFFFFFFF;
00871     guid <<= 32;
00872     guid |= serial_no;
00873 
00874     model = "Bumblebee2";
00875   }
00876 
00877   RectificationInfoFile *rif = new RectificationInfoFile(guid, model);
00878 
00879   RectificationLutInfoBlock *lib_left  = new RectificationLutInfoBlock(_width, _height,
00880                                                                        FIREVISION_RECTINFO_CAMERA_LEFT);
00881   RectificationLutInfoBlock *lib_right = new RectificationLutInfoBlock(_width, _height,
00882                                                                        FIREVISION_RECTINFO_CAMERA_RIGHT);
00883 
00884   register float row, col;
00885   for (unsigned int h = 0; h < _height; ++h) {
00886     for (unsigned int w = 0; w < _width; ++w) {
00887       if ( triclopsUnrectifyPixel(data->triclops, TriCam_LEFT, h, w, &row, &col) != TriclopsErrorOk ) {
00888         throw Exception("Failed to get unrectified position from Triclops SDK");
00889       }
00890       lib_left->set_mapping(w, h, (int)roundf(col), (int)roundf(row));
00891 
00892       if ( triclopsUnrectifyPixel(data->triclops, TriCam_RIGHT, h, w, &row, &col) != TriclopsErrorOk ) {
00893         throw Exception("Failed to get unrectified position from Triclops SDK");
00894       }
00895       lib_right->set_mapping(w, h, (int)roundf(col), (int)roundf(row));
00896     }
00897   }
00898 
00899   rif->add_rectinfo_block(lib_left);
00900   rif->add_rectinfo_block(lib_right);
00901 
00902   rif->write(lut_file);
00903 
00904   delete rif;
00905 }
00906 
00907 
00908 /** Verify rectification LUT.
00909  * @param lut_file Rectification LUT to verify
00910  * @return true if the LUT matches the current camera/context file, false otherwise.
00911  */
00912 bool
00913 TriclopsStereoProcessor::verify_rectification_lut(const char *lut_file)
00914 {
00915   RectificationInfoFile *rif = new RectificationInfoFile();
00916   rif->read(lut_file);
00917 
00918   if ( bb2 ) {
00919     if ( ! bb2->verify_guid(rif->guid()) ) {
00920       return false;
00921     }
00922   } else {
00923     int serial_no;
00924     uint64_t guid = 0;
00925     triclopsGetSerialNumber(data->triclops, &serial_no);
00926     guid = 0xFFFFFFFF;
00927     guid <<= 32;
00928     guid |= serial_no;
00929 
00930     if ( rif->guid() != guid ) {
00931       return false;
00932     }
00933   }
00934 
00935   if ( rif->num_blocks() != 2 ) {
00936     printf("Insufficient blocks, we only have %zu\n", rif->num_blocks());
00937     return false;
00938   }
00939 
00940   bool left_ok = false;
00941   bool right_ok = false;
00942 
00943   RectificationInfoFile::RectInfoBlockVector *blocks = rif->rectinfo_blocks();
00944   printf("We have %zu blocks\n", blocks->size());
00945   RectificationInfoFile::RectInfoBlockVector::const_iterator i;
00946   for (i = blocks->begin(); (i != blocks->end() && ! (left_ok && right_ok)); ++i) {
00947     printf("Veryfying block\n");
00948     RectificationInfoBlock *rib = *i;
00949 
00950     if ( (rib->camera() != FIREVISION_RECTINFO_CAMERA_LEFT) &&
00951          (rib->camera() != FIREVISION_RECTINFO_CAMERA_RIGHT) ) {
00952       continue;
00953     }
00954 
00955     if ( rib->type() == FIREVISION_RECTINFO_TYPE_LUT_16x16 ) {
00956       RectificationLutInfoBlock *rlib = dynamic_cast<RectificationLutInfoBlock *>(rib);
00957       if ( rlib == NULL ) {
00958         continue;
00959       }
00960 
00961       TriclopsCamera cam;
00962       if ( rib->camera() == FIREVISION_RECTINFO_CAMERA_LEFT ) {
00963         cam = TriCam_LEFT;
00964         if ( left_ok ) continue;
00965       } else {
00966         cam = TriCam_RIGHT;
00967         if ( right_ok ) continue;
00968       }
00969 
00970       register float row, col;
00971       register uint16_t rx, ry;
00972       bool lut_ok = true;
00973       for (unsigned int h = 0; (h < _height) && lut_ok; ++h) {
00974         for (unsigned int w = 0; w < _width; ++w) {
00975           if ( triclopsUnrectifyPixel(data->triclops, cam, h, w, &row, &col) != TriclopsErrorOk ) {
00976             throw Exception("Failed to get unrectified position from Triclops SDK");
00977           }
00978           rlib->mapping(w, h, &rx, &ry);
00979           if ( (rx != (int)roundf(col)) || (ry != (int)roundf(row)) ) {
00980             printf("Value at (%x,%u) not ok\n", rx, ry);
00981             lut_ok = false;
00982             break;
00983           }
00984         }
00985       }
00986       
00987       if ( lut_ok ) {
00988         if ( rib->camera() == FIREVISION_RECTINFO_CAMERA_LEFT ) {
00989           left_ok = true;
00990         } else {
00991           right_ok = true;
00992         }
00993       }
00994     }
00995   }
00996   delete blocks;
00997 
00998   delete rif;
00999   return (left_ok && right_ok);
01000 }
01001 
01002 /**Calculates all three cartesian coordinates of the entire disparity map
01003  * The values transformed are given by the window (specified by hoff, voff, width and height). settings contains all further information needed (in that order): the angle of the camera, the position of the camera relativ to the robot: x, y, z and the maximum distance of points allowed. Points filtered out by distance are marked as NaN in all three coordinates. Unknown Regions are marked the same way with INFINITY.
01004  * @param buffer buffer for the coordinates, 1st index 0: x; 1: y; 2:z, 2nd index denotes the lines, 3rd index denotes the columns
01005  * @param hoff horizontal offset of the window
01006  * @param voff vertical offset of the window
01007  * @param width width of the window
01008  * @param height height of the window
01009  * @param settings a vector of settings in floating point format (angle,position of camera x, y, z, maximum distance of points)
01010  */
01011 void
01012 TriclopsStereoProcessor::getall_world_xyz(float ***buffer, int hoff, int voff, int width, int height, float *settings){
01013 
01014   float fx,fy,fz,fed,rho;
01015   int   displine, /*zline,*/ px, py;
01016 
01017   float **x = buffer[0], **y = buffer[1], **z = buffer[2];
01018 
01019   const float dnc       = NAN;
01020   const float ur        = INFINITY;
01021   const float cos_angle = cos(deg2rad(settings[0]));
01022   const float sin_angle = sin(deg2rad(settings[0]));
01023   const float trans_x   = settings[1];
01024   const float trans_y   = settings[2];
01025   const float trans_z   = settings[3];
01026   const float mqd       = settings[4]*settings[4];
01027 
01028   if( data->enable_subpixel_interpolation ){
01029     unsigned short *disp = data->disparity_image_hires.data;
01030 
01031     for(py = 0; py < height; py++){
01032       displine = (py + voff) * _width;
01033       //zline = py * width;
01034       for(px = 0; px < width; px++){
01035         if( disp[displine+px+hoff] >= 0xFF00 ){
01036           z[py][px] = x[py][px] = y[py][px] = ur;
01037         }
01038         else{
01039           triclopsRCD16ToXYZ(data->triclops,py+voff,px+hoff,disp[displine+px+hoff],&fx,&fy,&fz);
01040           fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
01041           fz  = z[py][px] =  cos_angle * fy + sin_angle * fz + trans_z;
01042           fy  = y[py][px] =              fx + trans_y;
01043           fx  = fed;
01044           if(fz > 0.0f){
01045             rho = trans_z / ( fz - trans_z );
01046             x[py][px] = fx = fx - rho * ( fx - trans_x );
01047             y[py][px] = fy = fy - rho * ( fy - trans_y );
01048           }
01049           fed = fx*fx + fy*fy;
01050           if(fed > mqd){
01051             z[py][px] = x[py][px] = y[py][px] = dnc;
01052           }
01053         }
01054       }
01055     }
01056   }
01057   else{
01058     unsigned char *disp = data->disparity_image_lores.data;
01059 
01060     for(py = 0; py < height; py++){
01061       displine = (py + voff) * _width;
01062       //zline = py * width;
01063       for(px = 0; px < width; px++){
01064         if( disp[displine+px+hoff] == 0 ){
01065           z[py][px] = x[py][px] = y[py][px] = ur;
01066         }
01067         else{
01068           triclopsRCD8ToXYZ(data->triclops,py+voff,px+hoff,disp[displine+px+hoff],&fx,&fy,&fz);
01069           fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
01070           fz  = z[py][px] =  cos_angle * fy + sin_angle * fz + trans_z;
01071           fy  = y[py][px] =              fx + trans_y;
01072           fx  = fed;
01073           if(fz > 0.0f){
01074             rho = trans_z / ( fz - trans_z );
01075             x[py][px] = fx = fx - rho * ( fx - trans_x );
01076             y[py][px] = fy = fy - rho * ( fy - trans_y );
01077           }
01078           fed = fx*fx + fy*fy;
01079           if(fed > mqd){
01080             z[py][px] = x[py][px] = y[py][px] = dnc;
01081           }
01082         }
01083       }
01084     }
01085   }
01086 }
01087 
01088 } // end namespace firevision

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7