00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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
00048 }
00049 #endif
00050
00051
00052
00053
00054
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
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
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;
00113 }
00114
00115
00116
00117
00118
00119
00120
00121
00122
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
00145
00146
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;
00164
00165 }
00166
00167
00168 void
00169 TriclopsStereoProcessor::setup_triclops()
00170 {
00171
00172 data = new TriclopsStereoProcessorData();
00173
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
00180
00181
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
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
00236 TriclopsStereoProcessor::~TriclopsStereoProcessor()
00237 {
00238
00239
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
00262
00263
00264 void
00265 TriclopsStereoProcessor::set_raw_buffer(unsigned char *raw16_buffer)
00266 {
00267 buffer_raw16 = raw16_buffer;
00268 }
00269
00270
00271
00272
00273
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
00289
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
00300
00301
00302 void
00303 TriclopsStereoProcessor::set_edge_correlation(bool enabled)
00304 {
00305 triclopsSetEdgeCorrelation(data->triclops, enabled);
00306 }
00307
00308
00309
00310
00311
00312 void
00313 TriclopsStereoProcessor::set_lowpass(bool enabled)
00314 {
00315 triclopsSetLowpass(data->triclops, enabled);
00316 }
00317
00318
00319
00320
00321
00322
00323 void
00324 TriclopsStereoProcessor::set_disparity_range(int min, int max)
00325 {
00326 triclopsSetDisparity(data->triclops, min, max);
00327 }
00328
00329
00330
00331
00332
00333
00334
00335 void
00336 TriclopsStereoProcessor::set_edge_masksize(unsigned int mask_size)
00337 {
00338 triclopsSetEdgeMask(data->triclops, mask_size);
00339 }
00340
00341
00342
00343
00344
00345
00346 void
00347 TriclopsStereoProcessor::set_stereo_masksize(unsigned int mask_size)
00348 {
00349 triclopsSetStereoMask(data->triclops, mask_size);
00350 }
00351
00352
00353
00354
00355
00356 void
00357 TriclopsStereoProcessor::set_surface_validation(bool enabled)
00358 {
00359 triclopsSetSurfaceValidation(data->triclops, enabled);
00360 }
00361
00362
00363
00364
00365
00366 void
00367 TriclopsStereoProcessor::set_texture_validation(bool enabled)
00368 {
00369 triclopsSetTextureValidation(data->triclops, enabled);
00370 }
00371
00372
00373
00374
00375
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
00385
00386
00387 void
00388 TriclopsStereoProcessor::set_disparity_mapping(bool enabled)
00389 {
00390 triclopsSetDisparityMappingOn(data->triclops, enabled);
00391 }
00392
00393
00394
00395
00396 bool
00397 TriclopsStereoProcessor::subpixel_interpolation()
00398 {
00399 TriclopsBool on;
00400 triclopsGetSubpixelInterpolation(data->triclops, &on);
00401 return on;
00402 }
00403
00404
00405
00406
00407
00408 unsigned int
00409 TriclopsStereoProcessor::output_image_width()
00410 {
00411 return _output_image_width;
00412 }
00413
00414
00415
00416
00417
00418 unsigned int
00419 TriclopsStereoProcessor::output_image_height()
00420 {
00421 return _output_image_height;
00422 }
00423
00424
00425
00426
00427
00428 bool
00429 TriclopsStereoProcessor::edge_correlation()
00430 {
00431 TriclopsBool on;
00432 triclopsGetEdgeCorrelation(data->triclops, &on);
00433 return on;
00434 }
00435
00436
00437
00438
00439
00440 bool
00441 TriclopsStereoProcessor::lowpass()
00442 {
00443 TriclopsBool on;
00444 triclopsGetLowpass(data->triclops, &on);
00445 return on;
00446 }
00447
00448
00449
00450
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
00462
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
00474
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
00486
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
00498
00499
00500 bool
00501 TriclopsStereoProcessor::surface_validation()
00502 {
00503 TriclopsBool on;
00504 triclopsGetSurfaceValidation(data->triclops, &on);
00505 return on;
00506 }
00507
00508
00509
00510
00511
00512 bool
00513 TriclopsStereoProcessor::texture_validation()
00514 {
00515 TriclopsBool on;
00516 triclopsGetTextureValidation(data->triclops, &on);
00517 return on;
00518 }
00519
00520
00521
00522
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
00534
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
00546
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
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
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
00605 deinterlace_green( buffer_rgb, buffer_green, _width, 6 * _height );
00606
00607
00608 if ( (data->err = triclopsRectify( data->triclops, &(data->input))) != TriclopsErrorOk ) {
00609 throw Exception("Rectifying the image failed");
00610 }
00611
00612
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
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656 }
00657
00658
00659
00660
00661
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
00700
00701
00702
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
00723
00724
00725
00726
00727
00728
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
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
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
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
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
00808
00809
00810
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
00817
00818
00819
00820 x2 = y1;
00821 y2 = x1;
00822 z2 = z1;
00823
00824
00825 x3 = z2;
00826 y3 = y2;
00827 z3 = x2;
00828
00829
00830 *x = x3 + trans_x;
00831 *y = y3 + trans_y;
00832 *z = z3 + trans_z;
00833
00834
00835
00836
00837
00838
00839
00840
00841 return true;
00842 } else {
00843 return false;
00844 }
00845 }
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
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
00909
00910
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
01003
01004
01005
01006
01007
01008
01009
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, 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
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
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 }