bool CameraVideo::capture() {
// keep the requested interval
robottime_t currentTime = getCurrentTime();
robottime_t nextImageTime = lastImageCaptured + Millisecond(1./fps);
if (nextImageTime > currentTime)
delay(nextImageTime - currentTime);
cv::Mat cvImageBGR;
*video >> cvImageBGR;
uint16_t imageHeight = cvImageBGR.rows;
uint16_t imageWidth = cvImageBGR.cols;
#if defined IMAGEFORMAT_YUV422
// allocate an uint8_t array, initialize it to zero and provide a proper deleter
std::shared_ptr<uint8_t> yuv422(new uint8_t[imageWidth*imageHeight*2](), std::default_delete<uint8_t[]>());
if (!yuv422) {
ERROR("Could not allocate memory for image conversion.");
return false;
}
// convert image to yuv422
for (int x=0; x < imageWidth; x += 2) {
for (int y=0; y < imageHeight; y++) {
uint8_t y1, y2, u1, u2, v1, v2;
ColorConverter::rgb2yuv(cvImageBGR.at<cv::Vec3b>(y, x )[0], cvImageBGR.at<cv::Vec3b>(y, x )[1], cvImageBGR.at<cv::Vec3b>(y, x )[2], &y1, &u1, &v1);
ColorConverter::rgb2yuv(cvImageBGR.at<cv::Vec3b>(y, x+1)[0], cvImageBGR.at<cv::Vec3b>(y, x+1)[1], cvImageBGR.at<cv::Vec3b>(y, x+1)[2], &y2, &u2, &v2);
yuv422.get()[2*y*imageWidth + 2*x + 0] = y1;
yuv422.get()[2*y*imageWidth + 2*x + 1] = (v1 + v2)/2;
yuv422.get()[2*y*imageWidth + 2*x + 2] = y2;
yuv422.get()[2*y*imageWidth + 2*x + 3] = (u1 + u2)/2;
}
}
image->setImage(0, std::static_pointer_cast<void>(yuv422), imageWidth*imageHeight*2, imageWidth, imageHeight);
#else
#error("Video support not yet implemented for this image type.");
#endif
totalFrames++;
lastImageCaptured = getCurrentTime();
// proceed to next image
imageIdx++;
return true;
}
void BC_CModels::transfer(unsigned char **output_rows,
unsigned char **input_rows,
unsigned char *out_y_plane,
unsigned char *out_u_plane,
unsigned char *out_v_plane,
unsigned char *in_y_plane,
unsigned char *in_u_plane,
unsigned char *in_v_plane,
int in_x,
int in_y,
int in_w,
int in_h,
int out_x,
int out_y,
int out_w,
int out_h,
int in_colormodel,
int out_colormodel,
int bg_color,
int in_rowspan,
int out_rowspan)
{
int *column_table;
int *row_table;
int scale;
int bg_r, bg_g, bg_b;
int in_pixelsize = calculate_pixelsize(in_colormodel);
int out_pixelsize = calculate_pixelsize(out_colormodel);
bg_r = (bg_color & 0xff0000) >> 16;
bg_g = (bg_color & 0xff00) >> 8;
bg_b = (bg_color & 0xff);
// Get scaling
scale = (out_w != in_w) || (in_x != 0);
get_scale_tables(&column_table, &row_table,
in_x, in_y, in_x + in_w, in_y + in_h,
out_x, out_y, out_x + out_w, out_y + out_h);
// printf("BC_CModels::transfer %d %d %d %d,%d %d,%d %d,%d %d,%d\n",
// __LINE__,
// in_colormodel,
// out_colormodel,
// out_x,
// out_y,
// out_w,
// out_h,
// in_x,
// in_y,
// in_w,
// in_h);
#define PERMUTATION_VALUES \
output_rows, \
input_rows, \
out_y_plane, \
out_u_plane, \
out_v_plane, \
in_y_plane, \
in_u_plane, \
in_v_plane, \
in_x, \
in_y, \
in_w, \
in_h, \
out_x, \
out_y, \
out_w, \
out_h, \
in_colormodel, \
out_colormodel, \
bg_color, \
in_rowspan, \
out_rowspan, \
scale, \
out_pixelsize, \
in_pixelsize, \
row_table, \
column_table, \
bg_r, \
bg_g, \
bg_b
// Handle planar cmodels separately
switch(in_colormodel)
{
case BC_RGB_FLOAT:
case BC_RGBA_FLOAT:
cmodel_float(PERMUTATION_VALUES);
break;
case BC_YUV420P:
case BC_YUV422P:
yuv420p(PERMUTATION_VALUES);
break;
case BC_YUV9P:
yuv9p(PERMUTATION_VALUES);
break;
case BC_YUV444P:
yuv444p(PERMUTATION_VALUES);
break;
case BC_YUV422:
yuv422(PERMUTATION_VALUES);
break;
default:
cmodel_default(PERMUTATION_VALUES);
break;
}
/*
* printf("transfer 100 %d %d\n",
* in_colormodel,
* out_colormodel);
*/
free(column_table);
free(row_table);
}
