app/src/main/jni/libuvc-0.0.6/src/frame.c

   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 */
  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  */
  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  */
  92 void uvc_free_frame(uvc_frame_t *frame) {
  93   if (frame->data_bytes > 0 && frame->library_owns_data)
  94     free(frame->data);
  95
  96   free(frame);
  97 }
  98
  99 static inline unsigned char sat(int i) {
 100   return (unsigned char)( i >= 255 ? 255 : (i < 0 ? 0 : i));
 101 }
 102
 103 /** @brief Duplicate a frame, preserving color format
 104  * @ingroup frame
 105  *
 106  * @param in Original frame
 107  * @param out Duplicate frame
 108  */
 109 uvc_error_t uvc_duplicate_frame(uvc_frame_t *in, uvc_frame_t *out) {
 110   if (uvc_ensure_frame_size(out, in->data_bytes) < 0)
 111     return UVC_ERROR_NO_MEM;
 112
 113   out->width = in->width;
 114   out->height = in->height;
 115   out->frame_format = in->frame_format;
 116   out->step = in->step;
 117   out->sequence = in->sequence;
 118   out->capture_time = in->capture_time;
 119   out->source = in->source;
 120
 121   memcpy(out->data, in->data, in->data_bytes);
 122
 123   return UVC_SUCCESS;
 124 }
 125
 126 #define YUYV2RGB_2(pyuv, prgb) { \
 127     float r = 1.402f * ((pyuv)[3]-128); \
 128     float g = -0.34414f * ((pyuv)[1]-128) - 0.71414f * ((pyuv)[3]-128); \
 129     float b = 1.772f * ((pyuv)[1]-128); \
 130     (prgb)[0] = sat(pyuv[0] + r); \
 131     (prgb)[1] = sat(pyuv[0] + g); \
 132     (prgb)[2] = sat(pyuv[0] + b); \
 133     (prgb)[3] = sat(pyuv[2] + r); \
 134     (prgb)[4] = sat(pyuv[2] + g); \
 135     (prgb)[5] = sat(pyuv[2] + b); \
 136     }
 137 #define IYUYV2RGB_2(pyuv, prgb) { \
 138     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
 139     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
 140     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
 141     (prgb)[0] = sat(*(pyuv) + r); \
 142     (prgb)[1] = sat(*(pyuv) + g); \
 143     (prgb)[2] = sat(*(pyuv) + b); \
 144     (prgb)[3] = sat((pyuv)[2] + r); \
 145     (prgb)[4] = sat((pyuv)[2] + g); \
 146     (prgb)[5] = sat((pyuv)[2] + b); \
 147     }
 148 #define IYUYV2RGB_16(pyuv, prgb) IYUYV2RGB_8(pyuv, prgb); IYUYV2RGB_8(pyuv + 16, prgb + 24);
 149 #define IYUYV2RGB_8(pyuv, prgb) IYUYV2RGB_4(pyuv, prgb); IYUYV2RGB_4(pyuv + 8, prgb + 12);
 150 #define IYUYV2RGB_4(pyuv, prgb) IYUYV2RGB_2(pyuv, prgb); IYUYV2RGB_2(pyuv + 4, prgb + 6);
 151
 152 /** @brief Convert a frame from YUYV to RGB
 153  * @ingroup frame
 154  *
 155  * @param in YUYV frame
 156  * @param out RGB frame
 157  */
 158 uvc_error_t uvc_yuyv2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 159   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 160     return UVC_ERROR_INVALID_PARAM;
 161
 162   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 163     return UVC_ERROR_NO_MEM;
 164
 165   out->width = in->width;
 166   out->height = in->height;
 167   out->frame_format = UVC_FRAME_FORMAT_RGB;
 168   out->step = in->width * 3;
 169   out->sequence = in->sequence;
 170   out->capture_time = in->capture_time;
 171   out->source = in->source;
 172
 173   uint8_t *pyuv = in->data;
 174   uint8_t *prgb = out->data;
 175   uint8_t *prgb_end = prgb + out->data_bytes;
 176
 177   while (prgb < prgb_end) {
 178     IYUYV2RGB_8(pyuv, prgb);
 179
 180     prgb += 3 * 8;
 181     pyuv += 2 * 8;
 182   }
 183
 184   return UVC_SUCCESS;
 185 }
 186
 187 #define IYUYV2BGR_2(pyuv, pbgr) { \
 188     int r = (22987 * ((pyuv)[3] - 128)) >> 14; \
 189     int g = (-5636 * ((pyuv)[1] - 128) - 11698 * ((pyuv)[3] - 128)) >> 14; \
 190     int b = (29049 * ((pyuv)[1] - 128)) >> 14; \
 191     (pbgr)[0] = sat(*(pyuv) + b); \
 192     (pbgr)[1] = sat(*(pyuv) + g); \
 193     (pbgr)[2] = sat(*(pyuv) + r); \
 194     (pbgr)[3] = sat((pyuv)[2] + b); \
 195     (pbgr)[4] = sat((pyuv)[2] + g); \
 196     (pbgr)[5] = sat((pyuv)[2] + r); \
 197     }
 198 #define IYUYV2BGR_16(pyuv, pbgr) IYUYV2BGR_8(pyuv, pbgr); IYUYV2BGR_8(pyuv + 16, pbgr + 24);
 199 #define IYUYV2BGR_8(pyuv, pbgr) IYUYV2BGR_4(pyuv, pbgr); IYUYV2BGR_4(pyuv + 8, pbgr + 12);
 200 #define IYUYV2BGR_4(pyuv, pbgr) IYUYV2BGR_2(pyuv, pbgr); IYUYV2BGR_2(pyuv + 4, pbgr + 6);
 201
 202 /** @brief Convert a frame from YUYV to BGR
 203  * @ingroup frame
 204  *
 205  * @param in YUYV frame
 206  * @param out BGR frame
 207  */
 208 uvc_error_t uvc_yuyv2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 209   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 210     return UVC_ERROR_INVALID_PARAM;
 211
 212   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 213     return UVC_ERROR_NO_MEM;
 214
 215   out->width = in->width;
 216   out->height = in->height;
 217   out->frame_format = UVC_FRAME_FORMAT_BGR;
 218   out->step = in->width * 3;
 219   out->sequence = in->sequence;
 220   out->capture_time = in->capture_time;
 221   out->source = in->source;
 222
 223   uint8_t *pyuv = in->data;
 224   uint8_t *pbgr = out->data;
 225   uint8_t *pbgr_end = pbgr + out->data_bytes;
 226
 227   while (pbgr < pbgr_end) {
 228     IYUYV2BGR_8(pyuv, pbgr);
 229
 230     pbgr += 3 * 8;
 231     pyuv += 2 * 8;
 232   }
 233
 234   return UVC_SUCCESS;
 235 }
 236
 237 #define IYUYV2Y(pyuv, py) { \
 238     (py)[0] = (pyuv[0]); \
 239     }
 240
 241 /** @brief Convert a frame from YUYV to Y (GRAY8)
 242  * @ingroup frame
 243  *
 244  * @param in YUYV frame
 245  * @param out GRAY8 frame
 246  */
 247 uvc_error_t uvc_yuyv2y(uvc_frame_t *in, uvc_frame_t *out) {
 248   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 249     return UVC_ERROR_INVALID_PARAM;
 250
 251   if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
 252     return UVC_ERROR_NO_MEM;
 253
 254   out->width = in->width;
 255   out->height = in->height;
 256   out->frame_format = UVC_FRAME_FORMAT_GRAY8;
 257   out->step = in->width;
 258   out->sequence = in->sequence;
 259   out->capture_time = in->capture_time;
 260   out->source = in->source;
 261
 262   uint8_t *pyuv = in->data;
 263   uint8_t *py = out->data;
 264   uint8_t *py_end = py + out->data_bytes;
 265
 266   while (py < py_end) {
 267     IYUYV2Y(pyuv, py);
 268
 269     py += 1;
 270     pyuv += 2;
 271   }
 272
 273   return UVC_SUCCESS;
 274 }
 275
 276 #define IYUYV2UV(pyuv, puv) { \
 277     (puv)[0] = (pyuv[1]); \
 278     }
 279
 280 /** @brief Convert a frame from YUYV to UV (GRAY8)
 281  * @ingroup frame
 282  *
 283  * @param in YUYV frame
 284  * @param out GRAY8 frame
 285  */
 286 uvc_error_t uvc_yuyv2uv(uvc_frame_t *in, uvc_frame_t *out) {
 287   if (in->frame_format != UVC_FRAME_FORMAT_YUYV)
 288     return UVC_ERROR_INVALID_PARAM;
 289
 290   if (uvc_ensure_frame_size(out, in->width * in->height) < 0)
 291     return UVC_ERROR_NO_MEM;
 292
 293   out->width = in->width;
 294   out->height = in->height;
 295   out->frame_format = UVC_FRAME_FORMAT_GRAY8;
 296   out->step = in->width;
 297   out->sequence = in->sequence;
 298   out->capture_time = in->capture_time;
 299   out->source = in->source;
 300
 301   uint8_t *pyuv = in->data;
 302   uint8_t *puv = out->data;
 303   uint8_t *puv_end = puv + out->data_bytes;
 304
 305   while (puv < puv_end) {
 306     IYUYV2UV(pyuv, puv);
 307
 308     puv += 1;
 309     pyuv += 2;
 310   }
 311
 312   return UVC_SUCCESS;
 313 }
 314
 315 #define IUYVY2RGB_2(pyuv, prgb) { \
 316     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
 317     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
 318     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
 319     (prgb)[0] = sat((pyuv)[1] + r); \
 320     (prgb)[1] = sat((pyuv)[1] + g); \
 321     (prgb)[2] = sat((pyuv)[1] + b); \
 322     (prgb)[3] = sat((pyuv)[3] + r); \
 323     (prgb)[4] = sat((pyuv)[3] + g); \
 324     (prgb)[5] = sat((pyuv)[3] + b); \
 325     }
 326 #define IUYVY2RGB_16(pyuv, prgb) IUYVY2RGB_8(pyuv, prgb); IUYVY2RGB_8(pyuv + 16, prgb + 24);
 327 #define IUYVY2RGB_8(pyuv, prgb) IUYVY2RGB_4(pyuv, prgb); IUYVY2RGB_4(pyuv + 8, prgb + 12);
 328 #define IUYVY2RGB_4(pyuv, prgb) IUYVY2RGB_2(pyuv, prgb); IUYVY2RGB_2(pyuv + 4, prgb + 6);
 329
 330 /** @brief Convert a frame from UYVY to RGB
 331  * @ingroup frame
 332  * @param ini UYVY frame
 333  * @param out RGB frame
 334  */
 335 uvc_error_t uvc_uyvy2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 336   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
 337     return UVC_ERROR_INVALID_PARAM;
 338
 339   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 340     return UVC_ERROR_NO_MEM;
 341
 342   out->width = in->width;
 343   out->height = in->height;
 344   out->frame_format = UVC_FRAME_FORMAT_RGB;
 345   out->step = in->width *3;
 346   out->sequence = in->sequence;
 347   out->capture_time = in->capture_time;
 348   out->source = in->source;
 349
 350   uint8_t *pyuv = in->data;
 351   uint8_t *prgb = out->data;
 352   uint8_t *prgb_end = prgb + out->data_bytes;
 353
 354   while (prgb < prgb_end) {
 355     IUYVY2RGB_8(pyuv, prgb);
 356
 357     prgb += 3 * 8;
 358     pyuv += 2 * 8;
 359   }
 360
 361   return UVC_SUCCESS;
 362 }
 363
 364 #define IUYVY2BGR_2(pyuv, pbgr) { \
 365     int r = (22987 * ((pyuv)[2] - 128)) >> 14; \
 366     int g = (-5636 * ((pyuv)[0] - 128) - 11698 * ((pyuv)[2] - 128)) >> 14; \
 367     int b = (29049 * ((pyuv)[0] - 128)) >> 14; \
 368     (pbgr)[0] = sat((pyuv)[1] + b); \
 369     (pbgr)[1] = sat((pyuv)[1] + g); \
 370     (pbgr)[2] = sat((pyuv)[1] + r); \
 371     (pbgr)[3] = sat((pyuv)[3] + b); \
 372     (pbgr)[4] = sat((pyuv)[3] + g); \
 373     (pbgr)[5] = sat((pyuv)[3] + r); \
 374     }
 375 #define IUYVY2BGR_16(pyuv, pbgr) IUYVY2BGR_8(pyuv, pbgr); IUYVY2BGR_8(pyuv + 16, pbgr + 24);
 376 #define IUYVY2BGR_8(pyuv, pbgr) IUYVY2BGR_4(pyuv, pbgr); IUYVY2BGR_4(pyuv + 8, pbgr + 12);
 377 #define IUYVY2BGR_4(pyuv, pbgr) IUYVY2BGR_2(pyuv, pbgr); IUYVY2BGR_2(pyuv + 4, pbgr + 6);
 378
 379 /** @brief Convert a frame from UYVY to BGR
 380  * @ingroup frame
 381  * @param ini UYVY frame
 382  * @param out BGR frame
 383  */
 384 uvc_error_t uvc_uyvy2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 385   if (in->frame_format != UVC_FRAME_FORMAT_UYVY)
 386     return UVC_ERROR_INVALID_PARAM;
 387
 388   if (uvc_ensure_frame_size(out, in->width * in->height * 3) < 0)
 389     return UVC_ERROR_NO_MEM;
 390
 391   out->width = in->width;
 392   out->height = in->height;
 393   out->frame_format = UVC_FRAME_FORMAT_BGR;
 394   out->step = in->width *3;
 395   out->sequence = in->sequence;
 396   out->capture_time = in->capture_time;
 397   out->source = in->source;
 398
 399   uint8_t *pyuv = in->data;
 400   uint8_t *pbgr = out->data;
 401   uint8_t *pbgr_end = pbgr + out->data_bytes;
 402
 403   while (pbgr < pbgr_end) {
 404     IUYVY2BGR_8(pyuv, pbgr);
 405
 406     pbgr += 3 * 8;
 407     pyuv += 2 * 8;
 408   }
 409
 410   return UVC_SUCCESS;
 411 }
 412
 413 /** @brief Convert a frame to RGB
 414  * @ingroup frame
 415  *
 416  * @param in non-RGB frame
 417  * @param out RGB frame
 418  */
 419 uvc_error_t uvc_any2rgb(uvc_frame_t *in, uvc_frame_t *out) {
 420   switch (in->frame_format) {
 421     case UVC_FRAME_FORMAT_YUYV:
 422       return uvc_yuyv2rgb(in, out);
 423     case UVC_FRAME_FORMAT_UYVY:
 424       return uvc_uyvy2rgb(in, out);
 425     case UVC_FRAME_FORMAT_RGB:
 426       return uvc_duplicate_frame(in, out);
 427     default:
 428       return UVC_ERROR_NOT_SUPPORTED;
 429   }
 430 }
 431
 432 /** @brief Convert a frame to BGR
 433  * @ingroup frame
 434  *
 435  * @param in non-BGR frame
 436  * @param out BGR frame
 437  */
 438 uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
 439   switch (in->frame_format) {
 440     case UVC_FRAME_FORMAT_YUYV:
 441       return uvc_yuyv2bgr(in, out);
 442     case UVC_FRAME_FORMAT_UYVY:
 443       return uvc_uyvy2bgr(in, out);
 444     case UVC_FRAME_FORMAT_BGR:
 445       return uvc_duplicate_frame(in, out);
 446     default:
 447       return UVC_ERROR_NOT_SUPPORTED;
 448   }
 449 }