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>
50class TriclopsStereoProcessorData
53 TriclopsContext triclops;
56 TriclopsImage rectified_image;
57 TriclopsImage16 disparity_image_hires;
58 TriclopsImage disparity_image_lores;
59 bool enable_subpixel_interpolation;
77 buffer_deinterlaced = NULL;
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;
118 const char * context_file)
122 _output_image_width = _width;
123 _output_image_height = _height;
124 _context_file = strdup(context_file);
141TriclopsStereoProcessor::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;
159TriclopsStereoProcessor::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);
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;
258 buffer_raw16 = raw16_buffer;
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;
282 data->enable_subpixel_interpolation = enabled;
283 triclopsSetSubpixelInterpolation(data->triclops, enabled);
292 triclopsSetEdgeCorrelation(data->triclops, enabled);
301 triclopsSetLowpass(data->triclops, enabled);
311 triclopsSetDisparity(data->triclops, min, max);
322 triclopsSetEdgeMask(data->triclops, mask_size);
332 triclopsSetStereoMask(data->triclops, mask_size);
341 triclopsSetSurfaceValidation(data->triclops, enabled);
350 triclopsSetTextureValidation(data->triclops, enabled);
360 triclopsSetDisparityMapping(data->triclops, min, max);
369 triclopsSetDisparityMappingOn(data->triclops, enabled);
379 triclopsGetSubpixelInterpolation(data->triclops, &on);
389 return _output_image_width;
398 return _output_image_height;
408 triclopsGetEdgeCorrelation(data->triclops, &on);
419 triclopsGetLowpass(data->triclops, &on);
430 triclopsGetDisparity(data->triclops, &min, &max);
441 triclopsGetDisparity(data->triclops, &min, &max);
452 triclopsGetEdgeMask(data->triclops, &mask_size);
463 triclopsGetStereoMask(data->triclops, &mask_size);
474 triclopsGetSurfaceValidation(data->triclops, &on);
485 triclopsGetTextureValidation(data->triclops, &on);
495 unsigned char min, max;
496 triclopsGetDisparityMapping(data->triclops, &min, &max);
506 unsigned char min, max;
507 triclopsGetDisparityMapping(data->triclops, &min, &max);
518 triclopsGetDisparityMappingOn(data->triclops, &on);
531 buffer_deinterlaced, buffer_rgb, _width, _height, BAYER_PATTERN_BGGR);
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);
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));
634 if (data->enable_subpixel_interpolation) {
635 return (
unsigned char *)data->disparity_image_hires.data;
637 return data->disparity_image_lores.data;
644 if (data->enable_subpixel_interpolation) {
645 return _width * _height * 2;
647 return _width * _height;
654 return buffer_yuv_right;
660 return buffer_yuv_left;
669TriclopsStereoProcessor::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);
676 data->err = triclopsGetDefaultContextFromFile(&(data->triclops), tmpfile);
677 if (data->err != TriclopsErrorOk) {
679 throw Exception(
"Fetching Triclops context from camera failed");
694TriclopsStereoProcessor::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];
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);
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;
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);
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);
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;
File could not be opened.
Base class for exceptions in Fawkes.
A NULL pointer was supplied where not allowed.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
void decode_bayer()
Extract RGB color image from the bayer tile image.
bool is_bumblebee2()
Check if connected camera is a Bumblebee2.
static const unsigned int RGB_IMAGE
From bayer tile decoded RGB image.
virtual bool verify_guid(uint64_t ver_guid) const
Verify GUID validity.
virtual void set_image_number(unsigned int image_num)
Set image number to retrieve.
virtual unsigned char * buffer()
Get access to current image buffer.
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Camera interface for image aquiring devices in FireVision.
unsigned int type() const
Get block type.
size_t num_blocks()
Get the number of available info blocks.
virtual void write(const char *file_name)
Write file.
virtual const char * model() const
Get camera model.
virtual unsigned int pixel_width()
Width of image in pixels.
virtual unsigned int pixel_height()
Height of image in pixels.
virtual uint64_t guid() const
Get Firewire GUID of camera.
unsigned int height
ROI height.
fawkes::upoint_t start
ROI start.
unsigned int width
ROI width.
Rectification info block.
uint8_t camera() const
Get block camera identifier.
Vector that is used for maintaining the rectification info blocks.
virtual void read(const char *filename)
Read file.
RectInfoBlockVector * rectinfo_blocks()
Get all rectification info blocks.
uint64_t guid()
Get the GUID of camera.
void add_rectinfo_block(RectificationInfoBlock *block)
Add a rectification info block.
Recitification Lookup Table Block.
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).
void set_mapping(uint16_t x, uint16_t y, uint16_t to_x, uint16_t to_y)
Set mapping.
virtual int disparity_range_min()
Get disparity range min value.
virtual unsigned char disparity_mapping_min()
Get disparity mapping min value.
void generate_rectification_lut(const char *lut_file)
Generate rectification LUT.
TriclopsStereoProcessor(unsigned int width, unsigned int height, const char *context_file)
Constructor.
virtual bool get_world_xyz(unsigned int px, unsigned int py, float *x, float *y, float *z)
Get transformed coordinates of a point.
virtual unsigned char disparity_mapping_max()
Get disparity mapping max value.
bool verify_rectification_lut(const char *lut_file)
Verify rectification LUT.
virtual bool get_xyz(unsigned int px, unsigned int py, float *x, float *y, float *z)
Get camera-relative coordinates of a point.
virtual void set_output_image_size(unsigned int width, unsigned int height)
Set the resolution of the output images.
virtual void set_stereo_masksize(unsigned int mask_size)
Set stereo mask.
virtual void set_surface_validation(bool enabled)
Enable or disable surface validation.
virtual bool texture_validation()
Check state of texture validation.
virtual void preprocess_stereo()
Do any pre-processing needed.
virtual unsigned int stereo_masksize()
Get stereo mask size.
virtual void calculate_disparity(ROI *roi=0)
Caculate disparity images.
virtual unsigned int edge_masksize()
Get edge mask size.
virtual void set_subpixel_interpolation(bool enabled)
Enable or disable subpixel interpolation.
virtual void set_edge_masksize(unsigned int mask_size)
Set edge mask.
virtual unsigned char * yuv_buffer_left()
Get YUV-formatted buffer of left camera.
virtual unsigned int output_image_height()
Get height of ouput images.
virtual void set_disparity_mapping_range(unsigned char min, unsigned char max)
Set disparity mapping range.
virtual void calculate_yuv(bool both=false)
Caculate yuv images.
virtual unsigned char * yuv_buffer_right()
Get YUV-formatted buffer of reference camera.
virtual unsigned int output_image_width()
Get width of ouput images.
virtual void set_lowpass(bool enabled)
Enable or disable lowpass filtering before rectification.
virtual void set_disparity_mapping(bool enabled)
Enable or disable disparity mapping.
virtual size_t disparity_buffer_size() const
Get disparity buffer size.
virtual bool lowpass()
Check state of lowpass filtering.
virtual void set_disparity_range(int min, int max)
Set disparity range.
virtual void getall_world_xyz(float ***buffer, int hoff, int voff, int width, int height, float *settings)
Calculates all three cartesian coordinates of the entire disparity map The values transformed are giv...
virtual ~TriclopsStereoProcessor()
Destructor.
virtual bool edge_correlation()
Check state of edge correlation.
virtual void set_raw_buffer(unsigned char *raw16_buffer)
Set raw buffer.
virtual void set_texture_validation(bool enabled)
Enable or disable texture validation.
virtual bool subpixel_interpolation()
Check state of subpixel interpolation.
virtual void set_edge_correlation(bool enabled)
Enable or disable edge correlation.
virtual bool surface_validation()
Check state of surface validation.
virtual bool disparity_mapping()
Check state of disparity mapping.
virtual unsigned char * disparity_buffer()
Get the disparity image buffer.
virtual int disparity_range_max()
Get disparity range max value.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
unsigned int x
x coordinate
unsigned int y
y coordinate