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_left()
688 return buffer_yuv_right;
693 TriclopsStereoProcessor::yuv_buffer_right()
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.
unsigned int type() const
Get block type.
Camera interface for image aquiring devices in FireVision.
Fawkes library namespace.
Recitification Lookup Table Block.
fawkes::upoint_t start
ROI start.
unsigned int y
y coordinate
unsigned int x
x coordinate
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.
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.
uint8_t camera() const
Get block camera identifier.
RectInfoBlockVector * rectinfo_blocks()
Get all rectification info blocks.