JeVois  1.5
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
RawImageOps.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
19 #include <jevois/Core/VideoBuf.H>
20 #include <jevois/Util/Utils.H>
21 #include <jevois/Debug/Log.H>
22 #include <jevois/Image/Jpeg.H>
23 #include <future>
24 
25 #include <linux/videodev2.h>
26 #include <cmath>
27 #include <opencv2/imgproc/imgproc.hpp>
28 
29 // ####################################################################################################
31 {
32  unsigned int bpp = jevois::v4l2BytesPerPix(src.fmt);
33 
34  switch (bpp)
35  {
36  case 3: return cv::Mat(src.height, src.width, CV_8UC3, src.buf->data());
37  case 2: return cv::Mat(src.height, src.width, CV_8UC2, src.buf->data());
38  case 1: return cv::Mat(src.height, src.width, CV_8UC1, src.buf->data());
39  default: LFATAL("Unsupported RawImage format");
40  }
41 }
42 
43 // ####################################################################################################
44 namespace
45 {
46  inline void rgb565pixrgb(unsigned short rgb565, unsigned char & r, unsigned char & g, unsigned char & b)
47  {
48  r = ((((rgb565 >> 11) & 0x1F) * 527) + 23) >> 6;
49  g = ((((rgb565 >> 5) & 0x3F) * 259) + 33) >> 6;
50  b = (((rgb565 & 0x1F) * 527) + 23) >> 6;
51  }
52 
53  class rgb565ToGray : public cv::ParallelLoopBody
54  {
55  public:
56  rgb565ToGray(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
57  inImg(inputImage), outImg(outImage)
58  {
59  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
60  outlinesize = outw * 1; // 1 byte/pix for Gray
61  }
62 
63  virtual void operator()(const cv::Range & range) const
64  {
65  for (int j = range.start; j < range.end; ++j)
66  {
67  int const inoff = j * inlinesize;
68  int const outoff = j * outlinesize;
69 
70  for (int i = 0; i < inImg.cols; ++i)
71  {
72  int const in = inoff + i * 2;
73  int const out = outoff + i;
74  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
75  unsigned char r, g, b;
76  rgb565pixrgb(rgb565, r, g, b);
77  int lum = int(r + g + b) / 3;
78  outImg[out] = lum;
79  }
80  }
81  }
82 
83  private:
84  cv::Mat const & inImg;
85  unsigned char * outImg;
86  int inlinesize, outlinesize;
87  };
88 
89  class rgb565ToBGR : public cv::ParallelLoopBody
90  {
91  public:
92  rgb565ToBGR(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
93  inImg(inputImage), outImg(outImage)
94  {
95  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
96  outlinesize = outw * 3; // 3 bytes/pix for BGR
97  }
98 
99  virtual void operator()(const cv::Range & range) const
100  {
101  for (int j = range.start; j < range.end; ++j)
102  {
103  int const inoff = j * inlinesize;
104  int const outoff = j * outlinesize;
105 
106  for (int i = 0; i < inImg.cols; ++i)
107  {
108  int const in = inoff + i * 2;
109  int const out = outoff + i * 3;
110  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
111 
112  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
113  outImg[out + 0] = b;
114  outImg[out + 1] = g;
115  outImg[out + 2] = r;
116  }
117  }
118  }
119 
120  private:
121  cv::Mat const & inImg;
122  unsigned char * outImg;
123  int inlinesize, outlinesize;
124  };
125 
126  class rgb565ToRGB : public cv::ParallelLoopBody
127  {
128  public:
129  rgb565ToRGB(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
130  inImg(inputImage), outImg(outImage)
131  {
132  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
133  outlinesize = outw * 3; // 3 bytes/pix for RGB
134  }
135 
136  virtual void operator()(const cv::Range & range) const
137  {
138  for (int j = range.start; j < range.end; ++j)
139  {
140  int const inoff = j * inlinesize;
141  int const outoff = j * outlinesize;
142 
143  for (int i = 0; i < inImg.cols; ++i)
144  {
145  int const in = inoff + i * 2;
146  int const out = outoff + i * 3;
147  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
148 
149  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
150  outImg[out + 0] = r;
151  outImg[out + 1] = g;
152  outImg[out + 2] = b;
153  }
154  }
155  }
156 
157  private:
158  cv::Mat const & inImg;
159  unsigned char * outImg;
160  int inlinesize, outlinesize;
161  };
162 
163  class rgb565ToRGBA : public cv::ParallelLoopBody
164  {
165  public:
166  rgb565ToRGBA(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
167  inImg(inputImage), outImg(outImage)
168  {
169  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
170  outlinesize = outw * 4; // 4 bytes/pix for RGBA
171  }
172 
173  virtual void operator()(const cv::Range & range) const
174  {
175  for (int j = range.start; j < range.end; ++j)
176  {
177  int const inoff = j * inlinesize;
178  int const outoff = j * outlinesize;
179 
180  for (int i = 0; i < inImg.cols; ++i)
181  {
182  int const in = inoff + i * 2;
183  int const out = outoff + i * 4;
184  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
185 
186  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
187  outImg[out + 0] = r;
188  outImg[out + 1] = g;
189  outImg[out + 2] = b;
190  outImg[out + 3] = (unsigned char)(255);
191  }
192  }
193  }
194 
195  private:
196  cv::Mat const & inImg;
197  unsigned char * outImg;
198  int inlinesize, outlinesize;
199  };
200 } // anonymous namespace
201 
202 #ifdef JEVOIS_PLATFORM
203 // NEON accelerated YUYV to Gray:
204 namespace
205 {
206  class yuyvToGrayNEON : public cv::ParallelLoopBody
207  {
208  public:
209  yuyvToGrayNEON(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
210  inImg(inputImage), outImg(outImage)
211  {
212  inlinesize = inputImage.cols * 2; // 2 bytes/pix for YUYV
213  outlinesize = outw * 1; // 1 byte/pix for Gray
214  initer = (inputImage.cols >> 4); // we process 16 pixels (32 input bytes) at a time
215  }
216 
217  virtual void operator()(const cv::Range & range) const
218  {
219  unsigned char const * inptr = inImg.data + range.start * inlinesize;
220  unsigned char * outptr = outImg + range.start * outlinesize;
221 
222  for (int j = range.start; j < range.end; ++j)
223  {
224  unsigned char const * ip = inptr; unsigned char * op = outptr;
225 
226  for (int i = 0; i < initer; ++i)
227  {
228  uint8x16x2_t const pixels = vld2q_u8(ip); // load 16 YUYV pixels
229  vst1q_u8(op, pixels.val[0]); // store the 16 Y values
230  ip += 32; op += 16;
231  }
232  inptr += inlinesize; outptr += outlinesize;
233  }
234  }
235 
236  private:
237  cv::Mat const & inImg;
238  unsigned char * outImg;
239  int inlinesize, outlinesize, initer;
240  };
241 } // anonymous namespace
242 #endif
243 
244 // ####################################################################################################
246 {
247  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
248  cv::Mat result;
249 
250  switch (src.fmt)
251  {
252  case V4L2_PIX_FMT_YUYV:
253 #if 0
254  //#ifdef JEVOIS_PLATFORM
255  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC1);
256  cv::parallel_for_(cv::Range(0, src.height), yuyvToGrayNEON(rawimgcv, result.data, result.cols));
257 #else
258  cv::cvtColor(rawimgcv, result, CV_YUV2GRAY_YUYV);
259 #endif
260  return result;
261 
262  case V4L2_PIX_FMT_GREY: return rawimgcv;
263 
264  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, CV_BayerBG2GRAY); return result;
265 
266  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
267  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC1);
268  cv::parallel_for_(cv::Range(0, src.height), rgb565ToGray(rawimgcv, result.data, result.cols));
269  return result;
270 
271  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
272 
273  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, CV_BGR2GRAY); return result;
274  }
275  LFATAL("Unknown RawImage pixel format");
276 }
277 
278 // ####################################################################################################
280 {
281  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
282  cv::Mat result;
283 
284  switch (src.fmt)
285  {
286  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, CV_YUV2BGR_YUYV); return result;
287  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, CV_GRAY2BGR); return result;
288  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, CV_BayerBG2BGR); return result;
289 
290  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
291  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC3);
292  cv::parallel_for_(cv::Range(0, src.height), rgb565ToBGR(rawimgcv, result.data, result.cols));
293  return result;
294 
295  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
296  case V4L2_PIX_FMT_BGR24: return rawimgcv;
297  }
298  LFATAL("Unknown RawImage pixel format");
299 }
300 
301 // ####################################################################################################
303 {
304  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
305  cv::Mat result;
306 
307  switch (src.fmt)
308  {
309  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, CV_YUV2RGB_YUYV); return result;
310  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, CV_GRAY2RGB); return result;
311  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, CV_BayerBG2RGB); return result;
312 
313  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
314  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC3);
315  cv::parallel_for_(cv::Range(0, src.height), rgb565ToRGB(rawimgcv, result.data, result.cols));
316  return result;
317 
318  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
319  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, CV_BGR2RGB); return result;
320  }
321  LFATAL("Unknown RawImage pixel format");
322 }
323 
324 // ####################################################################################################
326 {
327  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
328  cv::Mat result;
329 
330  switch (src.fmt)
331  {
332  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, CV_YUV2RGBA_YUYV); return result;
333 
334  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, CV_GRAY2RGBA); return result;
335 
336  case V4L2_PIX_FMT_SRGGB8:
337  {
338  // FIXME: we do two conversions, should get a hold of the opencv source for bayer conversions and make an RGBA
339  // version of it:
340  cv::Mat fixme;
341  cv::cvtColor(rawimgcv, fixme, CV_BayerBG2RGB);
342  cv::cvtColor(fixme, result, CV_RGB2RGBA);
343  return result;
344  }
345 
346  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
347  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC4);
348  cv::parallel_for_(cv::Range(0, src.height), rgb565ToRGBA(rawimgcv, result.data, result.cols));
349  return result;
350 
351  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
352 
353  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, CV_BGR2RGBA); return result;
354  }
355  LFATAL("Unknown RawImage pixel format");
356 }
357 
358 // ####################################################################################################
360 {
361  if (img.bytesperpix() != 2) LFATAL("Can only byteswap images with 2 bytes/pixel");
362 
363 #ifdef JEVOIS_PLATFORM
364  // Use neon acceleration, in parallel threads:
365  unsigned int ncores = std::min(4U, std::thread::hardware_concurrency());
366  size_t const nbc = img.bytesize() / ncores;
367  unsigned char * ptr = img.pixelsw<unsigned char>();
368 
369  // FIXME check for possible size rounding problems
370 
371  // Launch ncores-1 threads and we will do the last chunk in the current thread:
372  std::vector<std::future<void> > fut;
373  for (unsigned int core = 0; core < ncores-1; ++core)
374  fut.push_back(std::async(std::launch::async, [&ptr, &nbc](int core) -> void {
375  unsigned char * cptr = ptr + core * nbc;
376  for (size_t i = 0; i < nbc; i += 16) vst1q_u8(cptr + i, vrev16q_u8(vld1q_u8(cptr + i)));
377  }, core));
378 
379  // Last chunk:
380  size_t const sz = img.bytesize();
381  for (size_t i = (ncores-1) * nbc; i < sz; i += 16) vst1q_u8(ptr + i, vrev16q_u8(vld1q_u8(ptr + i)));
382 
383  // Wait for all the threads to complete:
384  for (auto & f : fut) f.get();
385 #else
386  // Use CPU:
387  size_t const sz = img.width * img.height; // size in shorts
388  unsigned short * ptr = img.pixelsw<unsigned short>();
389  for (size_t i = 0; i < sz; ++i) ptr[i] = __builtin_bswap16(ptr[i]);
390 #endif
391 }
392 
393 // ####################################################################################################
394 void jevois::rawimage::paste(jevois::RawImage const & src, jevois::RawImage & dest, int x, int y)
395 {
396  if (src.fmt != dest.fmt) LFATAL("src and dest must have the same pixel format");
397  if (x < 0 || y < 0 || x + src.width > dest.width || y + src.height > dest.height)
398  LFATAL("src does not fit within dest");
399 
400  unsigned int const bpp = src.bytesperpix();
401 
402  unsigned char const * sptr = src.pixels<unsigned char>();
403  unsigned char * dptr = dest.pixelsw<unsigned char>() + (x + y * dest.width) * bpp;
404  size_t const srclinelen = src.width * bpp;
405  size_t const dstlinelen = dest.width * bpp;
406 
407  for (unsigned int j = 0; j < src.height; ++j)
408  {
409  memcpy(dptr, sptr, srclinelen);
410  sptr += srclinelen;
411  dptr += dstlinelen;
412  }
413 }
414 
415 // ####################################################################################################
416 void jevois::rawimage::roipaste(jevois::RawImage const & src, int x, int y, unsigned int w, unsigned int h,
417  jevois::RawImage & dest, int dx, int dy)
418 {
419  if (src.fmt != dest.fmt) LFATAL("src and dest must have the same pixel format");
420  if (x < 0 || y < 0 || x + w > src.width || y + h > src.height) LFATAL("roi not within source image");
421  if (dx < 0 || dy < 0 || dx + w > dest.width || dy + h > dest.height) LFATAL("roi not within dest image");
422 
423  unsigned int const bpp = src.bytesperpix();
424 
425  unsigned char const * sptr = src.pixels<unsigned char>() + (x + y * src.width) * bpp;
426  unsigned char * dptr = dest.pixelsw<unsigned char>() + (dx + dy * dest.width) * bpp;
427  size_t const srclinelen = src.width * bpp;
428  size_t const dstlinelen = dest.width * bpp;
429 
430  for (unsigned int j = 0; j < h; ++j)
431  {
432  memcpy(dptr, sptr, w * bpp);
433  sptr += srclinelen;
434  dptr += dstlinelen;
435  }
436 }
437 
438 // ####################################################################################################
439 void jevois::rawimage::pasteGreyToYUYV(cv::Mat const & src, jevois::RawImage & dest, int x, int y)
440 {
441  if (x + src.cols > int(dest.width) || y + src.rows > int(dest.height)) LFATAL("src does not fit within dest");
442  unsigned int const bpp = dest.bytesperpix();
443 
444  unsigned char const * sptr = src.data;
445  unsigned char * dptr = dest.pixelsw<unsigned char>() + (x + y * dest.width) * bpp;
446  size_t const dststride = (dest.width - src.cols) * bpp;
447 
448  for (int j = 0; j < src.rows; ++j)
449  {
450  for (int i = 0; i < src.cols; ++i) { *dptr++ = *sptr++; *dptr++ = 0x80; }
451  dptr += dststride;
452  }
453 }
454 
455 // ####################################################################################################
456 void jevois::rawimage::drawDisk(jevois::RawImage & img, int cx, int cy, unsigned int rad, unsigned int col)
457 {
458  // From the iLab Neuromorphic Vision C++ Toolkit
459  unsigned short * const dptr = img.pixelsw<unsigned short>();
460  int const w = int(img.width);
461 
462  if (rad == 0) { if (img.coordsOk(cx, cy)) dptr[cx + w * cy] = col; return; }
463 
464  int const intrad = rad;
465  for (int y = -intrad; y <= intrad; ++y)
466  {
467  int bound = int(std::sqrt(float(intrad * intrad - y * y)));
468  for (int x = -bound; x <= bound; ++x)
469  if (img.coordsOk(x + cx, y + cy)) dptr[x + cx + w * (y + cy)] = col;
470  }
471 }
472 
473 // ####################################################################################################
474 void jevois::rawimage::drawCircle(jevois::RawImage & img, int cx, int cy, unsigned int rad,
475  unsigned int thick, unsigned int col)
476 {
477  // From the iLab Neuromorphic Vision C++ Toolkit
478  if (rad == 0) { jevois::rawimage::drawDisk(img, cx, cy, thick, col); return; }
479 
480  jevois::rawimage::drawDisk(img, cx - rad, cy, thick, col);
481  jevois::rawimage::drawDisk(img, cx + rad, cy, thick, col);
482  int bound1 = rad, bound2;
483 
484  for (unsigned int dy = 1; dy <= rad; ++dy)
485  {
486  bound2 = bound1;
487  bound1 = int(0.4999F + sqrtf(rad*rad - dy*dy));
488  for (int dx = bound1; dx <= bound2; ++dx)
489  {
490  jevois::rawimage::drawDisk(img, cx - dx, cy - dy, thick, col);
491  jevois::rawimage::drawDisk(img, cx + dx, cy - dy, thick, col);
492  jevois::rawimage::drawDisk(img, cx + dx, cy + dy, thick, col);
493  jevois::rawimage::drawDisk(img, cx - dx, cy + dy, thick, col);
494  }
495  }
496 }
497 
498 // ####################################################################################################
499 void jevois::rawimage::drawLine(jevois::RawImage & img, int x1, int y1, int x2, int y2, unsigned int thick,
500  unsigned int col)
501 {
502  // From the iLab Neuromorphic Vision C++ Toolkit
503  // from Graphics Gems / Paul Heckbert
504  int const dx = x2 - x1; int const ax = std::abs(dx) << 1; int const sx = dx < 0 ? -1 : 1;
505  int const dy = y2 - y1; int const ay = std::abs(dy) << 1; int const sy = dy < 0 ? -1 : 1;
506  int const w = img.width; int const h = img.height;
507  int x = x1, y = y1;
508 
509  if (ax > ay)
510  {
511  int d = ay - (ax >> 1);
512  for (;;)
513  {
514  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
515 
516  if (x == x2) return;
517  if (d >= 0) { y += sy; d -= ax; }
518  x += sx; d += ay;
519  }
520  }
521  else
522  {
523  int d = ax - (ay >> 1);
524  for (;;)
525  {
526  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
527  if (y == y2) return;
528  if (d >= 0) { x += sx; d -= ay; }
529  y += sy; d += ax;
530  }
531  }
532 }
533 
534 // ####################################################################################################
535 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
536  unsigned int thick, unsigned int col)
537 {
538  if (thick == 0)
539  jevois::rawimage::drawRect(img, x, y, w, h, col);
540  else
541  {
542  // Draw so that the lines are drawn on top of the bottom-right corner at (x+w-1, y+h-1):
543  if (w) --w;
544  if (h) --h;
545  jevois::rawimage::drawLine(img, x, y, x+w, y, thick, col);
546  jevois::rawimage::drawLine(img, x, y+h, x+w, y+h, thick, col);
547  jevois::rawimage::drawLine(img, x, y, x, y+h, thick, col);
548  jevois::rawimage::drawLine(img, x+w, y, x+w, y+h, thick, col);
549  }
550 }
551 // ####################################################################################################
552 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
553  unsigned int col)
554 {
555  if (w == 0) w = 1;
556  if (h == 0) h = 1;
557  if (x > int(img.width)) x = img.width;
558  if (y > int(img.height)) y = img.height;
559  if (x + w > img.width) w = img.width - x;
560  if (y + h > img.height) h = img.height - y;
561 
562  unsigned int const imgw = img.width;
563  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
564 
565  // Two horizontal lines:
566  unsigned int const offy = (h-1) * imgw;
567  for (unsigned int xx = 0; xx < w; ++xx) { b[xx] = col; b[xx + offy] = col; }
568 
569  // Two vertical lines:
570  unsigned int const offx = w-1;
571  for (unsigned int yy = 0; yy < h * imgw; yy += imgw) { b[yy] = col; b[yy + offx] = col; }
572 }
573 // ####################################################################################################
574 void jevois::rawimage::drawFilledRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
575  unsigned int col)
576 {
577  if (w == 0) w = 1;
578  if (h == 0) h = 1;
579  if (x > int(img.width)) x = img.width;
580  if (y > int(img.height)) y = img.height;
581  if (x + w > img.width) w = img.width - x;
582  if (y + h > img.height) h = img.height - y;
583 
584  unsigned int const stride = img.width - w;
585  unsigned short * b = img.pixelsw<unsigned short>() + x + y * img.width;
586 
587  for (unsigned int yy = 0; yy < h; ++yy)
588  {
589  for (unsigned int xx = 0; xx < w; ++xx) *b++ = col;
590  b += stride;
591  }
592 }
593 
594 // ####################################################################################################
595 // Font pattern definitions:
596 namespace jevois
597 {
598  namespace font
599  {
600  extern const unsigned char font10x20[95][200];
601  extern const unsigned char font11x22[95][242];
602  extern const unsigned char font12x22[95][264];
603  extern const unsigned char font14x26[95][364];
604  extern const unsigned char font15x28[95][420];
605  extern const unsigned char font16x29[95][464];
606  extern const unsigned char font20x38[95][760];
607  extern const unsigned char font5x7[95][35];
608  extern const unsigned char font6x10[95][60];
609  extern const unsigned char font7x13[95][91];
610  extern const unsigned char font8x13bold[95][104];
611  extern const unsigned char font9x15bold[95][135];
612  } // namespace font
613 } // namespace jevois
614 
615 
616 // ####################################################################################################
617 void jevois::rawimage::writeText(jevois::RawImage & img, std::string const & txt, int x, int y, unsigned int col,
619 {
620  jevois::rawimage::writeText(img, txt.c_str(), x, y, col, font);
621 }
622 
623 // ####################################################################################################
624 void jevois::rawimage::writeText(jevois::RawImage & img, char const * txt, int x, int y, unsigned int col,
626 {
627  int len = int(strlen(txt));
628  unsigned int const imgw = img.width;
629 
630  int fontw, fonth; unsigned char const * fontptr;
631  switch (font)
632  {
633  case Font5x7: fontw = 5; fonth = 7; fontptr = &jevois::font::font5x7[0][0]; break;
634  case Font6x10: fontw = 6; fonth = 10; fontptr = &jevois::font::font6x10[0][0]; break;
635  case Font7x13: fontw = 7; fonth = 13; fontptr = &jevois::font::font7x13[0][0]; break;
636  case Font8x13bold: fontw = 8; fonth = 13; fontptr = &jevois::font::font8x13bold[0][0]; break;
637  case Font9x15bold: fontw = 9; fonth = 15; fontptr = &jevois::font::font9x15bold[0][0]; break;
638  case Font10x20: fontw = 10; fonth = 20; fontptr = &jevois::font::font10x20[0][0]; break;
639  case Font11x22: fontw = 11; fonth = 22; fontptr = &jevois::font::font11x22[0][0]; break;
640  case Font12x22: fontw = 12; fonth = 22; fontptr = &jevois::font::font12x22[0][0]; break;
641  case Font14x26: fontw = 14; fonth = 26; fontptr = &jevois::font::font14x26[0][0]; break;
642  case Font15x28: fontw = 15; fonth = 28; fontptr = &jevois::font::font15x28[0][0]; break;
643  case Font16x29: fontw = 16; fonth = 29; fontptr = &jevois::font::font16x29[0][0]; break;
644  case Font20x38: fontw = 20; fonth = 38; fontptr = &jevois::font::font20x38[0][0]; break;
645  default: LFATAL("Invalid font");
646  }
647 
648  // Clip the text so that it does not go outside the image:
649  if (y < 0 || y + fonth > int(img.height)) return;
650  while (x + len * fontw > int(imgw)) { --len; if (len <= 0) return; }
651 
652  // Be nice and handle various pixel formats:
653  switch (img.bytesperpix())
654  {
655  case 2:
656  {
657  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
658 
659  for (int i = 0; i < len; ++i)
660  {
661  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
662  unsigned char const * ptr = fontptr + fontw * fonth * idx;
663  unsigned short * bb = b;
664  for (int yy = 0; yy < fonth; ++yy)
665  {
666  // Draw one line of this letter, note the transparent background:
667  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
668  bb += imgw - fontw;
669  }
670  b += fontw;
671  }
672  }
673  break;
674 
675  case 1:
676  {
677  unsigned char * b = img.pixelsw<unsigned char>() + x + y * imgw;
678 
679  for (int i = 0; i < len; ++i)
680  {
681  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
682  unsigned char const * ptr = fontptr + fontw * fonth * idx;
683  unsigned char * bb = b;
684  for (int yy = 0; yy < fonth; ++yy)
685  {
686  // Draw one line of this letter, note the transparent background:
687  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
688  bb += imgw - fontw;
689  }
690  b += fontw;
691  }
692  }
693  break;
694 
695  default:
696  LFATAL("Sorry, only 1 and 2 bytes/pixel images are supported for now");
697  }
698 }
699 
700 // ####################################################################################################
701 namespace
702 {
703  class bgrToBayer : public cv::ParallelLoopBody
704  {
705  public:
706  bgrToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
707  inImg(inputImage), outImg(outImage)
708  {
709  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
710  outlinesize = outw * 1; // 1 byte/pix for Bayer
711  }
712 
713  virtual void operator()(const cv::Range & range) const
714  {
715  for (int j = range.start; j < range.end; ++j)
716  {
717  int const inoff = j * inlinesize;
718  int const outoff = j * outlinesize;
719 
720  for (int i = 0; i < inImg.cols; i += 2)
721  {
722  int const in = inoff + i * 3;
723  int const out = outoff + i;
724 
725  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 2]; outImg[out + 1] = inImg.data[in + 4]; }
726  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 3]; }
727  }
728  }
729  }
730 
731  private:
732  cv::Mat const & inImg;
733  unsigned char * outImg;
734  int inlinesize, outlinesize;
735  };
736 
737  // ####################################################################################################
738  void convertCvBGRtoBayer(cv::Mat const & src, jevois::RawImage & dst)
739  {
740  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
741  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
742  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
743 
744  cv::parallel_for_(cv::Range(0, src.rows), bgrToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
745  }
746 } // anonymous namespace
747 
748 // ####################################################################################################
749 namespace
750 {
751  class rgbToBayer : public cv::ParallelLoopBody
752  {
753  public:
754  rgbToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
755  inImg(inputImage), outImg(outImage)
756  {
757  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
758  outlinesize = outw * 1; // 1 byte/pix for Bayer
759  }
760 
761  virtual void operator()(const cv::Range & range) const
762  {
763  for (int j = range.start; j < range.end; ++j)
764  {
765  int const inoff = j * inlinesize;
766  int const outoff = j * outlinesize;
767 
768  for (int i = 0; i < inImg.cols; i += 2)
769  {
770  int const in = inoff + i * 3;
771  int const out = outoff + i;
772 
773  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 4]; }
774  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 5]; }
775  }
776  }
777  }
778 
779  private:
780  cv::Mat const & inImg;
781  unsigned char * outImg;
782  int inlinesize, outlinesize;
783  };
784 
785  // ####################################################################################################
786  void convertCvRGBtoBayer(cv::Mat const & src, jevois::RawImage & dst)
787  {
788  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
789  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
790  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
791 
792  cv::parallel_for_(cv::Range(0, src.rows), rgbToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
793  }
794 } // anonymous namespace
795 
796 // ####################################################################################################
797 namespace
798 {
799  class grayToBayer : public cv::ParallelLoopBody
800  {
801  public:
802  grayToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
803  inImg(inputImage), outImg(outImage)
804  {
805  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
806  outlinesize = outw * 1; // 1 byte/pix for Bayer
807  }
808 
809  virtual void operator()(const cv::Range & range) const
810  {
811  for (int j = range.start; j < range.end; ++j)
812  {
813  int const inoff = j * inlinesize;
814  int const outoff = j * outlinesize;
815 
816  memcpy(&outImg[outoff], &inImg.data[inoff], inlinesize);
817  }
818  }
819 
820  private:
821  cv::Mat const & inImg;
822  unsigned char * outImg;
823  int inlinesize, outlinesize;
824  };
825 
826  // ####################################################################################################
827  void convertCvGRAYtoBayer(cv::Mat const & src, jevois::RawImage & dst)
828  {
829  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
830  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
831  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
832 
833  cv::parallel_for_(cv::Range(0, src.rows), grayToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
834  }
835 } // anonymous namespace
836 
837 // ####################################################################################################
838 namespace
839 {
840  class rgbaToBayer : public cv::ParallelLoopBody
841  {
842  public:
843  rgbaToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
844  inImg(inputImage), outImg(outImage)
845  {
846  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
847  outlinesize = outw * 1; // 1 byte/pix for Bayer
848  }
849 
850  virtual void operator()(const cv::Range & range) const
851  {
852  for (int j = range.start; j < range.end; ++j)
853  {
854  int const inoff = j * inlinesize;
855  int const outoff = j * outlinesize;
856 
857  for (int i = 0; i < inImg.cols; i += 2)
858  {
859  int const in = inoff + i * 4;
860  int const out = outoff + i;
861 
862  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 5]; }
863  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 6]; }
864  }
865  }
866  }
867 
868  private:
869  cv::Mat const & inImg;
870  unsigned char * outImg;
871  int inlinesize, outlinesize;
872  };
873 
874  // ####################################################################################################
875  void convertCvRGBAtoBayer(cv::Mat const & src, jevois::RawImage & dst)
876  {
877  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
878  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
879  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
880 
881  cv::parallel_for_(cv::Range(0, src.rows), rgbaToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
882  }
883 } // anonymous namespace
884 
885 // ####################################################################################################
886 namespace
887 {
888  class bgrToYUYV : public cv::ParallelLoopBody
889  {
890  public:
891  bgrToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
892  inImg(inputImage), outImg(outImage)
893  {
894  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
895  outlinesize = outw * 2; // 2 bytes/pix for YUYV
896  }
897 
898  virtual void operator()(const cv::Range & range) const
899  {
900  for (int j = range.start; j < range.end; ++j)
901  {
902  int const inoff = j * inlinesize;
903  int const outoff = j * outlinesize;
904 
905  for (int i = 0; i < inImg.cols; i += 2)
906  {
907  int mc = inoff + i * 3;
908  unsigned char const B1 = inImg.data[mc + 0];
909  unsigned char const G1 = inImg.data[mc + 1];
910  unsigned char const R1 = inImg.data[mc + 2];
911  unsigned char const B2 = inImg.data[mc + 3];
912  unsigned char const G2 = inImg.data[mc + 4];
913  unsigned char const R2 = inImg.data[mc + 5];
914 
915  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
916  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
917  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
918  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
919  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
920  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
921 
922  mc = outoff + i * 2;
923  outImg[mc + 0] = Y1;
924  outImg[mc + 1] = U1;
925  outImg[mc + 2] = Y2;
926  outImg[mc + 3] = V2;
927  }
928  }
929  }
930 
931  private:
932  cv::Mat const & inImg;
933  unsigned char * outImg;
934  int inlinesize, outlinesize;
935  };
936 
937  // ####################################################################################################
938  void convertCvBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
939  {
940  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
941  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
942  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
943 
944  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
945  }
946 } // anonymous namespace
947 
948 // ####################################################################################################
949 void jevois::rawimage::convertCvBGRtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
950 {
951  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
952  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
953 
954  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.data, dst.cols));
955 }
956 
957 // ####################################################################################################
958 void jevois::rawimage::pasteBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
959 {
960  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
961  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
962  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
963 
964  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>() +
965  (x + y * dst.width) * dst.bytesperpix(), dst.width));
966 }
967 
968 // ####################################################################################################
969 namespace
970 {
971  class rgbToYUYV : public cv::ParallelLoopBody
972  {
973  public:
974  rgbToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
975  inImg(inputImage), outImg(outImage)
976  {
977  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
978  outlinesize = outw * 2; // 2 bytes/pix for YUYV
979  }
980 
981  virtual void operator()(const cv::Range & range) const
982  {
983  for (int j = range.start; j < range.end; ++j)
984  {
985  int const inoff = j * inlinesize;
986  int const outoff = j * outlinesize;
987 
988  for (int i = 0; i < inImg.cols; i += 2)
989  {
990  int mc = inoff + i * 3;
991  unsigned char const R1 = inImg.data[mc + 0];
992  unsigned char const G1 = inImg.data[mc + 1];
993  unsigned char const B1 = inImg.data[mc + 2];
994  unsigned char const R2 = inImg.data[mc + 3];
995  unsigned char const G2 = inImg.data[mc + 4];
996  unsigned char const B2 = inImg.data[mc + 5];
997 
998  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
999  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
1000  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1001  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1002  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1003  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
1004 
1005  mc = outoff + i * 2;
1006  outImg[mc + 0] = Y1;
1007  outImg[mc + 1] = U1;
1008  outImg[mc + 2] = Y2;
1009  outImg[mc + 3] = V2;
1010  }
1011  }
1012  }
1013 
1014  private:
1015  cv::Mat const & inImg;
1016  unsigned char * outImg;
1017  int inlinesize, outlinesize;
1018  };
1019 
1020  // ####################################################################################################
1021  void convertCvRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1022  {
1023  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1024  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1025  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1026 
1027  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1028  }
1029 } // anonymous namespace
1030 
1031 // ####################################################################################################
1032 void jevois::rawimage::convertCvRGBtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1033 {
1034  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1035  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1036 
1037  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.data, dst.cols));
1038 }
1039 
1040 // ####################################################################################################
1041 void jevois::rawimage::pasteRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
1042 {
1043  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1044  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1045  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
1046 
1047  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>() +
1048  (x + y * dst.width) * dst.bytesperpix(), dst.width));
1049 }
1050 
1051 // ####################################################################################################
1052 namespace
1053 {
1054  class grayToYUYV : public cv::ParallelLoopBody
1055  {
1056  public:
1057  grayToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1058  inImg(inputImage), outImg(outImage)
1059  {
1060  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
1061  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1062  }
1063 
1064  virtual void operator()(const cv::Range & range) const
1065  {
1066  for (int j = range.start; j < range.end; ++j)
1067  {
1068  int const inoff = j * inlinesize;
1069  int const outoff = j * outlinesize;
1070 
1071  for (int i = 0; i < inImg.cols; ++i)
1072  {
1073  int mc = inoff + i;
1074  unsigned char const G = inImg.data[mc + 0];
1075 
1076  mc = outoff + i * 2;
1077  outImg[mc + 0] = G;
1078  outImg[mc + 1] = 0x80;
1079  }
1080  }
1081  }
1082 
1083  private:
1084  cv::Mat const & inImg;
1085  unsigned char * outImg;
1086  int inlinesize, outlinesize;
1087  };
1088 
1089  // ####################################################################################################
1090  void convertCvGRAYtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1091  {
1092  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
1093  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1094  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1095 
1096  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1097  }
1098 } // anonymous namespace
1099 
1100 // ####################################################################################################
1101 void jevois::rawimage::convertCvGRAYtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1102 {
1103  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
1104  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1105 
1106  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.data, dst.cols));
1107 }
1108 
1109 // ####################################################################################################
1110 namespace
1111 {
1112  class rgbaToYUYV : public cv::ParallelLoopBody
1113  {
1114  public:
1115  rgbaToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1116  inImg(inputImage), outImg(outImage)
1117  {
1118  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
1119  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1120  }
1121 
1122  virtual void operator()(const cv::Range & range) const
1123  {
1124  for (int j = range.start; j < range.end; ++j)
1125  {
1126  int const inoff = j * inlinesize;
1127  int const outoff = j * outlinesize;
1128 
1129  for (int i = 0; i < inImg.cols; i += 2)
1130  {
1131  int mc = inoff + i * 4;
1132  unsigned char const R1 = inImg.data[mc + 0];
1133  unsigned char const G1 = inImg.data[mc + 1];
1134  unsigned char const B1 = inImg.data[mc + 2];
1135  unsigned char const R2 = inImg.data[mc + 4];
1136  unsigned char const G2 = inImg.data[mc + 5];
1137  unsigned char const B2 = inImg.data[mc + 6];
1138 
1139  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
1140  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1141  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1142  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1143 
1144  mc = outoff + i * 2;
1145  outImg[mc + 0] = Y1;
1146  outImg[mc + 1] = U1;
1147  outImg[mc + 2] = Y2;
1148  outImg[mc + 3] = V2;
1149  }
1150  }
1151  }
1152 
1153  private:
1154  cv::Mat const & inImg;
1155  unsigned char * outImg;
1156  int inlinesize, outlinesize;
1157  };
1158 
1159  // ####################################################################################################
1160  void convertCvRGBAtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1161  {
1162  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1163  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1164  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1165 
1166  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1167  }
1168 } // anonymous namespace
1169 
1170 // ####################################################################################################
1171 void jevois::rawimage::convertCvRGBAtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1172 {
1173  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1174  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1175 
1176  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.data, dst.cols));
1177 }
1178 
1179 // ####################################################################################################
1180 void jevois::rawimage::convertCvBGRtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1181 {
1182  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
1183  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1184 
1185  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1186  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1187 
1188  switch (dst.fmt)
1189  {
1190  case V4L2_PIX_FMT_SRGGB8: convertCvBGRtoBayer(src, dst); break;
1191  case V4L2_PIX_FMT_YUYV: convertCvBGRtoYUYV(src, dst); break;
1192  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_BGR2GRAY); break;
1193  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_BGR2BGR565); break;
1194  case V4L2_PIX_FMT_MJPEG: jevois::compressBGRtoJpeg(src, dst, quality); break;
1195  case V4L2_PIX_FMT_BGR24: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1196  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1197  }
1198 }
1199 
1200 // ####################################################################################################
1201 void jevois::rawimage::convertCvRGBtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1202 {
1203  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1204  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1205 
1206  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1207  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1208 
1209  switch (dst.fmt)
1210  {
1211  case V4L2_PIX_FMT_SRGGB8: convertCvRGBtoBayer(src, dst); break;
1212  case V4L2_PIX_FMT_YUYV: convertCvRGBtoYUYV(src, dst); break;
1213  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_RGB2GRAY); break;
1214  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_RGB2BGR565); break;
1215  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBtoJpeg(src, dst, quality); break;
1216  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_RGB2BGR); break;
1217  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1218  }
1219 }
1220 
1221 // ####################################################################################################
1223 {
1224  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1225  if (dst.fmt != V4L2_PIX_FMT_GREY) LFATAL("dst must have pixel type V4L2_PIX_FMT_GREY");
1226  int const w = src.cols, h = src.rows;
1227  if (int(dst.width) < w || int(dst.height) < 4 * h) LFATAL("dst must be at least as wide and 4x as tall as src");
1228 
1229  unsigned char const * sptr = src.data; unsigned char * dptr = dst.pixelsw<unsigned char>();
1230  int const stride = int(dst.width) - w;
1231 
1232  // Do R, G, B in 3 threads then A in the current thread:
1233  std::vector<std::future<void> > fut;
1234  for (int i = 0; i < 3; ++i) fut.push_back(std::async(std::launch::async, [&](int offset) {
1235  unsigned char const * s = sptr + offset; unsigned char * d = dptr + offset * w * h;
1236  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; } }, i));
1237 
1238  unsigned char const * s = sptr + 3; unsigned char * d = dptr + 3 * w * h;
1239  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; }
1240 
1241  // Wait for all threads to complete (those should never throw):
1242  for (auto & f : fut) f.get();
1243 }
1244 
1245 // ####################################################################################################
1246 void jevois::rawimage::convertCvRGBAtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1247 {
1248  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1249  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1250 
1251  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1252 
1253  switch (dst.fmt)
1254  {
1255  case V4L2_PIX_FMT_SRGGB8: convertCvRGBAtoBayer(src, dst); break;
1256  case V4L2_PIX_FMT_YUYV: convertCvRGBAtoYUYV(src, dst); break;
1257  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_RGBA2GRAY); break;
1258  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_BGRA2BGR565); break;
1259  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBAtoJpeg(src, dst, quality); break;
1260  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_RGBA2BGR); break;
1261  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1262  }
1263 }
1264 
1265 // ####################################################################################################
1266 void jevois::rawimage::convertCvGRAYtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1267 {
1268  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and Gray pixels");
1269  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1270 
1271  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1272 
1273  switch (dst.fmt)
1274  {
1275  case V4L2_PIX_FMT_SRGGB8: convertCvGRAYtoBayer(src, dst); break;
1276  case V4L2_PIX_FMT_YUYV: convertCvGRAYtoYUYV(src, dst); break;
1277  case V4L2_PIX_FMT_GREY: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1278  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_GRAY2BGR565); break;
1279  case V4L2_PIX_FMT_MJPEG: jevois::compressGRAYtoJpeg(src, dst, quality); break;
1280  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_GRAY2BGR); break;
1281  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1282  }
1283 }
1284 
1285 // ####################################################################################################
1286 namespace
1287 {
1288  class hflipYUYV : public cv::ParallelLoopBody
1289  {
1290  public:
1291  hflipYUYV(unsigned char * outImage, size_t outw) :
1292  outImg(outImage), linesize(outw * 2) // 2 bytes/pix for YUYV
1293  { }
1294 
1295  virtual void operator()(const cv::Range & range) const
1296  {
1297  for (int j = range.start; j < range.end; ++j)
1298  {
1299  int const off = j * linesize;
1300 
1301  for (int i = 0; i < linesize / 2; i += 4)
1302  {
1303  unsigned char * ptr1 = outImg + off + i;
1304  unsigned char * ptr2 = outImg + off + linesize - 4 - i;
1305  std::swap(ptr1[0], ptr2[2]);
1306  std::swap(ptr1[1], ptr2[1]);
1307  std::swap(ptr1[2], ptr2[0]);
1308  std::swap(ptr1[3], ptr2[3]);
1309  }
1310  }
1311  }
1312 
1313  private:
1314  unsigned char * outImg;
1315  int linesize;
1316  };
1317 
1318 } // anonymous namespace
1319 
1320 // ####################################################################################################
1322 {
1323  if (img.fmt != V4L2_PIX_FMT_YUYV) LFATAL("img format must be V4L2_PIX_FMT_YUYV");
1324  cv::parallel_for_(cv::Range(0, img.height), hflipYUYV(img.pixelsw<unsigned char>(), img.width));
1325 }
1326 
cv::Mat convertToCvRGB(RawImage const &src)
Convert RawImage to OpenCV doing color conversion from any RawImage source pixel to OpenCV RGB byte...
Definition: RawImageOps.C:302
cv::Mat convertToCvRGBA(RawImage const &src)
Convert RawImage to OpenCV doing color conversion from any RawImage source pixel to OpenCV RGB-A byte...
Definition: RawImageOps.C:325
cv::Mat cvImage(RawImage const &src)
Create an OpenCV image from the existing RawImage data, sharing the pixel memory rather than copying ...
Definition: RawImageOps.C:30
const unsigned char font7x13[95][91]
Definition: Font7x13.C:8
const unsigned char font16x29[95][464]
Definition: Font16x29.C:8
bool coordsOk(int x, int y) const
Helper function to check that coords are within image bounds.
Definition: RawImage.C:76
void drawCircle(RawImage &img, int x, int y, unsigned int rad, unsigned int thick, unsigned int col)
Draw a circle in a YUYV image.
Definition: RawImageOps.C:474
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
Write some text in an image.
Definition: RawImageOps.C:617
const unsigned char font12x22[95][264]
Definition: Font12x22.C:8
cv::Mat convertToCvBGR(RawImage const &src)
Convert RawImage to OpenCV doing color conversion from any RawImage source pixel to OpenCV BGR byte...
Definition: RawImageOps.C:279
unsigned int height
Image height in pixels.
Definition: RawImage.H:146
void convertCvRGBAtoCvYUYV(cv::Mat const &src, cv::Mat &dst)
OpenCV does not provide conversion from RGBA to YUYV in cvtColor(), so this function provides it...
Definition: RawImageOps.C:1171
const unsigned char font9x15bold[95][135]
Definition: Font9x15bold.C:8
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
Draw a line in a YUYV image.
Definition: RawImageOps.C:499
T * pixelsw()
Shortcut access to pixels, read-write.
Definition: RawImageImpl.H:24
unsigned long compressGRAYtoJpeg(unsigned char const *src, int width, int height, unsigned char *dst, int quality=75)
Compress raw pixel buffer to jpeg.
Definition: Jpeg.C:94
const unsigned char font20x38[95][760]
Definition: Font20x38.C:8
const unsigned char font11x22[95][242]
Definition: Font11x22.C:8
unsigned int fmt
Pixel format as a V4L2_PIX_FMT_XXX.
Definition: RawImage.H:147
std::shared_ptr< VideoBuf > buf
The pixel data buffer.
Definition: RawImage.H:149
const unsigned char font10x20[95][200]
Definition: Font10x20.C:8
void roipaste(RawImage const &src, int x, int y, unsigned int w, unsigned int h, RawImage &dest, int dx, int dy)
Paste an ROI from an image to within another of same pixel type.
Definition: RawImageOps.C:416
const unsigned char font5x7[95][35]
Definition: Font5x7.C:7
unsigned long compressBGRtoJpeg(unsigned char const *src, int width, int height, unsigned char *dst, int quality=75)
Compress raw pixel buffer to jpeg.
Definition: Jpeg.C:52
unsigned int v4l2BytesPerPix(unsigned int fcc)
Return the number of bytes per pixel for a given V4L2_PIX_FMT_...
Definition: Utils.C:63
A raw image as coming from a V4L2 Camera and/or being sent out to a USB Gadget.
Definition: RawImage.H:110
Font
Available fonts for writeText()
Definition: RawImageOps.H:147
void hFlipYUYV(RawImage &img)
Flip a YUYV RawImage horizontally while preserving color information.
Definition: RawImageOps.C:1321
void convertCvBGRtoCvYUYV(cv::Mat const &src, cv::Mat &dst)
OpenCV does not provide conversion from BGR to YUYV in cvtColor(), so this function provides it...
Definition: RawImageOps.C:949
void pasteBGRtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a BGR byte image into a YUYV image.
Definition: RawImageOps.C:958
std::string fccstr(unsigned int fcc)
Convert a V4L2 four-cc code (V4L2_PIX_FMT_...) to a 4-char string.
Definition: Utils.C:37
cv::Mat convertToCvGray(RawImage const &src)
Convert RawImage to OpenCV doing color conversion from any RawImage source pixel to OpenCV gray byte...
Definition: RawImageOps.C:245
void convertCvBGRtoRawImage(cv::Mat const &src, RawImage &dst, int quality)
Convert a BGR cv::Mat to RawImage with already-allocated pixels and pixel type.
Definition: RawImageOps.C:1180
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level. ...
Definition: Log.H:212
unsigned long compressRGBAtoJpeg(unsigned char const *src, int width, int height, unsigned char *dst, int quality=75)
Compress raw pixel buffer to jpeg.
Definition: Jpeg.C:80
void convertCvRGBtoCvYUYV(cv::Mat const &src, cv::Mat &dst)
OpenCV does not provide conversion from RGB to YUYV in cvtColor(), so this function provides it...
Definition: RawImageOps.C:1032
void drawFilledRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int col)
Draw a filled rectangle in a YUYV image.
Definition: RawImageOps.C:574
void convertCvRGBAtoRawImage(cv::Mat const &src, RawImage &dst, int quality)
Convert an RGBA cv::Mat to RawImage with already-allocated pixels and pixel type. ...
Definition: RawImageOps.C:1246
const unsigned char font15x28[95][420]
Definition: Font15x28.C:8
void unpackCvRGBAtoGrayRawImage(cv::Mat const &src, RawImage &dst)
Split an RGBA cv::Mat into a 4x taller grey RawImage with already-allocated pixels.
Definition: RawImageOps.C:1222
const unsigned char font8x13bold[95][104]
Definition: Font8x13bold.C:8
const unsigned char font6x10[95][60]
Definition: Font6x10.C:8
void pasteGreyToYUYV(cv::Mat const &src, RawImage &dest, int dx, int dy)
Paste a grey byte image into a YUYV image.
Definition: RawImageOps.C:439
void convertCvGRAYtoRawImage(cv::Mat const &src, RawImage &dst, int quality)
Convert a Gray cv::Mat to RawImage with already-allocated pixels and pixel type.
Definition: RawImageOps.C:1266
void pasteRGBtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a RGB byte image into a YUYV image.
Definition: RawImageOps.C:1041
void convertCvGRAYtoCvYUYV(cv::Mat const &src, cv::Mat &dst)
OpenCV does not provide conversion from GRAY to YUYV in cvtColor(), so this function provides it...
Definition: RawImageOps.C:1101
unsigned long compressRGBtoJpeg(unsigned char const *src, int width, int height, unsigned char *dst, int quality=75)
Compress raw pixel buffer to jpeg.
Definition: Jpeg.C:66
T const * pixels() const
Shortcut access to pixels, read-only.
Definition: RawImageImpl.H:29
unsigned int bytesize() const
Helper function to get the total number of bytes in the RawImage, i.e., width * height * bytesperpix(...
Definition: RawImage.C:38
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
Draw a disk in a YUYV image.
Definition: RawImageOps.C:456
void drawRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int thick, unsigned int col)
Draw a rectangle in a YUYV image.
Definition: RawImageOps.C:535
void byteSwap(RawImage &img)
Swap pairs of bytes in a RawImage.
Definition: RawImageOps.C:359
const unsigned char font14x26[95][364]
Definition: Font14x26.C:8
unsigned int width
Image width in pixels.
Definition: RawImage.H:145
void convertCvRGBtoRawImage(cv::Mat const &src, RawImage &dst, int quality)
Convert a RGB cv::Mat to RawImage with already-allocated pixels and pixel type.
Definition: RawImageOps.C:1201
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
Paste an image within another of same pixel type.
Definition: RawImageOps.C:394
unsigned int bytesperpix() const
Helper function to get the number of bytes/pixel given the RawImage pixel format. ...
Definition: RawImage.C:34