15 #include "webcamVideoV4L.h" 17 #ifdef HAVE_VIDEO4LINUX 21 #include <sys/ioctl.h> 34 #define clamp(x) min(max(x, 0.0), 255.0) 36 INLINE
static void yuv_to_bgr(
unsigned char *dest,
const unsigned char *src) {
37 double y1 = (255 / 219.0) * (src[0] - 16);
38 double pb = (255 / 224.0) * (src[1] - 128);
39 double pr = (255 / 224.0) * (src[2] - 128);
40 dest[2] = clamp(1.0 * y1 + 0 * pb + 1.402 * pr);
41 dest[1] = clamp(1.0 * y1 - 0.344 * pb - 0.714 * pr);
42 dest[0] = clamp(1.0 * y1 + 1.772 * pb + 0 * pr);
45 INLINE
static void yuyv_to_bgrbgr(
unsigned char *dest,
const unsigned char *src) {
46 unsigned char yuv[] = {src[0], src[1], src[3]};
47 yuv_to_bgr(dest, yuv);
49 yuv_to_bgr(dest + 3, yuv);
52 INLINE
static void yuyv_to_bgrabgra(
unsigned char *dest,
const unsigned char *src) {
53 unsigned char yuv[] = {src[0], src[1], src[3]};
54 yuv_to_bgr(dest, yuv);
56 yuv_to_bgr(dest + 4, yuv);
61 INLINE
static void rgb_to_bgr(
unsigned char *dest,
const unsigned char *src) {
67 INLINE
static void rgb_to_bgra(
unsigned char *dest,
const unsigned char *src) {
74 #if defined(HAVE_JPEG) && !defined(CPPPARSER) 77 struct jpeg_error_mgr pub;
78 jmp_buf setjmp_buffer;
81 typedef struct my_error_mgr *my_error_ptr;
83 static void my_error_exit (j_common_ptr cinfo) {
85 char buffer[JMSG_LENGTH_MAX];
86 (*cinfo->err->format_message) (cinfo, buffer);
87 vision_cat.error() << buffer <<
"\n";
89 my_error_ptr myerr = (my_error_ptr) cinfo->err;
91 longjmp(myerr->setjmp_buffer, 1);
94 static void my_output_message (j_common_ptr cinfo){
95 char buffer[JMSG_LENGTH_MAX];
96 (*cinfo->err->format_message) (cinfo, buffer);
97 vision_cat.warning() << buffer <<
"\n";
100 static void my_init_source(j_decompress_ptr cinfo) {
103 static boolean my_fill_input_buffer(j_decompress_ptr cinfo) {
104 struct jpeg_source_mgr *src = cinfo->src;
105 static JOCTET FakeEOI[] = {0xFF, JPEG_EOI};
107 WARNMS(cinfo, JWRN_JPEG_EOF);
109 src->next_input_byte = FakeEOI;
110 src->bytes_in_buffer = 2;
115 static void my_skip_input_data(j_decompress_ptr cinfo,
long num_bytes) {
116 struct jpeg_source_mgr *src = cinfo->src;
118 if (num_bytes >= (
long) src->bytes_in_buffer) {
119 my_fill_input_buffer(cinfo);
123 src->bytes_in_buffer -= num_bytes;
124 src->next_input_byte += num_bytes;
127 static void my_term_source(j_decompress_ptr cinfo) {
131 static JHUFF_TBL dc_luminance_tbl = {
132 {0, 0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0},
133 {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11},
137 static JHUFF_TBL dc_chrominance_tbl = {
138 {0, 0, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0},
139 {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11},
143 static JHUFF_TBL ac_luminance_tbl = {
144 {0, 0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d},
146 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21,
147 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71,
148 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42, 0xb1,
149 0xc1, 0x15, 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72,
150 0x82, 0x09, 0x0a, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x25,
151 0x26, 0x27, 0x28, 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37,
152 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
153 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
154 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a,
155 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a, 0x83,
156 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93,
157 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
158 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3,
159 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
160 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3,
161 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
162 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1,
163 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
168 static JHUFF_TBL ac_chrominance_tbl = {
169 {0, 0, 2, 1, 2, 4, 4, 3, 4, 7, 5, 4, 4, 0, 1, 2, 0x77},
171 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31,
172 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22,
173 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xa1, 0xb1, 0xc1,
174 0x09, 0x23, 0x33, 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1,
175 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25, 0xf1, 0x17, 0x18,
176 0x19, 0x1a, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36,
177 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47,
178 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
179 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
180 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
181 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a,
182 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
183 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa,
184 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba,
185 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca,
186 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
187 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
188 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
200 WebcamVideoCursorV4L::
202 _size_x = src->_size_x;
203 _size_y = src->_size_y;
207 _can_seek_fast =
false;
211 memset(&_format, 0,
sizeof(
struct v4l2_format));
215 _fd = open(src->_device.c_str(), O_RDWR);
217 vision_cat.error() <<
"Failed to open " << src->_device.c_str() <<
"\n";
223 _format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
224 _format.fmt.pix.pixelformat = src->_pformat;
226 switch (_format.fmt.pix.pixelformat) {
228 case V4L2_PIX_FMT_MJPEG:
233 case V4L2_PIX_FMT_YUYV:
237 case V4L2_PIX_FMT_BGR24:
241 case V4L2_PIX_FMT_BGR32:
245 case V4L2_PIX_FMT_RGB24:
249 case V4L2_PIX_FMT_RGB32:
254 vision_cat.error() <<
"Unsupported pixel format " << src->get_pixel_format() <<
"!\n";
262 _format.fmt.pix.width = _size_x;
263 _format.fmt.pix.height = _size_y;
264 _format.fmt.pix.field = V4L2_FIELD_NONE;
267 if (-1 == ioctl(_fd, VIDIOC_S_FMT, &_format)) {
268 vision_cat.error() <<
"Driver rejected format!\n";
275 _size_x = _format.fmt.pix.width;
276 _size_y = _format.fmt.pix.height;
278 struct v4l2_streamparm streamparm;
279 memset(&streamparm, 0,
sizeof streamparm);
280 streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
281 streamparm.parm.capture.timeperframe.numerator = 1;
282 streamparm.parm.capture.timeperframe.denominator = src->_fps;
283 if (ioctl(_fd, VIDIOC_S_PARM, &streamparm) < 0) {
284 vision_cat.error() <<
"Driver rejected framerate!\n";
287 struct v4l2_requestbuffers req;
288 memset(&req, 0,
sizeof req);
290 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
291 req.memory = V4L2_MEMORY_MMAP;
293 if (-1 == ioctl (_fd, VIDIOC_REQBUFS, &req)) {
294 vision_cat.error() <<
"Failed to request buffers from webcam!\n";
298 vision_cat.error() <<
"Insufficient buffer memory!\n";
301 _bufcount = req.count;
302 _buffers = (
void **) calloc (req.count, sizeof (
void*));
303 _buflens = (
size_t*) calloc (req.count, sizeof (
size_t));
305 if (!_buffers || !_buflens) {
306 vision_cat.error() <<
"Not enough memory!\n";
310 struct v4l2_buffer buf;
311 for (
int i = 0; i < _bufcount; ++i) {
312 memset(&buf, 0,
sizeof buf);
313 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
314 buf.memory = V4L2_MEMORY_MMAP;
317 if (-1 == ioctl(_fd, VIDIOC_QUERYBUF, &buf)) {
318 vision_cat.error() <<
"Failed to query buffer!\n";
321 _buflens[i] = buf.length;
322 _buffers[i] = mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, buf.m.offset);
324 if (_buffers[i] == MAP_FAILED) {
325 vision_cat.error() <<
"Failed to map buffer!\n";
328 if (-1 == ioctl(_fd, VIDIOC_QBUF, &buf)) {
329 vision_cat.error() <<
"Failed to exchange buffer with driver!\n";
333 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
334 if (-1 == ioctl(_fd, VIDIOC_STREAMON, &type)) {
335 vision_cat.error() <<
"Failed to stream from buffer!\n";
340 if (_format.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
341 jpeg_create_decompress(&_cinfo);
343 _cinfo.src = (
struct jpeg_source_mgr *)
344 (*_cinfo.mem->alloc_small) ((j_common_ptr) &_cinfo, JPOOL_PERMANENT,
345 sizeof(
struct jpeg_source_mgr));
347 _cinfo.src->init_source = my_init_source;
348 _cinfo.src->fill_input_buffer = my_fill_input_buffer;
349 _cinfo.src->skip_input_data = my_skip_input_data;
350 _cinfo.src->resync_to_restart = jpeg_resync_to_restart;
351 _cinfo.src->term_source = my_term_source;
362 WebcamVideoCursorV4L::
363 ~WebcamVideoCursorV4L() {
365 if (_format.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
366 jpeg_destroy_decompress(&_cinfo);
370 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
371 ioctl(_fd, VIDIOC_STREAMOFF, &type);
375 for (
int i = 0; i < _bufcount; ++i) {
376 munmap(_buffers[i], _buflens[i]);
396 PT(
Buffer) buffer = get_standard_buffer();
397 unsigned char *block = buffer->_block;
398 struct v4l2_buffer vbuf;
399 memset(&vbuf, 0,
sizeof vbuf);
400 vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
401 vbuf.memory = V4L2_MEMORY_MMAP;
402 if (-1 == ioctl(_fd, VIDIOC_DQBUF, &vbuf) && errno != EIO) {
403 vision_cat.error() <<
"Failed to dequeue buffer!\n";
406 nassertr(vbuf.index < _bufcount, NULL);
407 size_t bufsize = _buflens[vbuf.index];
408 size_t old_bpl = _format.fmt.pix.bytesperline;
409 size_t new_bpl = _size_x * _num_components;
410 unsigned char *buf = (
unsigned char *) _buffers[vbuf.index];
412 switch (_format.fmt.pix.pixelformat) {
413 case V4L2_PIX_FMT_MJPEG: {
415 struct my_error_mgr jerr;
416 _cinfo.err = jpeg_std_error(&jerr.pub);
417 jerr.pub.error_exit = my_error_exit;
418 jerr.pub.output_message = my_output_message;
420 unsigned char *newbuf = (
unsigned char*) malloc(new_bpl * _size_y);
423 if (setjmp(jerr.setjmp_buffer)) {
424 jpeg_abort_decompress(&_cinfo);
427 _cinfo.src->bytes_in_buffer = bufsize;
428 _cinfo.src->next_input_byte = buf;
430 if (jpeg_read_header(&_cinfo, TRUE) == JPEG_HEADER_OK) {
431 if (_cinfo.dc_huff_tbl_ptrs[0] == NULL) {
433 _cinfo.dc_huff_tbl_ptrs[0] = &dc_luminance_tbl;
434 _cinfo.dc_huff_tbl_ptrs[1] = &dc_chrominance_tbl;
435 _cinfo.ac_huff_tbl_ptrs[0] = &ac_luminance_tbl;
436 _cinfo.ac_huff_tbl_ptrs[1] = &ac_chrominance_tbl;
439 _cinfo.scale_num = 1;
440 _cinfo.scale_denom = 1;
441 _cinfo.out_color_space = JCS_RGB;
442 _cinfo.dct_method = JDCT_IFAST;
444 if (jpeg_start_decompress(&_cinfo) && _cinfo.output_components == 3
445 && _size_x == _cinfo.output_width && _size_y == _cinfo.output_height) {
447 JSAMPLE *buffer_end = newbuf + new_bpl * _cinfo.output_height;
448 JSAMPLE *rowptr = newbuf;
449 while (_cinfo.output_scanline < _cinfo.output_height) {
450 nassertd(rowptr + new_bpl <= buffer_end)
break;
451 jpeg_read_scanlines(&_cinfo, &rowptr, _cinfo.output_height);
455 if (_cinfo.output_scanline < _cinfo.output_height) {
456 jpeg_abort_decompress(&_cinfo);
458 jpeg_finish_decompress(&_cinfo);
465 for (
size_t row = 0; row < _size_y; ++row) {
466 memcpy(block + (_size_y - row - 1) * new_bpl, newbuf + row * new_bpl, new_bpl);
472 for (
size_t i = 0; i < new_bpl * _size_y; i += 3) {
474 block[i] = block[i + 2];
478 nassertr(
false , NULL);
482 case V4L2_PIX_FMT_YUYV:
483 for (
size_t row = 0; row < _size_y; ++row) {
485 for (
size_t i = 0; i < old_bpl; i += 4) {
486 yuyv_to_bgrbgr(block + (_size_y - row - 1) * new_bpl + c, buf + row * old_bpl + i);
492 case V4L2_PIX_FMT_BGR24:
493 case V4L2_PIX_FMT_BGR32:
495 nassertr(old_bpl == new_bpl, NULL);
497 for (
size_t row = 0; row < _size_y; ++row) {
498 memcpy(block + (_size_y - row - 1) * new_bpl, buf + row * old_bpl, new_bpl);
502 case V4L2_PIX_FMT_RGB24:
504 nassertr(old_bpl == new_bpl, NULL);
506 for (
size_t row = 0; row < _size_y; ++row) {
507 for (
size_t i = 0; i < old_bpl; i += 3) {
508 rgb_to_bgr(block + (_size_y - row - 1) * old_bpl + i, buf + row * old_bpl + i);
513 case V4L2_PIX_FMT_RGB32:
515 nassertr(old_bpl == new_bpl, NULL);
517 for (
size_t row = 0; row < _size_y; ++row) {
518 for (
size_t i = 0; i < old_bpl; i += 4) {
519 rgb_to_bgra(block + (_size_y - row - 1) * old_bpl + i, buf + row * old_bpl + i + 1);
525 if (-1 == ioctl(_fd, VIDIOC_QBUF, &vbuf)) {
526 vision_cat.error() <<
"Failed to exchange buffer with driver!\n";
A MovieVideo is actually any source that provides a sequence of video frames.
TypeHandle is the identifier used to differentiate C++ class types.