1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (C) 2010-2012 Ken Tossell
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the author nor other contributors may be
18 * used to endorse or promote products derived from this software
19 * without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 /**
35 * @defgroup frame Frame processing
36 * @brief Tools for managing frame buffers and converting between image formats
37 */
38 #include "libuvc/libuvc.h"
39 #include "libuvc/libuvc_internal.h"
40
41 /** @internal */
uvc_ensure_frame_size(uvc_frame_t * frame,size_t need_bytes)42 uvc_error_t uvc_ensure_frame_size(uvc_frame_t *frame, size_t need_bytes) {
43 if (frame->library_owns_data) {
44 if (!frame->data || frame->data_bytes != need_bytes) {
45 frame->data_bytes = need_bytes;
46 frame->data = realloc(frame->data, frame->data_bytes);
47 }
48 if (!frame->data)
49 return UVC_ERROR_NO_MEM;
50 return UVC_SUCCESS;
51 } else {
52 if (!frame->data || frame->data_bytes < need_bytes)
53 return UVC_ERROR_NO_MEM;
54 return UVC_SUCCESS;
55 }
56 }
57
58 /** @brief Allocate a frame structure
59 * @ingroup frame
60 *
61 * @param data_bytes Number of bytes to allocate, or zero
62 * @return New frame, or NULL on error
63 */
uvc_allocate_frame(size_t data_bytes)64 uvc_frame_t *uvc_allocate_frame(size_t data_bytes) {
65 uvc_frame_t *frame = malloc(sizeof(*frame));
66
67 if (!frame)
68 return NULL;
69
70 memset(frame, 0, sizeof(*frame));
71
72 frame->library_owns_data = 1;
73
74 if (data_bytes > 0) {
75 frame->data_bytes = data_bytes;
76 frame->data = malloc(data_bytes);
77
78 if (!frame->data) {
79 free(frame);
80 return NULL;
81 }
82 }
83
84 return frame;
85 }
86
87 /** @brief Free a frame structure
88 * @ingroup frame
89 *
90 * @param frame Frame to destroy
91 */
uvc_free_frame(uvc_frame_t * frame)92 void uvc_free_frame(uvc_frame_t *frame) {
93 if (frame->library_owns_data)
94 {
95 if (frame->data_bytes > 0)
96 free(frame->data);
97 if (frame->metadata_bytes > 0)
98 free(frame->metadata);
99 }
100
101 free(frame);
102 }
103
sat(int i)104 static inline unsigned char sat(int i) {
105 return (unsigned char)( i >= 255 ? 255 : (i < 0 ? 0 : i));
106 }
107
108 /** @brief Duplicate a frame, preserving color format
109 * @ingroup frame
110 *
111 * @param in Original frame
112 * @param out Duplicate frame
113 */
uvc_duplicate_frame(uvc_frame_t * in,uvc_frame_t * out)114 uvc_error_t uvc_duplicate_frame(uvc_frame_t *in, uvc_frame_t *out) {
115 if (uvc_ensure_frame_size(out, in->data_bytes) < 0)
116 return UVC_ERROR_NO_MEM;
117
118 out->width = in->width;
119 out->height = in->height;
120 out->frame_format = in->frame_format;
121 out->step = in->step;
122 out->sequence = in->sequence;
123 out->capture_time = in->capture_time;
124 out->capture_time_finished = in->capture_time_finished;
125 out->source = in->source;
126
127 memcpy(out->data, in->data, in->data_bytes);
128
129 if (in->metadata && in->metadata_bytes > 0)
130 {
131 if (out->metadata_bytes < in->metadata_bytes)
132 {
133 out->metadata = realloc(out->metadata, in->metadata_bytes);
134 }
135 out->metadata_bytes = in->metadata_bytes;
136 memcpy(out->metadata, in->metadata, in->metadata_bytes);
137 }
138
139 return UVC_SUCCESS;
140 }
141
142 #define YUYV2RGB_2(pyuv, prgb) { \
143 float r = 1.402f * ((pyuv)[3]-128); \
144 float g = -0.34414f * ((pyuv)[1]-128) - 0.71414f * ((pyuv)[3]-128); \
145 float b = 1.772f * ((pyuv)[1]-128); \
146 (prgb)[0] = sat(pyuv[0] + r); \
147 (prgb)[1] = sat(pyuv[0] + g); \
148 (prgb)[2] = sat(pyuv[0] + b); \
149 (prgb)[3] = sat(pyuv[2] + r); \
150 (prgb)[4] = sat(pyuv[2] + g); \
151 (prgb)[5] = sat(pyuv[2] + b); \
152 }
153 #define IYUYV2RGB_2(pyuv, prgb) { \
154 int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
155 int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
156 int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
157 (prgb)[0] = sat(*(pyuv) + r); \
158 (prgb)[1] = sat(*(pyuv) + g); \
159 (prgb)[2] = sat(*(pyuv) + b); \
160 (prgb)[3] = sat((pyuv)[2] + r); \
161 (prgb)[4] = sat((pyuv)[2] + g); \
162 (prgb)[5] = sat((pyuv)[2] + b); \
163 }
164 #define IYUYV2RGB_16(pyuv, prgb) IYUYV2RGB_8(pyuv, prgb); IYUYV2RGB_8(pyuv + 16, prgb + 24);
165 #define IYUYV2RGB_8(pyuv, prgb) IYUYV2RGB_4(pyuv, prgb); IYUYV2RGB_4(pyuv + 8, prgb + 12);
166 #define IYUYV2RGB_4(pyuv, prgb) IYUYV2RGB_2(pyuv, prgb); IYUYV2RGB_2(pyuv + 4, prgb + 6);
167
168 /** @brief Convert a frame from YUYV to RGB
169 * @ingroup frame
170 *
171 * @param in YUYV frame
172 * @param out RGB frame
173 */
uvc_yuyv2rgb(uvc_frame_t * in,uvc_frame_t * out)174 uvc_error_t uvc_yuyv2rgb(uvc_frame_t *in, uvc_frame_t *out) {
175 if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
176 return UVC_ERROR_INVALID_PARAM;
177
178 if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
179 return UVC_ERROR_NO_MEM;
180
181 out->width = in->width;
182 out->height = in->height;
183 out->frame_format = UVC_FRAME_FORMAT_RGB;
184 out->step = in->width * 3;
185 out->sequence = in->sequence;
186 out->capture_time = in->capture_time;
187 out->capture_time_finished = in->capture_time_finished;
188 out->source = in->source;
189
190 uint8_t *pyuv = in->data;
191 uint8_t *prgb = out->data;
192 uint8_t *prgb_end = prgb + out->data_bytes;
193
194 while (prgb < prgb_end) {
195 IYUYV2RGB_8(pyuv, prgb);
196
197 prgb += 3 * 8;
198 pyuv += 2 * 8;
199 }
200
201 return UVC_SUCCESS;
202 }
203
204 #define IYUYV2BGR_2(pyuv, pbgr) { \
205 int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
206 int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
207 int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
208 (pbgr)[0] = sat(*(pyuv) + b); \
209 (pbgr)[1] = sat(*(pyuv) + g); \
210 (pbgr)[2] = sat(*(pyuv) + r); \
211 (pbgr)[3] = sat((pyuv)[2] + b); \
212 (pbgr)[4] = sat((pyuv)[2] + g); \
213 (pbgr)[5] = sat((pyuv)[2] + r); \
214 }
215 #define IYUYV2BGR_16(pyuv, pbgr) IYUYV2BGR_8(pyuv, pbgr); IYUYV2BGR_8(pyuv + 16, pbgr + 24);
216 #define IYUYV2BGR_8(pyuv, pbgr) IYUYV2BGR_4(pyuv, pbgr); IYUYV2BGR_4(pyuv + 8, pbgr + 12);
217 #define IYUYV2BGR_4(pyuv, pbgr) IYUYV2BGR_2(pyuv, pbgr); IYUYV2BGR_2(pyuv + 4, pbgr + 6);
218
219 /** @brief Convert a frame from YUYV to BGR
220 * @ingroup frame
221 *
222 * @param in YUYV frame
223 * @param out BGR frame
224 */
uvc_yuyv2bgr(uvc_frame_t * in,uvc_frame_t * out)225 uvc_error_t uvc_yuyv2bgr(uvc_frame_t *in, uvc_frame_t *out) {
226 if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
227 return UVC_ERROR_INVALID_PARAM;
228
229 if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
230 return UVC_ERROR_NO_MEM;
231
232 out->width = in->width;
233 out->height = in->height;
234 out->frame_format = UVC_FRAME_FORMAT_BGR;
235 out->step = in->width * 3;
236 out->sequence = in->sequence;
237 out->capture_time = in->capture_time;
238 out->capture_time_finished = in->capture_time_finished;
239 out->source = in->source;
240
241 uint8_t *pyuv = in->data;
242 uint8_t *pbgr = out->data;
243 uint8_t *pbgr_end = pbgr + out->data_bytes;
244
245 while (pbgr < pbgr_end) {
246 IYUYV2BGR_8(pyuv, pbgr);
247
248 pbgr += 3 * 8;
249 pyuv += 2 * 8;
250 }
251
252 return UVC_SUCCESS;
253 }
254
255 #define IYUYV2Y(pyuv, py) { \
256 (py)[0] = (pyuv[0]); \
257 }
258
259 /** @brief Convert a frame from YUYV to Y (GRAY8)
260 * @ingroup frame
261 *
262 * @param in YUYV frame
263 * @param out GRAY8 frame
264 */
uvc_yuyv2y(uvc_frame_t * in,uvc_frame_t * out)265 uvc_error_t uvc_yuyv2y(uvc_frame_t *in, uvc_frame_t *out) {
266 if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
267 return UVC_ERROR_INVALID_PARAM;
268
269 if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
270 return UVC_ERROR_NO_MEM;
271
272 out->width = in->width;
273 out->height = in->height;
274 out->frame_format = UVC_FRAME_FORMAT_GRAY8;
275 out->step = in->width;
276 out->sequence = in->sequence;
277 out->capture_time = in->capture_time;
278 out->capture_time_finished = in->capture_time_finished;
279 out->source = in->source;
280
281 uint8_t *pyuv = in->data;
282 uint8_t *py = out->data;
283 uint8_t *py_end = py + out->data_bytes;
284
285 while (py < py_end) {
286 IYUYV2Y(pyuv, py);
287
288 py += 1;
289 pyuv += 2;
290 }
291
292 return UVC_SUCCESS;
293 }
294
295 #define IYUYV2UV(pyuv, puv) { \
296 (puv)[0] = (pyuv[1]); \
297 }
298
299 /** @brief Convert a frame from YUYV to UV (GRAY8)
300 * @ingroup frame
301 *
302 * @param in YUYV frame
303 * @param out GRAY8 frame
304 */
uvc_yuyv2uv(uvc_frame_t * in,uvc_frame_t * out)305 uvc_error_t uvc_yuyv2uv(uvc_frame_t *in, uvc_frame_t *out) {
306 if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
307 return UVC_ERROR_INVALID_PARAM;
308
309 if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
310 return UVC_ERROR_NO_MEM;
311
312 out->width = in->width;
313 out->height = in->height;
314 out->frame_format = UVC_FRAME_FORMAT_GRAY8;
315 out->step = in->width;
316 out->sequence = in->sequence;
317 out->capture_time = in->capture_time;
318 out->capture_time_finished = in->capture_time_finished;
319 out->source = in->source;
320
321 uint8_t *pyuv = in->data;
322 uint8_t *puv = out->data;
323 uint8_t *puv_end = puv + out->data_bytes;
324
325 while (puv < puv_end) {
326 IYUYV2UV(pyuv, puv);
327
328 puv += 1;
329 pyuv += 2;
330 }
331
332 return UVC_SUCCESS;
333 }
334
335 #define IUYVY2RGB_2(pyuv, prgb) { \
336 int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
337 int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
338 int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
339 (prgb)[0] = sat((pyuv)[1] + r); \
340 (prgb)[1] = sat((pyuv)[1] + g); \
341 (prgb)[2] = sat((pyuv)[1] + b); \
342 (prgb)[3] = sat((pyuv)[3] + r); \
343 (prgb)[4] = sat((pyuv)[3] + g); \
344 (prgb)[5] = sat((pyuv)[3] + b); \
345 }
346 #define IUYVY2RGB_16(pyuv, prgb) IUYVY2RGB_8(pyuv, prgb); IUYVY2RGB_8(pyuv + 16, prgb + 24);
347 #define IUYVY2RGB_8(pyuv, prgb) IUYVY2RGB_4(pyuv, prgb); IUYVY2RGB_4(pyuv + 8, prgb + 12);
348 #define IUYVY2RGB_4(pyuv, prgb) IUYVY2RGB_2(pyuv, prgb); IUYVY2RGB_2(pyuv + 4, prgb + 6);
349
350 /** @brief Convert a frame from UYVY to RGB
351 * @ingroup frame
352 * @param ini UYVY frame
353 * @param out RGB frame
354 */
uvc_uyvy2rgb(uvc_frame_t * in,uvc_frame_t * out)355 uvc_error_t uvc_uyvy2rgb(uvc_frame_t *in, uvc_frame_t *out) {
356 if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
357 return UVC_ERROR_INVALID_PARAM;
358
359 if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
360 return UVC_ERROR_NO_MEM;
361
362 out->width = in->width;
363 out->height = in->height;
364 out->frame_format = UVC_FRAME_FORMAT_RGB;
365 out->step = in->width *3;
366 out->sequence = in->sequence;
367 out->capture_time = in->capture_time;
368 out->capture_time_finished = in->capture_time_finished;
369 out->source = in->source;
370
371 uint8_t *pyuv = in->data;
372 uint8_t *prgb = out->data;
373 uint8_t *prgb_end = prgb + out->data_bytes;
374
375 while (prgb < prgb_end) {
376 IUYVY2RGB_8(pyuv, prgb);
377
378 prgb += 3 * 8;
379 pyuv += 2 * 8;
380 }
381
382 return UVC_SUCCESS;
383 }
384
385 #define IUYVY2BGR_2(pyuv, pbgr) { \
386 int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
387 int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
388 int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
389 (pbgr)[0] = sat((pyuv)[1] + b); \
390 (pbgr)[1] = sat((pyuv)[1] + g); \
391 (pbgr)[2] = sat((pyuv)[1] + r); \
392 (pbgr)[3] = sat((pyuv)[3] + b); \
393 (pbgr)[4] = sat((pyuv)[3] + g); \
394 (pbgr)[5] = sat((pyuv)[3] + r); \
395 }
396 #define IUYVY2BGR_16(pyuv, pbgr) IUYVY2BGR_8(pyuv, pbgr); IUYVY2BGR_8(pyuv + 16, pbgr + 24);
397 #define IUYVY2BGR_8(pyuv, pbgr) IUYVY2BGR_4(pyuv, pbgr); IUYVY2BGR_4(pyuv + 8, pbgr + 12);
398 #define IUYVY2BGR_4(pyuv, pbgr) IUYVY2BGR_2(pyuv, pbgr); IUYVY2BGR_2(pyuv + 4, pbgr + 6);
399
400 /** @brief Convert a frame from UYVY to BGR
401 * @ingroup frame
402 * @param ini UYVY frame
403 * @param out BGR frame
404 */
uvc_uyvy2bgr(uvc_frame_t * in,uvc_frame_t * out)405 uvc_error_t uvc_uyvy2bgr(uvc_frame_t *in, uvc_frame_t *out) {
406 if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
407 return UVC_ERROR_INVALID_PARAM;
408
409 if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
410 return UVC_ERROR_NO_MEM;
411
412 out->width = in->width;
413 out->height = in->height;
414 out->frame_format = UVC_FRAME_FORMAT_BGR;
415 out->step = in->width *3;
416 out->sequence = in->sequence;
417 out->capture_time = in->capture_time;
418 out->capture_time_finished = in->capture_time_finished;
419 out->source = in->source;
420
421 uint8_t *pyuv = in->data;
422 uint8_t *pbgr = out->data;
423 uint8_t *pbgr_end = pbgr + out->data_bytes;
424
425 while (pbgr < pbgr_end) {
426 IUYVY2BGR_8(pyuv, pbgr);
427
428 pbgr += 3 * 8;
429 pyuv += 2 * 8;
430 }
431
432 return UVC_SUCCESS;
433 }
434
435 /** @brief Convert a frame to RGB
436 * @ingroup frame
437 *
438 * @param in non-RGB frame
439 * @param out RGB frame
440 */
uvc_any2rgb(uvc_frame_t * in,uvc_frame_t * out)441 uvc_error_t uvc_any2rgb(uvc_frame_t *in, uvc_frame_t *out) {
442 switch (in->frame_format) {
443 #ifdef LIBUVC_HAS_JPEG
444 case UVC_FRAME_FORMAT_MJPEG:
445 return uvc_mjpeg2rgb(in, out);
446 #endif
447 case UVC_FRAME_FORMAT_YUYV:
448 return uvc_yuyv2rgb(in, out);
449 case UVC_FRAME_FORMAT_UYVY:
450 return uvc_uyvy2rgb(in, out);
451 case UVC_FRAME_FORMAT_RGB:
452 return uvc_duplicate_frame(in, out);
453 default:
454 return UVC_ERROR_NOT_SUPPORTED;
455 }
456 }
457
458 /** @brief Convert a frame to BGR
459 * @ingroup frame
460 *
461 * @param in non-BGR frame
462 * @param out BGR frame
463 */
uvc_any2bgr(uvc_frame_t * in,uvc_frame_t * out)464 uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
465 switch (in->frame_format) {
466 case UVC_FRAME_FORMAT_YUYV:
467 return uvc_yuyv2bgr(in, out);
468 case UVC_FRAME_FORMAT_UYVY:
469 return uvc_uyvy2bgr(in, out);
470 case UVC_FRAME_FORMAT_BGR:
471 return uvc_duplicate_frame(in, out);
472 default:
473 return UVC_ERROR_NOT_SUPPORTED;
474 }
475 }
476