24 #include <core/exceptions/software.h>
25 #include <core/exceptions/system.h>
26 #include <fvcams/bumblebee2.h>
27 #include <fvstereo/triclops.h>
28 #include <fvutils/base/roi.h>
29 #include <fvutils/color/conversions.h>
30 #include <fvutils/rectification/rectfile.h>
31 #include <fvutils/rectification/rectinfo_lut_block.h>
32 #include <utils/math/angle.h>
43 namespace firevision {
50 class TriclopsStereoProcessorData
53 TriclopsContext triclops;
56 TriclopsImage rectified_image;
57 TriclopsImage16 disparity_image_hires;
58 TriclopsImage disparity_image_lores;
59 bool enable_subpixel_interpolation;
74 TriclopsStereoProcessor::TriclopsStereoProcessor(
Camera *camera)
77 buffer_deinterlaced = NULL;
83 if (!bb2->is_bumblebee2()) {
87 bb2->set_image_number(Bumblebee2Camera::RGB_IMAGE);
89 _width = bb2->pixel_width();
90 _height = bb2->pixel_height();
92 _output_image_width = _width;
93 _output_image_height = _height;
102 buffer_rgb = bb2->buffer();
103 buffer_rgb_right = buffer_rgb;
104 buffer_rgb_left = buffer_rgb + colorspace_buffer_size(RGB, _width, _height);
105 buffer_rgb_center = buffer_rgb_left;
116 TriclopsStereoProcessor::TriclopsStereoProcessor(
unsigned int width,
118 const char * context_file)
122 _output_image_width = _width;
123 _output_image_height = _height;
124 _context_file = strdup(context_file);
141 TriclopsStereoProcessor::create_buffers()
143 buffer_green = (
unsigned char *)malloc(_width * _height * 2);
144 buffer_yuv_right = malloc_buffer(YUV422_PLANAR, _width, _height);
145 buffer_yuv_left = malloc_buffer(YUV422_PLANAR, _width, _height);
148 buffer_rgb = bb2->buffer();
150 buffer_rgb = (
unsigned char *)malloc(colorspace_buffer_size(RGB, _width, _height) * 2);
151 buffer_deinterlaced = (
unsigned char *)malloc(_width * _height * 2);
153 buffer_rgb_right = buffer_rgb;
154 buffer_rgb_left = buffer_rgb + colorspace_buffer_size(RGB, _width, _height);
155 buffer_rgb_center = buffer_rgb_left;
159 TriclopsStereoProcessor::setup_triclops()
162 data =
new TriclopsStereoProcessorData();
164 data->input.inputType = TriInp_RGB;
165 data->input.nrows = _height;
166 data->input.ncols = _width;
167 data->input.rowinc = data->input.ncols;
173 data->input.u.rgb.red = buffer_green;
174 data->input.u.rgb.green = buffer_green + _width * _height;
175 data->input.u.rgb.blue = data->input.u.rgb.green;
179 get_triclops_context_from_camera();
185 if (!_context_file) {
188 "to a valid BB2 context file");
191 if (access(_context_file, F_OK | R_OK) != 0) {
195 data->err = triclopsGetDefaultContextFromFile(&(data->triclops), _context_file);
196 if (data->err != TriclopsErrorOk) {
198 throw Exception(
"Fetching Triclops context from camera failed");
203 data->enable_subpixel_interpolation =
false;
205 triclopsSetSubpixelInterpolation(data->triclops, 0);
207 triclopsSetResolutionAndPrepare(data->triclops, _height, _width, _height, _width);
209 triclopsSetEdgeCorrelation(data->triclops, 1);
210 triclopsSetLowpass(data->triclops, 1);
211 triclopsSetDisparity(data->triclops, 5, 100);
212 triclopsSetEdgeMask(data->triclops, 11);
213 triclopsSetStereoMask(data->triclops, 23);
214 triclopsSetSurfaceValidation(data->triclops, 1);
215 triclopsSetTextureValidation(data->triclops, 0);
217 triclopsSetDisparityMapping(data->triclops, 10, 85);
218 triclopsSetDisparityMappingOn(data->triclops, 1);
222 TriclopsStereoProcessor::~TriclopsStereoProcessor()
227 if (buffer_green != NULL)
229 if (buffer_yuv_right != NULL)
230 free(buffer_yuv_right);
231 if (buffer_yuv_left != NULL)
232 free(buffer_yuv_left);
233 if (_context_file != NULL)
239 if (buffer_deinterlaced)
240 free(buffer_deinterlaced);
245 buffer_yuv_right = NULL;
246 buffer_yuv_left = NULL;
256 TriclopsStereoProcessor::set_raw_buffer(
unsigned char *raw16_buffer)
258 buffer_raw16 = raw16_buffer;
266 TriclopsStereoProcessor::set_output_image_size(
unsigned int width,
unsigned int height)
268 data->err = triclopsSetResolutionAndPrepare(data->triclops, height, width, _height, _width);
270 if (data->err == TriclopsErrorOk) {
271 _output_image_width = width;
272 _output_image_height = height;
280 TriclopsStereoProcessor::set_subpixel_interpolation(
bool enabled)
282 data->enable_subpixel_interpolation = enabled;
283 triclopsSetSubpixelInterpolation(data->triclops, enabled);
290 TriclopsStereoProcessor::set_edge_correlation(
bool enabled)
292 triclopsSetEdgeCorrelation(data->triclops, enabled);
299 TriclopsStereoProcessor::set_lowpass(
bool enabled)
301 triclopsSetLowpass(data->triclops, enabled);
309 TriclopsStereoProcessor::set_disparity_range(
int min,
int max)
311 triclopsSetDisparity(data->triclops, min, max);
320 TriclopsStereoProcessor::set_edge_masksize(
unsigned int mask_size)
322 triclopsSetEdgeMask(data->triclops, mask_size);
330 TriclopsStereoProcessor::set_stereo_masksize(
unsigned int mask_size)
332 triclopsSetStereoMask(data->triclops, mask_size);
339 TriclopsStereoProcessor::set_surface_validation(
bool enabled)
341 triclopsSetSurfaceValidation(data->triclops, enabled);
348 TriclopsStereoProcessor::set_texture_validation(
bool enabled)
350 triclopsSetTextureValidation(data->triclops, enabled);
358 TriclopsStereoProcessor::set_disparity_mapping_range(
unsigned char min,
unsigned char max)
360 triclopsSetDisparityMapping(data->triclops, min, max);
367 TriclopsStereoProcessor::set_disparity_mapping(
bool enabled)
369 triclopsSetDisparityMappingOn(data->triclops, enabled);
376 TriclopsStereoProcessor::subpixel_interpolation()
379 triclopsGetSubpixelInterpolation(data->triclops, &on);
387 TriclopsStereoProcessor::output_image_width()
389 return _output_image_width;
396 TriclopsStereoProcessor::output_image_height()
398 return _output_image_height;
405 TriclopsStereoProcessor::edge_correlation()
408 triclopsGetEdgeCorrelation(data->triclops, &on);
416 TriclopsStereoProcessor::lowpass()
419 triclopsGetLowpass(data->triclops, &on);
427 TriclopsStereoProcessor::disparity_range_min()
430 triclopsGetDisparity(data->triclops, &min, &max);
438 TriclopsStereoProcessor::disparity_range_max()
441 triclopsGetDisparity(data->triclops, &min, &max);
449 TriclopsStereoProcessor::edge_masksize()
452 triclopsGetEdgeMask(data->triclops, &mask_size);
460 TriclopsStereoProcessor::stereo_masksize()
463 triclopsGetStereoMask(data->triclops, &mask_size);
471 TriclopsStereoProcessor::surface_validation()
474 triclopsGetSurfaceValidation(data->triclops, &on);
482 TriclopsStereoProcessor::texture_validation()
485 triclopsGetTextureValidation(data->triclops, &on);
493 TriclopsStereoProcessor::disparity_mapping_min()
495 unsigned char min, max;
496 triclopsGetDisparityMapping(data->triclops, &min, &max);
504 TriclopsStereoProcessor::disparity_mapping_max()
506 unsigned char min, max;
507 triclopsGetDisparityMapping(data->triclops, &min, &max);
515 TriclopsStereoProcessor::disparity_mapping()
518 triclopsGetDisparityMappingOn(data->triclops, &on);
523 TriclopsStereoProcessor::preprocess_stereo()
526 bb2->deinterlace_stereo();
529 Bumblebee2Camera::deinterlace_stereo(buffer_raw16, buffer_deinterlaced, _width, _height);
530 Bumblebee2Camera::decode_bayer(
531 buffer_deinterlaced, buffer_rgb, _width, _height, BAYER_PATTERN_BGGR);
536 TriclopsStereoProcessor::calculate_yuv(
bool both)
539 convert(RGB, YUV422_PLANAR, buffer_rgb_right, buffer_yuv_right, _width, _height);
541 convert(RGB, YUV422_PLANAR, buffer_rgb_left, buffer_yuv_left, _width, _height);
546 TriclopsStereoProcessor::calculate_disparity(
ROI *roi)
552 if (TriclopsErrorOk == triclopsGetROIs(data->triclops, &rois, &max_rois)) {
554 rois[0].col = roi->
start.
x;
555 rois[0].row = roi->
start.
y;
556 rois[0].ncols = roi->
width;
557 rois[0].nrows = roi->
height;
559 triclopsSetNumberOfROIs(data->triclops, 1);
561 triclopsSetNumberOfROIs(data->triclops, 0);
564 triclopsSetNumberOfROIs(data->triclops, 0);
568 deinterlace_green(buffer_rgb, buffer_green, _width, 6 * _height);
571 if ((data->err = triclopsRectify(data->triclops, &(data->input))) != TriclopsErrorOk) {
572 throw Exception(
"Rectifying the image failed");
576 if ((data->err = triclopsStereo(data->triclops)) != TriclopsErrorOk) {
577 throw Exception(
"Calculating the disparity image failed");
580 triclopsGetImage(data->triclops, TriImg_RECTIFIED, TriCam_REFERENCE, &(data->rectified_image));
582 if (data->enable_subpixel_interpolation) {
583 triclopsGetImage16(data->triclops,
586 &(data->disparity_image_hires));
588 triclopsGetImage(data->triclops,
591 &(data->disparity_image_lores));
632 TriclopsStereoProcessor::disparity_buffer()
634 if (data->enable_subpixel_interpolation) {
635 return (
unsigned char *)data->disparity_image_hires.data;
637 return data->disparity_image_lores.data;
642 TriclopsStereoProcessor::disparity_buffer_size()
const
644 if (data->enable_subpixel_interpolation) {
645 return _width * _height * 2;
647 return _width * _height;
652 TriclopsStereoProcessor::yuv_buffer_left()
654 return buffer_yuv_right;
658 TriclopsStereoProcessor::yuv_buffer_right()
660 return buffer_yuv_left;
669 TriclopsStereoProcessor::get_triclops_context_from_camera()
671 char *tmpname = (
char *)malloc(strlen(
"triclops_cal_XXXXXX") + 1);
672 strcpy(tmpname,
"triclops_cal_XXXXXX");
673 char *tmpfile = mktemp(tmpname);
674 bb2->write_triclops_config_from_camera_to_file(tmpfile);
676 data->err = triclopsGetDefaultContextFromFile(&(data->triclops), tmpfile);
677 if (data->err != TriclopsErrorOk) {
679 throw Exception(
"Fetching Triclops context from camera failed");
694 TriclopsStereoProcessor::deinterlace_green(
unsigned char *src,
699 register int i = (width * height) - 2;
700 register int g = ((width * height) / 3) - 1;
703 dest[g--] = src[i -= 3];
723 TriclopsStereoProcessor::get_xyz(
unsigned int px,
unsigned int py,
float *x,
float *y,
float *z)
725 if (data->enable_subpixel_interpolation) {
726 unsigned short disparity = data->disparity_image_hires.data[_width * py + px];
727 if (disparity < 0xFF00) {
728 triclopsRCD16ToXYZ(data->triclops, py, px, disparity, x, y, z);
732 unsigned char disparity = data->disparity_image_lores.data[_width * py + px];
733 if (disparity != 0) {
734 triclopsRCD8ToXYZ(data->triclops, py, px, disparity, x, y, z);
758 TriclopsStereoProcessor::get_world_xyz(
unsigned int px,
765 float trans_x = -0.1;
766 float trans_y = 0.05;
767 float trans_z = -0.78;
769 if (get_xyz(px, py, &tx, &ty, &tz)) {
775 float x1, y1, z1, x2, y2, z2, x3, y3, z3;
777 y1 = cos(cam_angle) * ty + sin(cam_angle) * tz;
778 z1 = -sin(cam_angle) * ty + cos(cam_angle) * tz;
821 TriclopsStereoProcessor::generate_rectification_lut(
const char *lut_file)
828 model = bb2->model();
831 triclopsGetSerialNumber(data->triclops, &serial_no);
836 model =
"Bumblebee2";
846 register float row, col;
847 for (
unsigned int h = 0; h < _height; ++h) {
848 for (
unsigned int w = 0; w < _width; ++w) {
849 if (triclopsUnrectifyPixel(data->triclops, TriCam_LEFT, h, w, &row, &col)
850 != TriclopsErrorOk) {
851 throw Exception(
"Failed to get unrectified position from Triclops SDK");
853 lib_left->
set_mapping(w, h, (
int)roundf(col), (
int)roundf(row));
855 if (triclopsUnrectifyPixel(data->triclops, TriCam_RIGHT, h, w, &row, &col)
856 != TriclopsErrorOk) {
857 throw Exception(
"Failed to get unrectified position from Triclops SDK");
859 lib_right->
set_mapping(w, h, (
int)roundf(col), (
int)roundf(row));
866 rif->
write(lut_file);
876 TriclopsStereoProcessor::verify_rectification_lut(
const char *lut_file)
882 if (!bb2->verify_guid(rif->
guid())) {
888 triclopsGetSerialNumber(data->triclops, &serial_no);
893 if (rif->
guid() != guid) {
899 printf(
"Insufficient blocks, we only have %zu\n", rif->
num_blocks());
903 bool left_ok =
false;
904 bool right_ok =
false;
907 printf(
"We have %zu blocks\n", blocks->size());
908 RectificationInfoFile::RectInfoBlockVector::const_iterator i;
909 for (i = blocks->begin(); (i != blocks->end() && !(left_ok && right_ok)); ++i) {
910 printf(
"Veryfying block\n");
913 if ((rib->
camera() != FIREVISION_RECTINFO_CAMERA_LEFT)
914 && (rib->
camera() != FIREVISION_RECTINFO_CAMERA_RIGHT)) {
918 if (rib->
type() == FIREVISION_RECTINFO_TYPE_LUT_16x16) {
925 if (rib->
camera() == FIREVISION_RECTINFO_CAMERA_LEFT) {
935 register float row, col;
936 register uint16_t rx, ry;
938 for (
unsigned int h = 0; (h < _height) && lut_ok; ++h) {
939 for (
unsigned int w = 0; w < _width; ++w) {
940 if (triclopsUnrectifyPixel(data->triclops, cam, h, w, &row, &col) != TriclopsErrorOk) {
941 throw Exception(
"Failed to get unrectified position from Triclops SDK");
944 if ((rx != (
int)roundf(col)) || (ry != (
int)roundf(row))) {
945 printf(
"Value at (%x,%u) not ok\n", rx, ry);
953 if (rib->
camera() == FIREVISION_RECTINFO_CAMERA_LEFT) {
964 return (left_ok && right_ok);
977 TriclopsStereoProcessor::getall_world_xyz(
float ***buffer,
984 float fx, fy, fz, fed, rho;
985 int displine, px, py;
987 float **x = buffer[0], **y = buffer[1], **z = buffer[2];
989 const float dnc = NAN;
990 const float ur = INFINITY;
991 const float cos_angle = cos(
deg2rad(settings[0]));
992 const float sin_angle = sin(
deg2rad(settings[0]));
993 const float trans_x = settings[1];
994 const float trans_y = settings[2];
995 const float trans_z = settings[3];
996 const float mqd = settings[4] * settings[4];
998 if (data->enable_subpixel_interpolation) {
999 unsigned short *disp = data->disparity_image_hires.data;
1001 for (py = 0; py < height; py++) {
1002 displine = (py + voff) * _width;
1004 for (px = 0; px < width; px++) {
1005 if (disp[displine + px + hoff] >= 0xFF00) {
1006 z[py][px] = x[py][px] = y[py][px] = ur;
1009 data->triclops, py + voff, px + hoff, disp[displine + px + hoff], &fx, &fy, &fz);
1010 fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
1011 fz = z[py][px] = cos_angle * fy + sin_angle * fz + trans_z;
1012 fy = y[py][px] = fx + trans_y;
1015 rho = trans_z / (fz - trans_z);
1016 x[py][px] = fx = fx - rho * (fx - trans_x);
1017 y[py][px] = fy = fy - rho * (fy - trans_y);
1019 fed = fx * fx + fy * fy;
1021 z[py][px] = x[py][px] = y[py][px] = dnc;
1027 unsigned char *disp = data->disparity_image_lores.data;
1029 for (py = 0; py < height; py++) {
1030 displine = (py + voff) * _width;
1032 for (px = 0; px < width; px++) {
1033 if (disp[displine + px + hoff] == 0) {
1034 z[py][px] = x[py][px] = y[py][px] = ur;
1037 data->triclops, py + voff, px + hoff, disp[displine + px + hoff], &fx, &fy, &fz);
1038 fed = x[py][px] = -sin_angle * fy + cos_angle * fz + trans_x;
1039 fz = z[py][px] = cos_angle * fy + sin_angle * fz + trans_z;
1040 fy = y[py][px] = fx + trans_y;
1043 rho = trans_z / (fz - trans_z);
1044 x[py][px] = fx = fx - rho * (fx - trans_x);
1045 y[py][px] = fy = fy - rho * (fy - trans_y);
1047 fed = fx * fx + fy * fy;
1049 z[py][px] = x[py][px] = y[py][px] = dnc;