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