Fawkes API
Fawkes Development Version
|
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 <fvstereo/triclops.h> 00025 00026 #include <core/exceptions/software.h> 00027 #include <core/exceptions/system.h> 00028 #include <fvcams/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 <fvstereo/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