24 #include <fvstereo/triclops.h> 26 #include <core/exceptions/software.h> 27 #include <core/exceptions/system.h> 28 #include <fvcams/bumblebee2.h> 29 #include <fvutils/base/roi.h> 30 #include <utils/math/angle.h> 31 #include <fvutils/color/conversions.h> 32 #include <fvutils/rectification/rectfile.h> 33 #include <fvutils/rectification/rectinfo_lut_block.h> 56 class TriclopsStereoProcessorData
59 TriclopsContext triclops;
62 TriclopsImage rectified_image;
63 TriclopsImage16 disparity_image_hires;
64 TriclopsImage disparity_image_lores;
65 bool enable_subpixel_interpolation;
81 TriclopsStereoProcessor::TriclopsStereoProcessor(
Camera *camera)
84 buffer_deinterlaced = NULL;
90 if ( ! bb2->is_bumblebee2() ) {
94 bb2->set_image_number(Bumblebee2Camera::RGB_IMAGE);
96 _width = bb2->pixel_width();
97 _height = bb2->pixel_height();
99 _output_image_width = _width;
100 _output_image_height = _height;
109 buffer_rgb = bb2->buffer();
110 buffer_rgb_right = buffer_rgb;
111 buffer_rgb_left = buffer_rgb + colorspace_buffer_size(RGB, _width, _height);
112 buffer_rgb_center = buffer_rgb_left;
124 TriclopsStereoProcessor::TriclopsStereoProcessor(
unsigned int width,
unsigned int height,
125 const char *context_file)
129 _output_image_width = _width;
130 _output_image_height = _height;
131 _context_file = strdup(context_file);
149 TriclopsStereoProcessor::create_buffers()
151 buffer_green = (
unsigned char *)malloc(_width * _height * 2);
152 buffer_yuv_right = malloc_buffer(YUV422_PLANAR, _width, _height);
153 buffer_yuv_left = malloc_buffer(YUV422_PLANAR, _width, _height);
156 buffer_rgb = bb2->buffer();
158 buffer_rgb = (
unsigned char *)malloc(colorspace_buffer_size(RGB, _width, _height) * 2);
159 buffer_deinterlaced = (
unsigned char *)malloc(_width * _height * 2);
161 buffer_rgb_right = buffer_rgb;
162 buffer_rgb_left = buffer_rgb + colorspace_buffer_size(RGB, _width, _height);
163 buffer_rgb_center = buffer_rgb_left;
169 TriclopsStereoProcessor::setup_triclops()
172 data =
new TriclopsStereoProcessorData();
174 data->input.inputType = TriInp_RGB;
175 data->input.nrows = _height;
176 data->input.ncols = _width;
177 data->input.rowinc = data->input.ncols;
183 data->input.u.rgb.red = buffer_green;
184 data->input.u.rgb.green = buffer_green + _width * _height;
185 data->input.u.rgb.blue = data->input.u.rgb.green;
189 get_triclops_context_from_camera();
196 if ( ! _context_file ) {
199 "to a valid BB2 context file");
202 if ( access(_context_file, F_OK | R_OK) != 0 ) {
206 data->err = triclopsGetDefaultContextFromFile(&(data->triclops), _context_file);
207 if ( data->err != TriclopsErrorOk ) {
209 throw Exception(
"Fetching Triclops context from camera failed");
214 data->enable_subpixel_interpolation =
false;
216 triclopsSetSubpixelInterpolation( data->triclops, 0);
218 triclopsSetResolutionAndPrepare( data->triclops,
222 triclopsSetEdgeCorrelation( data->triclops, 1 );
223 triclopsSetLowpass( data->triclops, 1 );
224 triclopsSetDisparity( data->triclops, 5, 100);
225 triclopsSetEdgeMask(data->triclops, 11);
226 triclopsSetStereoMask(data->triclops, 23);
227 triclopsSetSurfaceValidation(data->triclops, 1);
228 triclopsSetTextureValidation(data->triclops, 0);
230 triclopsSetDisparityMapping( data->triclops, 10, 85 );
231 triclopsSetDisparityMappingOn( data->triclops, 1 );
236 TriclopsStereoProcessor::~TriclopsStereoProcessor()
241 if ( buffer_green != NULL ) free(buffer_green);
242 if ( buffer_yuv_right != NULL ) free(buffer_yuv_right);
243 if ( buffer_yuv_left != NULL ) free(buffer_yuv_left);
244 if ( _context_file != NULL) free(_context_file);
247 if ( buffer_rgb) free(buffer_rgb);
248 if ( buffer_deinterlaced) free(buffer_deinterlaced);
253 buffer_yuv_right = NULL;
254 buffer_yuv_left = NULL;
265 TriclopsStereoProcessor::set_raw_buffer(
unsigned char *raw16_buffer)
267 buffer_raw16 = raw16_buffer;
276 TriclopsStereoProcessor::set_output_image_size(
unsigned int width,
unsigned int height)
278 data->err = triclopsSetResolutionAndPrepare(data->triclops, height, width, _height, _width);
280 if ( data->err == TriclopsErrorOk )
282 _output_image_width = width;
283 _output_image_height = height;
292 TriclopsStereoProcessor::set_subpixel_interpolation(
bool enabled)
294 data->enable_subpixel_interpolation = enabled;
295 triclopsSetSubpixelInterpolation(data->triclops, enabled);
303 TriclopsStereoProcessor::set_edge_correlation(
bool enabled)
305 triclopsSetEdgeCorrelation(data->triclops, enabled);
313 TriclopsStereoProcessor::set_lowpass(
bool enabled)
315 triclopsSetLowpass(data->triclops, enabled);
324 TriclopsStereoProcessor::set_disparity_range(
int min,
int max)
326 triclopsSetDisparity(data->triclops, min, max);
336 TriclopsStereoProcessor::set_edge_masksize(
unsigned int mask_size)
338 triclopsSetEdgeMask(data->triclops, mask_size);
347 TriclopsStereoProcessor::set_stereo_masksize(
unsigned int mask_size)
349 triclopsSetStereoMask(data->triclops, mask_size);
357 TriclopsStereoProcessor::set_surface_validation(
bool enabled)
359 triclopsSetSurfaceValidation(data->triclops, enabled);
367 TriclopsStereoProcessor::set_texture_validation(
bool enabled)
369 triclopsSetTextureValidation(data->triclops, enabled);
378 TriclopsStereoProcessor::set_disparity_mapping_range(
unsigned char min,
unsigned char max)
380 triclopsSetDisparityMapping(data->triclops, min, max);
388 TriclopsStereoProcessor::set_disparity_mapping(
bool enabled)
390 triclopsSetDisparityMappingOn(data->triclops, enabled);
397 TriclopsStereoProcessor::subpixel_interpolation()
400 triclopsGetSubpixelInterpolation(data->triclops, &on);
409 TriclopsStereoProcessor::output_image_width()
411 return _output_image_width;
419 TriclopsStereoProcessor::output_image_height()
421 return _output_image_height;
429 TriclopsStereoProcessor::edge_correlation()
432 triclopsGetEdgeCorrelation(data->triclops, &on);
441 TriclopsStereoProcessor::lowpass()
444 triclopsGetLowpass(data->triclops, &on);
453 TriclopsStereoProcessor::disparity_range_min()
456 triclopsGetDisparity( data->triclops, &min, &max );
465 TriclopsStereoProcessor::disparity_range_max()
468 triclopsGetDisparity( data->triclops, &min, &max );
477 TriclopsStereoProcessor::edge_masksize()
480 triclopsGetEdgeMask( data->triclops, &mask_size );
489 TriclopsStereoProcessor::stereo_masksize()
492 triclopsGetStereoMask( data->triclops, &mask_size );
501 TriclopsStereoProcessor::surface_validation()
504 triclopsGetSurfaceValidation(data->triclops, &on);
513 TriclopsStereoProcessor::texture_validation()
516 triclopsGetTextureValidation(data->triclops, &on);
525 TriclopsStereoProcessor::disparity_mapping_min()
527 unsigned char min, max;
528 triclopsGetDisparityMapping( data->triclops, &min, &max );
537 TriclopsStereoProcessor::disparity_mapping_max()
539 unsigned char min, max;
540 triclopsGetDisparityMapping( data->triclops, &min, &max );
549 TriclopsStereoProcessor::disparity_mapping()
552 triclopsGetDisparityMappingOn(data->triclops, &on);
558 TriclopsStereoProcessor::preprocess_stereo()
561 bb2->deinterlace_stereo();
564 Bumblebee2Camera::deinterlace_stereo(buffer_raw16, buffer_deinterlaced,
566 Bumblebee2Camera::decode_bayer(buffer_deinterlaced, buffer_rgb,
567 _width, _height, BAYER_PATTERN_BGGR);
572 TriclopsStereoProcessor::calculate_yuv(
bool both)
575 convert(RGB, YUV422_PLANAR, buffer_rgb_right, buffer_yuv_right, _width, _height);
577 convert(RGB, YUV422_PLANAR, buffer_rgb_left, buffer_yuv_left, _width, _height);
583 TriclopsStereoProcessor::calculate_disparity(
ROI *roi)
589 if ( TriclopsErrorOk == triclopsGetROIs( data->triclops, &rois, &max_rois ) ) {
591 rois[0].col = roi->
start.
x;
592 rois[0].row = roi->
start.
y;
593 rois[0].ncols = roi->
width;
594 rois[0].nrows = roi->
height;
596 triclopsSetNumberOfROIs( data->triclops, 1 );
598 triclopsSetNumberOfROIs( data->triclops, 0 );
601 triclopsSetNumberOfROIs( data->triclops, 0 );
605 deinterlace_green( buffer_rgb, buffer_green, _width, 6 * _height );
608 if ( (data->err = triclopsRectify( data->triclops, &(data->input))) != TriclopsErrorOk ) {
609 throw Exception(
"Rectifying the image failed");
613 if ( (data->err = triclopsStereo( data->triclops )) != TriclopsErrorOk ) {
614 throw Exception(
"Calculating the disparity image failed");
617 triclopsGetImage(data->triclops, TriImg_RECTIFIED, TriCam_REFERENCE, &(data->rectified_image));
619 if ( data->enable_subpixel_interpolation ) {
620 triclopsGetImage16( data->triclops, TriImg16_DISPARITY, TriCam_REFERENCE, &(data->disparity_image_hires) );
622 triclopsGetImage( data->triclops, TriImg_DISPARITY, TriCam_REFERENCE, &(data->disparity_image_lores) );
664 TriclopsStereoProcessor::disparity_buffer()
666 if ( data->enable_subpixel_interpolation ) {
667 return (
unsigned char *)data->disparity_image_hires.data;
669 return data->disparity_image_lores.data;
675 TriclopsStereoProcessor::disparity_buffer_size()
const 677 if ( data->enable_subpixel_interpolation ) {
678 return _width * _height * 2;
680 return _width * _height;
686 TriclopsStereoProcessor::yuv_buffer()
688 return buffer_yuv_right;
693 TriclopsStereoProcessor::auxiliary_yuv_buffer()
695 return buffer_yuv_left;
705 TriclopsStereoProcessor::get_triclops_context_from_camera()
707 char *tmpname = (
char *)malloc(strlen(
"triclops_cal_XXXXXX") + 1);
708 strcpy(tmpname,
"triclops_cal_XXXXXX");
709 char *tmpfile = mktemp(tmpname);
710 bb2->write_triclops_config_from_camera_to_file(tmpfile);
712 data->err = triclopsGetDefaultContextFromFile(&(data->triclops), tmpfile);
713 if ( data->err != TriclopsErrorOk ) {
715 throw Exception(
"Fetching Triclops context from camera failed");
731 TriclopsStereoProcessor::deinterlace_green(
unsigned char* src,
736 register int i = (width*height)-2;
737 register int g = ((width*height)/3)-1;
740 dest[g--] = src[i-=3];
763 TriclopsStereoProcessor::get_xyz(
unsigned int px,
unsigned int py,
float *x,
float *y,
float *z)
765 if ( data->enable_subpixel_interpolation ) {
766 unsigned short disparity = data->disparity_image_hires.data[_width * py + px];
767 if ( disparity < 0xFF00 ) {
768 triclopsRCD16ToXYZ(data->triclops, py, px, disparity, x, y, z);
772 unsigned char disparity = data->disparity_image_lores.data[_width * py + px];
773 if ( disparity != 0 ) {
774 triclopsRCD8ToXYZ(data->triclops, py, px, disparity, x, y, z);
799 TriclopsStereoProcessor::get_world_xyz(
unsigned int px,
unsigned int py,
float *x,
float *y,
float *z)
802 float trans_x = -0.1;
803 float trans_y = 0.05;
804 float trans_z = -0.78;
806 if ( get_xyz(px, py, &tx, &ty, &tz) ) {
812 float x1, y1, z1, x2, y2, z2, x3, y3, z3;
814 y1 = cos(cam_angle) * ty + sin(cam_angle) * tz;
815 z1 = -sin(cam_angle) * ty + cos(cam_angle) * tz;
859 TriclopsStereoProcessor::generate_rectification_lut(
const char *lut_file)
866 model = bb2->model();
869 triclopsGetSerialNumber(data->triclops, &serial_no);
874 model =
"Bumblebee2";
880 FIREVISION_RECTINFO_CAMERA_LEFT);
882 FIREVISION_RECTINFO_CAMERA_RIGHT);
884 register float row, col;
885 for (
unsigned int h = 0; h < _height; ++h) {
886 for (
unsigned int w = 0; w < _width; ++w) {
887 if ( triclopsUnrectifyPixel(data->triclops, TriCam_LEFT, h, w, &row, &col) != TriclopsErrorOk ) {
888 throw Exception(
"Failed to get unrectified position from Triclops SDK");
890 lib_left->
set_mapping(w, h, (
int)roundf(col), (
int)roundf(row));
892 if ( triclopsUnrectifyPixel(data->triclops, TriCam_RIGHT, h, w, &row, &col) != TriclopsErrorOk ) {
893 throw Exception(
"Failed to get unrectified position from Triclops SDK");
895 lib_right->
set_mapping(w, h, (
int)roundf(col), (
int)roundf(row));
902 rif->
write(lut_file);
913 TriclopsStereoProcessor::verify_rectification_lut(
const char *lut_file)
919 if ( ! bb2->verify_guid(rif->
guid()) ) {
925 triclopsGetSerialNumber(data->triclops, &serial_no);
930 if ( rif->
guid() != guid ) {
936 printf(
"Insufficient blocks, we only have %zu\n", rif->
num_blocks());
940 bool left_ok =
false;
941 bool right_ok =
false;
944 printf(
"We have %zu blocks\n", blocks->size());
945 RectificationInfoFile::RectInfoBlockVector::const_iterator i;
946 for (i = blocks->begin(); (i != blocks->end() && ! (left_ok && right_ok)); ++i) {
947 printf(
"Veryfying block\n");
950 if ( (rib->
camera() != FIREVISION_RECTINFO_CAMERA_LEFT) &&
951 (rib->
camera() != FIREVISION_RECTINFO_CAMERA_RIGHT) ) {
955 if ( rib->
type() == FIREVISION_RECTINFO_TYPE_LUT_16x16 ) {
957 if ( rlib == NULL ) {
962 if ( rib->
camera() == FIREVISION_RECTINFO_CAMERA_LEFT ) {
964 if ( left_ok )
continue;
967 if ( right_ok )
continue;
970 register float row, col;
971 register uint16_t rx, ry;
973 for (
unsigned int h = 0; (h < _height) && lut_ok; ++h) {
974 for (
unsigned int w = 0; w < _width; ++w) {
975 if ( triclopsUnrectifyPixel(data->triclops, cam, h, w, &row, &col) != TriclopsErrorOk ) {
976 throw Exception(
"Failed to get unrectified position from Triclops SDK");
979 if ( (rx != (
int)roundf(col)) || (ry != (
int)roundf(row)) ) {
980 printf(
"Value at (%x,%u) not ok\n", rx, ry);
988 if ( rib->
camera() == FIREVISION_RECTINFO_CAMERA_LEFT ) {
999 return (left_ok && right_ok);
1012 TriclopsStereoProcessor::getall_world_xyz(
float ***buffer,
int hoff,
int voff,
int width,
int height,
float *settings){
1014 float fx,fy,fz,fed,rho;
1015 int displine, px, py;
1017 float **x = buffer[0], **y = buffer[1], **z = buffer[2];
1019 const float dnc = NAN;
1020 const float ur = INFINITY;
1021 const float cos_angle = cos(
deg2rad(settings[0]));
1022 const float sin_angle = sin(
deg2rad(settings[0]));
1023 const float trans_x = settings[1];
1024 const float trans_y = settings[2];
1025 const float trans_z = settings[3];
1026 const float mqd = settings[4]*settings[4];
1028 if( data->enable_subpixel_interpolation ){
1029 unsigned short *disp = data->disparity_image_hires.data;
1031 for(py = 0; py < height; py++){
1032 displine = (py + voff) * _width;
1034 for(px = 0; px < width; px++){
1035 if( disp[displine+px+hoff] >= 0xFF00 ){
1036 z[py][px] = x[py][px] = y[py][px] = ur;
1039 triclopsRCD16ToXYZ(data->triclops,py+voff,px+hoff,disp[displine+px+hoff],&fx,&fy,&fz);
1040 fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
1041 fz = z[py][px] = cos_angle * fy + sin_angle * fz + trans_z;
1042 fy = y[py][px] = fx + trans_y;
1045 rho = trans_z / ( fz - trans_z );
1046 x[py][px] = fx = fx - rho * ( fx - trans_x );
1047 y[py][px] = fy = fy - rho * ( fy - trans_y );
1049 fed = fx*fx + fy*fy;
1051 z[py][px] = x[py][px] = y[py][px] = dnc;
1058 unsigned char *disp = data->disparity_image_lores.data;
1060 for(py = 0; py < height; py++){
1061 displine = (py + voff) * _width;
1063 for(px = 0; px < width; px++){
1064 if( disp[displine+px+hoff] == 0 ){
1065 z[py][px] = x[py][px] = y[py][px] = ur;
1068 triclopsRCD8ToXYZ(data->triclops,py+voff,px+hoff,disp[displine+px+hoff],&fx,&fy,&fz);
1069 fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
1070 fz = z[py][px] = cos_angle * fy + sin_angle * fz + trans_z;
1071 fy = y[py][px] = fx + trans_y;
1074 rho = trans_z / ( fz - trans_z );
1075 x[py][px] = fx = fx - rho * ( fx - trans_x );
1076 y[py][px] = fy = fy - rho * ( fy - trans_y );
1078 fed = fx*fx + fy*fy;
1080 z[py][px] = x[py][px] = y[py][px] = dnc;
size_t num_blocks()
Get the number of available info blocks.
File could not be opened.
Camera interface for image aquiring devices in FireVision.
Fawkes library namespace.
fawkes::point_t start
ROI start.
Recitification Lookup Table Block.
unsigned int x
x coordinate
unsigned int type() const
Get block type.
void add_rectinfo_block(RectificationInfoBlock *block)
Add a rectification info block.
unsigned int width
ROI width.
A NULL pointer was supplied where not allowed.
Vector that is used for maintaining the rectification info blocks.
uint64_t guid()
Get the GUID of camera.
virtual void write(const char *file_name)
Write file.
virtual void mapping(uint16_t x, uint16_t y, uint16_t *to_x, uint16_t *to_y)
Get mapping (to_x, to_y) for (x, y).
Base class for exceptions in Fawkes.
unsigned int y
y coordinate
uint8_t camera() const
Get block camera identifier.
virtual void read(const char *filename)
Read file.
Rectification info block.
unsigned int height
ROI height.
void set_mapping(uint16_t x, uint16_t y, uint16_t to_x, uint16_t to_y)
Set mapping.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
RectInfoBlockVector * rectinfo_blocks()
Get all rectification info blocks.