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;
79 bb2 = dynamic_cast<Bumblebee2Camera *>(camera);
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;
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.