JeVois  1.16
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/Util/Async.H>
22 #include <jevois/Debug/Log.H>
23 #include <jevois/Image/Jpeg.H>
24 #include <future>
25 
26 #include <linux/videodev2.h>
27 #include <cmath>
28 #include <opencv2/imgproc/imgproc.hpp>
29 
30 // ####################################################################################################
32 {
33  unsigned int bpp = jevois::v4l2BytesPerPix(src.fmt);
34 
35  switch (bpp)
36  {
37  case 3: return cv::Mat(src.height, src.width, CV_8UC3, src.buf->data());
38  case 2: return cv::Mat(src.height, src.width, CV_8UC2, src.buf->data());
39  case 1: return cv::Mat(src.height, src.width, CV_8UC1, src.buf->data());
40  default: LFATAL("Unsupported RawImage format");
41  }
42 }
43 
44 // ####################################################################################################
45 namespace
46 {
47  inline void rgb565pixrgb(unsigned short rgb565, unsigned char & r, unsigned char & g, unsigned char & b)
48  {
49  r = ((((rgb565 >> 11) & 0x1F) * 527) + 23) >> 6;
50  g = ((((rgb565 >> 5) & 0x3F) * 259) + 33) >> 6;
51  b = (((rgb565 & 0x1F) * 527) + 23) >> 6;
52  }
53 
54  class rgb565ToGray : public cv::ParallelLoopBody
55  {
56  public:
57  rgb565ToGray(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
58  inImg(inputImage), outImg(outImage)
59  {
60  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
61  outlinesize = outw * 1; // 1 byte/pix for Gray
62  }
63 
64  virtual void operator()(const cv::Range & range) const
65  {
66  for (int j = range.start; j < range.end; ++j)
67  {
68  int const inoff = j * inlinesize;
69  int const outoff = j * outlinesize;
70 
71  for (int i = 0; i < inImg.cols; ++i)
72  {
73  int const in = inoff + i * 2;
74  int const out = outoff + i;
75  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
76  unsigned char r, g, b;
77  rgb565pixrgb(rgb565, r, g, b);
78  int lum = int(r + g + b) / 3;
79  outImg[out] = lum;
80  }
81  }
82  }
83 
84  private:
85  cv::Mat const & inImg;
86  unsigned char * outImg;
87  int inlinesize, outlinesize;
88  };
89 
90  class rgb565ToBGR : public cv::ParallelLoopBody
91  {
92  public:
93  rgb565ToBGR(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
94  inImg(inputImage), outImg(outImage)
95  {
96  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
97  outlinesize = outw * 3; // 3 bytes/pix for BGR
98  }
99 
100  virtual void operator()(const cv::Range & range) const
101  {
102  for (int j = range.start; j < range.end; ++j)
103  {
104  int const inoff = j * inlinesize;
105  int const outoff = j * outlinesize;
106 
107  for (int i = 0; i < inImg.cols; ++i)
108  {
109  int const in = inoff + i * 2;
110  int const out = outoff + i * 3;
111  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
112 
113  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
114  outImg[out + 0] = b;
115  outImg[out + 1] = g;
116  outImg[out + 2] = r;
117  }
118  }
119  }
120 
121  private:
122  cv::Mat const & inImg;
123  unsigned char * outImg;
124  int inlinesize, outlinesize;
125  };
126 
127  class rgb565ToRGB : public cv::ParallelLoopBody
128  {
129  public:
130  rgb565ToRGB(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
131  inImg(inputImage), outImg(outImage)
132  {
133  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
134  outlinesize = outw * 3; // 3 bytes/pix for RGB
135  }
136 
137  virtual void operator()(const cv::Range & range) const
138  {
139  for (int j = range.start; j < range.end; ++j)
140  {
141  int const inoff = j * inlinesize;
142  int const outoff = j * outlinesize;
143 
144  for (int i = 0; i < inImg.cols; ++i)
145  {
146  int const in = inoff + i * 2;
147  int const out = outoff + i * 3;
148  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
149 
150  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
151  outImg[out + 0] = r;
152  outImg[out + 1] = g;
153  outImg[out + 2] = b;
154  }
155  }
156  }
157 
158  private:
159  cv::Mat const & inImg;
160  unsigned char * outImg;
161  int inlinesize, outlinesize;
162  };
163 
164  class rgb565ToRGBA : public cv::ParallelLoopBody
165  {
166  public:
167  rgb565ToRGBA(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
168  inImg(inputImage), outImg(outImage)
169  {
170  inlinesize = inputImage.cols * 2; // 2 bytes/pix for RGB565
171  outlinesize = outw * 4; // 4 bytes/pix for RGBA
172  }
173 
174  virtual void operator()(const cv::Range & range) const
175  {
176  for (int j = range.start; j < range.end; ++j)
177  {
178  int const inoff = j * inlinesize;
179  int const outoff = j * outlinesize;
180 
181  for (int i = 0; i < inImg.cols; ++i)
182  {
183  int const in = inoff + i * 2;
184  int const out = outoff + i * 4;
185  unsigned short const rgb565 = ((unsigned short)(inImg.data[in + 0]) << 8) | inImg.data[in + 1];
186 
187  unsigned char r, g, b; rgb565pixrgb(rgb565, r, g, b);
188  outImg[out + 0] = r;
189  outImg[out + 1] = g;
190  outImg[out + 2] = b;
191  outImg[out + 3] = (unsigned char)(255);
192  }
193  }
194  }
195 
196  private:
197  cv::Mat const & inImg;
198  unsigned char * outImg;
199  int inlinesize, outlinesize;
200  };
201 } // anonymous namespace
202 
203 #ifdef JEVOIS_PLATFORM
204 // NEON accelerated YUYV to Gray:
205 namespace
206 {
207  class yuyvToGrayNEON : public cv::ParallelLoopBody
208  {
209  public:
210  yuyvToGrayNEON(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
211  inImg(inputImage), outImg(outImage)
212  {
213  inlinesize = inputImage.cols * 2; // 2 bytes/pix for YUYV
214  outlinesize = outw * 1; // 1 byte/pix for Gray
215  initer = (inputImage.cols >> 4); // we process 16 pixels (32 input bytes) at a time
216  }
217 
218  virtual void operator()(const cv::Range & range) const
219  {
220  unsigned char const * inptr = inImg.data + range.start * inlinesize;
221  unsigned char * outptr = outImg + range.start * outlinesize;
222 
223  for (int j = range.start; j < range.end; ++j)
224  {
225  unsigned char const * ip = inptr; unsigned char * op = outptr;
226 
227  for (int i = 0; i < initer; ++i)
228  {
229  uint8x16x2_t const pixels = vld2q_u8(ip); // load 16 YUYV pixels
230  vst1q_u8(op, pixels.val[0]); // store the 16 Y values
231  ip += 32; op += 16;
232  }
233  inptr += inlinesize; outptr += outlinesize;
234  }
235  }
236 
237  private:
238  cv::Mat const & inImg;
239  unsigned char * outImg;
240  int inlinesize, outlinesize, initer;
241  };
242 } // anonymous namespace
243 #endif
244 
245 // ####################################################################################################
247 {
248  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
249  cv::Mat result;
250 
251  switch (src.fmt)
252  {
253  case V4L2_PIX_FMT_GREY: return rawimgcv;
254 
255  case V4L2_PIX_FMT_YUYV:
256 #if 0
257  //#ifdef JEVOIS_PLATFORM
258  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC1);
259  cv::parallel_for_(cv::Range(0, src.height), yuyvToGrayNEON(rawimgcv, result.data, result.cols));
260 #else
261  cv::cvtColor(rawimgcv, result, cv::COLOR_YUV2GRAY_YUYV);
262 #endif
263  return result;
264 
265  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, cv::COLOR_BayerBG2GRAY); return result;
266 
267  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
268  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC1);
269  cv::parallel_for_(cv::Range(0, src.height), rgb565ToGray(rawimgcv, result.data, result.cols));
270  return result;
271 
272  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
273 
274  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, cv::COLOR_BGR2GRAY); return result;
275 
276  case V4L2_PIX_FMT_RGB24: cv::cvtColor(rawimgcv, result, cv::COLOR_RGB2GRAY); return result;
277  }
278  LFATAL("Unknown RawImage pixel format");
279 }
280 
281 // ####################################################################################################
283 {
284  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
285  cv::Mat result;
286 
287  switch (src.fmt)
288  {
289  case V4L2_PIX_FMT_BGR24: return rawimgcv;
290 
291  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, cv::COLOR_YUV2BGR_YUYV); return result;
292  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, cv::COLOR_GRAY2BGR); return result;
293  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, cv::COLOR_BayerBG2BGR); return result;
294 
295  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
296  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC3);
297  cv::parallel_for_(cv::Range(0, src.height), rgb565ToBGR(rawimgcv, result.data, result.cols));
298  return result;
299 
300  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
301  case V4L2_PIX_FMT_RGB24: cv::cvtColor(rawimgcv, result, cv::COLOR_RGB2BGR); return result;
302  }
303  LFATAL("Unknown RawImage pixel format");
304 }
305 
306 // ####################################################################################################
308 {
309  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
310  cv::Mat result;
311 
312  switch (src.fmt)
313  {
314  case V4L2_PIX_FMT_RGB24: return rawimgcv;
315 
316  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, cv::COLOR_YUV2RGB_YUYV); return result;
317  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, cv::COLOR_GRAY2RGB); return result;
318  case V4L2_PIX_FMT_SRGGB8: cv::cvtColor(rawimgcv, result, cv::COLOR_BayerBG2RGB); return result;
319 
320  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
321  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC3);
322  cv::parallel_for_(cv::Range(0, src.height), rgb565ToRGB(rawimgcv, result.data, result.cols));
323  return result;
324 
325  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
326  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, cv::COLOR_BGR2RGB); return result;
327  }
328  LFATAL("Unknown RawImage pixel format");
329 }
330 
331 // ####################################################################################################
333 {
334  cv::Mat rawimgcv = jevois::rawimage::cvImage(src);
335  cv::Mat result;
336 
337  switch (src.fmt)
338  {
339  case V4L2_PIX_FMT_YUYV: cv::cvtColor(rawimgcv, result, cv::COLOR_YUV2RGBA_YUYV); return result;
340 
341  case V4L2_PIX_FMT_GREY: cv::cvtColor(rawimgcv, result, cv::COLOR_GRAY2RGBA); return result;
342 
343  case V4L2_PIX_FMT_SRGGB8:
344  {
345  // FIXME: we do two conversions, should get a hold of the opencv source for bayer conversions and make an RGBA
346  // version of it:
347  cv::Mat fixme;
348  cv::cvtColor(rawimgcv, fixme, cv::COLOR_BayerBG2RGB);
349  cv::cvtColor(fixme, result, cv::COLOR_RGB2RGBA);
350  return result;
351  }
352 
353  case V4L2_PIX_FMT_RGB565: // camera outputs big-endian pixels, cv::cvtColor() assumes little-endian
354  result = cv::Mat(cv::Size(src.width, src.height), CV_8UC4);
355  cv::parallel_for_(cv::Range(0, src.height), rgb565ToRGBA(rawimgcv, result.data, result.cols));
356  return result;
357 
358  case V4L2_PIX_FMT_MJPEG: LFATAL("MJPEG not supported");
359 
360  case V4L2_PIX_FMT_BGR24: cv::cvtColor(rawimgcv, result, cv::COLOR_BGR2RGBA); return result;
361 
362  case V4L2_PIX_FMT_RGB24: cv::cvtColor(rawimgcv, result, cv::COLOR_RGB2RGBA); return result;
363  }
364  LFATAL("Unknown RawImage pixel format");
365 }
366 
367 // ####################################################################################################
369 {
370  if (img.bytesperpix() != 2) LFATAL("Can only byteswap images with 2 bytes/pixel");
371 
372 #ifdef JEVOIS_PLATFORM
373  // Use neon acceleration, in parallel threads:
374  unsigned int ncores = 4; //std::min(4U, std::thread::hardware_concurrency());
375  size_t const nbc = img.bytesize() / ncores;
376  unsigned char * ptr = img.pixelsw<unsigned char>();
377 
378  // FIXME check for possible size rounding problems
379 
380  // Launch ncores-1 threads and we will do the last chunk in the current thread:
381  std::vector<std::future<void> > fut;
382  for (unsigned int core = 0; core < ncores-1; ++core)
383  fut.push_back(jevois::async([&ptr, &nbc](int core) -> void {
384  unsigned char * cptr = ptr + core * nbc;
385  for (size_t i = 0; i < nbc; i += 16) vst1q_u8(cptr + i, vrev16q_u8(vld1q_u8(cptr + i)));
386  }, core));
387 
388  // Last chunk:
389  size_t const sz = img.bytesize();
390  for (size_t i = (ncores-1) * nbc; i < sz; i += 16) vst1q_u8(ptr + i, vrev16q_u8(vld1q_u8(ptr + i)));
391 
392  // Wait for all the threads to complete:
393  for (auto & f : fut) f.get();
394 #else
395  // Use CPU:
396  size_t const sz = img.width * img.height; // size in shorts
397  unsigned short * ptr = img.pixelsw<unsigned short>();
398  for (size_t i = 0; i < sz; ++i) ptr[i] = __builtin_bswap16(ptr[i]);
399 #endif
400 }
401 
402 // ####################################################################################################
403 void jevois::rawimage::paste(jevois::RawImage const & src, jevois::RawImage & dest, int x, int y)
404 {
405  if (src.fmt != dest.fmt) LFATAL("src and dest must have the same pixel format");
406  if (x < 0 || y < 0 || x + src.width > dest.width || y + src.height > dest.height)
407  LFATAL("src does not fit within dest");
408 
409  unsigned int const bpp = src.bytesperpix();
410 
411  unsigned char const * sptr = src.pixels<unsigned char>();
412  unsigned char * dptr = dest.pixelsw<unsigned char>() + (x + y * dest.width) * bpp;
413  size_t const srclinelen = src.width * bpp;
414  size_t const dstlinelen = dest.width * bpp;
415 
416  for (unsigned int j = 0; j < src.height; ++j)
417  {
418  memcpy(dptr, sptr, srclinelen);
419  sptr += srclinelen;
420  dptr += dstlinelen;
421  }
422 }
423 
424 // ####################################################################################################
425 void jevois::rawimage::roipaste(jevois::RawImage const & src, int x, int y, unsigned int w, unsigned int h,
426  jevois::RawImage & dest, int dx, int dy)
427 {
428  if (src.fmt != dest.fmt) LFATAL("src and dest must have the same pixel format");
429  if (x < 0 || y < 0 || x + w > src.width || y + h > src.height) LFATAL("roi not within source image");
430  if (dx < 0 || dy < 0 || dx + w > dest.width || dy + h > dest.height) LFATAL("roi not within dest image");
431 
432  unsigned int const bpp = src.bytesperpix();
433 
434  unsigned char const * sptr = src.pixels<unsigned char>() + (x + y * src.width) * bpp;
435  unsigned char * dptr = dest.pixelsw<unsigned char>() + (dx + dy * dest.width) * bpp;
436  size_t const srclinelen = src.width * bpp;
437  size_t const dstlinelen = dest.width * bpp;
438 
439  for (unsigned int j = 0; j < h; ++j)
440  {
441  memcpy(dptr, sptr, w * bpp);
442  sptr += srclinelen;
443  dptr += dstlinelen;
444  }
445 }
446 
447 // ####################################################################################################
448 void jevois::rawimage::pasteGreyToYUYV(cv::Mat const & src, jevois::RawImage & dest, int x, int y)
449 {
450  if (x + src.cols > int(dest.width) || y + src.rows > int(dest.height)) LFATAL("src does not fit within dest");
451  unsigned int const bpp = dest.bytesperpix();
452 
453  unsigned char const * sptr = src.data;
454  unsigned char * dptr = dest.pixelsw<unsigned char>() + (x + y * dest.width) * bpp;
455  size_t const dststride = (dest.width - src.cols) * bpp;
456 
457  for (int j = 0; j < src.rows; ++j)
458  {
459  for (int i = 0; i < src.cols; ++i) { *dptr++ = *sptr++; *dptr++ = 0x80; }
460  dptr += dststride;
461  }
462 }
463 
464 // ####################################################################################################
465 void jevois::rawimage::drawDisk(jevois::RawImage & img, int cx, int cy, unsigned int rad, unsigned int col)
466 {
467  // From the iLab Neuromorphic Vision C++ Toolkit
468  unsigned short * const dptr = img.pixelsw<unsigned short>();
469  int const w = int(img.width);
470 
471  if (rad == 0) { if (img.coordsOk(cx, cy)) dptr[cx + w * cy] = col; return; }
472 
473  int const intrad = rad;
474  for (int y = -intrad; y <= intrad; ++y)
475  {
476  int bound = int(std::sqrt(float(intrad * intrad - y * y)));
477  for (int x = -bound; x <= bound; ++x)
478  if (img.coordsOk(x + cx, y + cy)) dptr[x + cx + w * (y + cy)] = col;
479  }
480 }
481 
482 // ####################################################################################################
483 void jevois::rawimage::drawCircle(jevois::RawImage & img, int cx, int cy, unsigned int rad,
484  unsigned int thick, unsigned int col)
485 {
486  // From the iLab Neuromorphic Vision C++ Toolkit
487  if (rad == 0) { jevois::rawimage::drawDisk(img, cx, cy, thick, col); return; }
488 
489  jevois::rawimage::drawDisk(img, cx - rad, cy, thick, col);
490  jevois::rawimage::drawDisk(img, cx + rad, cy, thick, col);
491  int bound1 = rad, bound2;
492 
493  for (unsigned int dy = 1; dy <= rad; ++dy)
494  {
495  bound2 = bound1;
496  bound1 = int(0.4999F + sqrtf(rad*rad - dy*dy));
497  for (int dx = bound1; dx <= bound2; ++dx)
498  {
499  jevois::rawimage::drawDisk(img, cx - dx, cy - dy, thick, col);
500  jevois::rawimage::drawDisk(img, cx + dx, cy - dy, thick, col);
501  jevois::rawimage::drawDisk(img, cx + dx, cy + dy, thick, col);
502  jevois::rawimage::drawDisk(img, cx - dx, cy + dy, thick, col);
503  }
504  }
505 }
506 
507 // ####################################################################################################
508 namespace
509 {
510  // Liang-Barsky algo from http://hinjang.com/articles/04.html#eight
511  inline bool isZero(double a)
512  { return (a < 0.0001 && a > -0.0001 ); }
513 
514  bool clipT(double num, double denom, double & tE, double & tL)
515  {
516  if (isZero(denom)) return (num <= 0.0);
517 
518  double t = num / denom;
519 
520  if (denom > 0.0) {
521  if (t > tL) return false;
522  if (t > tE) tE = t;
523  } else {
524  if (t < tE) return false;
525  if (t < tL) tL = t;
526  }
527  return true;
528  }
529 }
530 
531 // ####################################################################################################
532 // Liang-Barsky algo from http://hinjang.com/articles/04.html#eight
533 bool jevois::rawimage::clipLine(int wxmin, int wymin, int wxmax, int wymax, int & x1, int & y1, int & x2, int & y2)
534 {
535  // This algo does not handle lines completely outside the window? quick test here that should work for most lines (but
536  // not all, may need to fix later):
537  if (x1 < wxmin && x2 < wxmin) return false;
538  if (x1 >= wxmax && x2 >= wxmax) return false;
539  if (y1 < wymin && y2 < wymin) return false;
540  if (y1 >= wymax && y2 >= wymax) return false;
541 
542  int const toofar = 5000;
543  if (x1 < -toofar || x1 > toofar || y1 < -toofar || y1 > toofar) return false;
544  if (x2 < -toofar || x2 > toofar || y2 < -toofar || y2 > toofar) return false;
545 
546  --wxmax; --wymax; // exclude right and bottom edges of the window
547 
548  double dx = x2 - x1, dy = y2 - y1;
549  if (isZero(dx) && isZero(dy)) return true;
550 
551  double tE = 0.0, tL = 1.0;
552 
553  if (clipT(wxmin - x1, dx, tE, tL) && clipT(x1 - wxmax, -dx, tE, tL) &&
554  clipT(wymin - y1, dy, tE, tL) && clipT(y1 - wymax, -dy, tE, tL))
555  {
556  if (tL < 1) { x2 = x1 + tL * dx; y2 = y1 + tL * dy; }
557  if (tE > 0) { x1 += tE * dx; y1 += tE * dy; }
558  }
559 
560  return true;
561 }
562 
563 // ####################################################################################################
564 void jevois::rawimage::drawLine(jevois::RawImage & img, int x1, int y1, int x2, int y2, unsigned int thick,
565  unsigned int col)
566 {
567  // If thickness is very large, refuse to draw, as it will hang for a very long time and be confusing:
568  if (thick > 500) LFATAL("Thickness " << thick << " too large. Did you mistakenly swap thick and col?");
569 
570  // First clip the line so we don't waste time trying to sometimes draw very long lines that may result from singular
571  // 3D projections:
572  if (jevois::rawimage::clipLine(0, 0, img.width, img.height, x1, y1, x2, y2) == false) return; // line fully outside
573 
574  // From the iLab Neuromorphic Vision C++ Toolkit
575  // from Graphics Gems / Paul Heckbert
576  int const dx = x2 - x1; int const ax = std::abs(dx) << 1; int const sx = dx < 0 ? -1 : 1;
577  int const dy = y2 - y1; int const ay = std::abs(dy) << 1; int const sy = dy < 0 ? -1 : 1;
578  int const w = img.width; int const h = img.height;
579  int x = x1, y = y1;
580 
581  if (ax > ay)
582  {
583  int d = ay - (ax >> 1);
584  for (;;)
585  {
586  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
587 
588  if (x == x2) return;
589  if (d >= 0) { y += sy; d -= ax; }
590  x += sx; d += ay;
591  }
592  }
593  else
594  {
595  int d = ax - (ay >> 1);
596  for (;;)
597  {
598  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
599  if (y == y2) return;
600  if (d >= 0) { x += sx; d -= ay; }
601  y += sy; d += ax;
602  }
603  }
604 }
605 
606 // ####################################################################################################
607 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
608  unsigned int thick, unsigned int col)
609 {
610  if (thick == 0)
611  jevois::rawimage::drawRect(img, x, y, w, h, col);
612  else
613  {
614  // Draw so that the lines are drawn on top of the bottom-right corner at (x+w-1, y+h-1):
615  if (w) --w;
616  if (h) --h;
617  jevois::rawimage::drawLine(img, x, y, x+w, y, thick, col);
618  jevois::rawimage::drawLine(img, x, y+h, x+w, y+h, thick, col);
619  jevois::rawimage::drawLine(img, x, y, x, y+h, thick, col);
620  jevois::rawimage::drawLine(img, x+w, y, x+w, y+h, thick, col);
621  }
622 }
623 // ####################################################################################################
624 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
625  unsigned int col)
626 {
627  if (w == 0) w = 1;
628  if (h == 0) h = 1;
629  if (x >= int(img.width)) x = img.width - 1;
630  if (y >= int(img.height)) y = img.height - 1;
631  if (x + w > img.width) w = img.width - x;
632  if (y + h > img.height) h = img.height - y;
633 
634  unsigned int const imgw = img.width;
635  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
636 
637  // Two horizontal lines:
638  unsigned int const offy = (h-1) * imgw;
639  for (unsigned int xx = 0; xx < w; ++xx) { b[xx] = col; b[xx + offy] = col; }
640 
641  // Two vertical lines:
642  unsigned int const offx = w-1;
643  for (unsigned int yy = 0; yy < h * imgw; yy += imgw) { b[yy] = col; b[yy + offx] = col; }
644 }
645 // ####################################################################################################
646 void jevois::rawimage::drawFilledRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
647  unsigned int col)
648 {
649  if (w == 0) w = 1;
650  if (h == 0) h = 1;
651  if (x >= int(img.width)) x = img.width - 1;
652  if (y >= int(img.height)) y = img.height - 1;
653  if (x + w > img.width) w = img.width - x;
654  if (y + h > img.height) h = img.height - y;
655 
656  unsigned int const stride = img.width - w;
657  unsigned short * b = img.pixelsw<unsigned short>() + x + y * img.width;
658 
659  for (unsigned int yy = 0; yy < h; ++yy)
660  {
661  for (unsigned int xx = 0; xx < w; ++xx) *b++ = col;
662  b += stride;
663  }
664 }
665 
666 // ####################################################################################################
667 // Font pattern definitions:
668 namespace jevois
669 {
670  namespace font
671  {
672  extern const unsigned char font10x20[95][200];
673  extern const unsigned char font11x22[95][242];
674  extern const unsigned char font12x22[95][264];
675  extern const unsigned char font14x26[95][364];
676  extern const unsigned char font15x28[95][420];
677  extern const unsigned char font16x29[95][464];
678  extern const unsigned char font20x38[95][760];
679  extern const unsigned char font5x7[95][35];
680  extern const unsigned char font6x10[95][60];
681  extern const unsigned char font7x13[95][91];
682  extern const unsigned char font8x13bold[95][104];
683  extern const unsigned char font9x15bold[95][135];
684  } // namespace font
685 } // namespace jevois
686 
687 
688 // ####################################################################################################
689 void jevois::rawimage::writeText(jevois::RawImage & img, std::string const & txt, int x, int y, unsigned int col,
691 {
692  jevois::rawimage::writeText(img, txt.c_str(), x, y, col, font);
693 }
694 
695 // ####################################################################################################
696 void jevois::rawimage::writeText(jevois::RawImage & img, char const * txt, int x, int y, unsigned int col,
698 {
699  int len = int(strlen(txt));
700  unsigned int const imgw = img.width;
701 
702  int fontw, fonth; unsigned char const * fontptr;
703  switch (font)
704  {
705  case Font5x7: fontw = 5; fonth = 7; fontptr = &jevois::font::font5x7[0][0]; break;
706  case Font6x10: fontw = 6; fonth = 10; fontptr = &jevois::font::font6x10[0][0]; break;
707  case Font7x13: fontw = 7; fonth = 13; fontptr = &jevois::font::font7x13[0][0]; break;
708  case Font8x13bold: fontw = 8; fonth = 13; fontptr = &jevois::font::font8x13bold[0][0]; break;
709  case Font9x15bold: fontw = 9; fonth = 15; fontptr = &jevois::font::font9x15bold[0][0]; break;
710  case Font10x20: fontw = 10; fonth = 20; fontptr = &jevois::font::font10x20[0][0]; break;
711  case Font11x22: fontw = 11; fonth = 22; fontptr = &jevois::font::font11x22[0][0]; break;
712  case Font12x22: fontw = 12; fonth = 22; fontptr = &jevois::font::font12x22[0][0]; break;
713  case Font14x26: fontw = 14; fonth = 26; fontptr = &jevois::font::font14x26[0][0]; break;
714  case Font15x28: fontw = 15; fonth = 28; fontptr = &jevois::font::font15x28[0][0]; break;
715  case Font16x29: fontw = 16; fonth = 29; fontptr = &jevois::font::font16x29[0][0]; break;
716  case Font20x38: fontw = 20; fonth = 38; fontptr = &jevois::font::font20x38[0][0]; break;
717  default: LFATAL("Invalid font");
718  }
719 
720  // Clip the text so that it does not go outside the image:
721  if (y < 0 || y + fonth > int(img.height)) return;
722  while (x + len * fontw > int(imgw)) { --len; if (len <= 0) return; }
723 
724  // Be nice and handle various pixel formats:
725  switch (img.bytesperpix())
726  {
727  case 2:
728  {
729  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
730 
731  for (int i = 0; i < len; ++i)
732  {
733  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
734  unsigned char const * ptr = fontptr + fontw * fonth * idx;
735  unsigned short * bb = b;
736  for (int yy = 0; yy < fonth; ++yy)
737  {
738  // Draw one line of this letter, note the transparent background:
739  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
740  bb += imgw - fontw;
741  }
742  b += fontw;
743  }
744  }
745  break;
746 
747  case 1:
748  {
749  unsigned char * b = img.pixelsw<unsigned char>() + x + y * imgw;
750 
751  for (int i = 0; i < len; ++i)
752  {
753  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
754  unsigned char const * ptr = fontptr + fontw * fonth * idx;
755  unsigned char * bb = b;
756  for (int yy = 0; yy < fonth; ++yy)
757  {
758  // Draw one line of this letter, note the transparent background:
759  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
760  bb += imgw - fontw;
761  }
762  b += fontw;
763  }
764  }
765  break;
766 
767  default:
768  LFATAL("Sorry, only 1 and 2 bytes/pixel images are supported for now");
769  }
770 }
771 
772 // ####################################################################################################
773 namespace
774 {
775  class bgrToBayer : public cv::ParallelLoopBody
776  {
777  public:
778  bgrToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
779  inImg(inputImage), outImg(outImage)
780  {
781  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
782  outlinesize = outw * 1; // 1 byte/pix for Bayer
783  }
784 
785  virtual void operator()(const cv::Range & range) const
786  {
787  for (int j = range.start; j < range.end; ++j)
788  {
789  int const inoff = j * inlinesize;
790  int const outoff = j * outlinesize;
791 
792  for (int i = 0; i < inImg.cols; i += 2)
793  {
794  int const in = inoff + i * 3;
795  int const out = outoff + i;
796 
797  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 2]; outImg[out + 1] = inImg.data[in + 4]; }
798  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 3]; }
799  }
800  }
801  }
802 
803  private:
804  cv::Mat const & inImg;
805  unsigned char * outImg;
806  int inlinesize, outlinesize;
807  };
808 
809  // ####################################################################################################
810  void convertCvBGRtoBayer(cv::Mat const & src, jevois::RawImage & dst)
811  {
812  if (src.type() != CV_8UC3)
813  LFATAL("src must have type CV_8UC3 and BGR pixels; your image has " << jevois::cvtypestr(src.type()));
814  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
815  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
816 
817  cv::parallel_for_(cv::Range(0, src.rows), bgrToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
818  }
819 } // anonymous namespace
820 
821 // ####################################################################################################
822 namespace
823 {
824  class rgbToBayer : public cv::ParallelLoopBody
825  {
826  public:
827  rgbToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
828  inImg(inputImage), outImg(outImage)
829  {
830  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
831  outlinesize = outw * 1; // 1 byte/pix for Bayer
832  }
833 
834  virtual void operator()(const cv::Range & range) const
835  {
836  for (int j = range.start; j < range.end; ++j)
837  {
838  int const inoff = j * inlinesize;
839  int const outoff = j * outlinesize;
840 
841  for (int i = 0; i < inImg.cols; i += 2)
842  {
843  int const in = inoff + i * 3;
844  int const out = outoff + i;
845 
846  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 4]; }
847  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 5]; }
848  }
849  }
850  }
851 
852  private:
853  cv::Mat const & inImg;
854  unsigned char * outImg;
855  int inlinesize, outlinesize;
856  };
857 
858  // ####################################################################################################
859  void convertCvRGBtoBayer(cv::Mat const & src, jevois::RawImage & dst)
860  {
861  if (src.type() != CV_8UC3)
862  LFATAL("src must have type CV_8UC3 and RGB pixels; your image has " << jevois::cvtypestr(src.type()));
863  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
864  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
865 
866  cv::parallel_for_(cv::Range(0, src.rows), rgbToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
867  }
868 } // anonymous namespace
869 
870 // ####################################################################################################
871 namespace
872 {
873  class grayToBayer : public cv::ParallelLoopBody
874  {
875  public:
876  grayToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
877  inImg(inputImage), outImg(outImage)
878  {
879  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
880  outlinesize = outw * 1; // 1 byte/pix for Bayer
881  }
882 
883  virtual void operator()(const cv::Range & range) const
884  {
885  for (int j = range.start; j < range.end; ++j)
886  {
887  int const inoff = j * inlinesize;
888  int const outoff = j * outlinesize;
889 
890  memcpy(&outImg[outoff], &inImg.data[inoff], inlinesize);
891  }
892  }
893 
894  private:
895  cv::Mat const & inImg;
896  unsigned char * outImg;
897  int inlinesize, outlinesize;
898  };
899 
900  // ####################################################################################################
901  void convertCvGRAYtoBayer(cv::Mat const & src, jevois::RawImage & dst)
902  {
903  if (src.type() != CV_8UC1)
904  LFATAL("src must have type CV_8UC1 and GRAY pixels; your image has " << jevois::cvtypestr(src.type()));
905  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
906  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
907 
908  cv::parallel_for_(cv::Range(0, src.rows), grayToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
909  }
910 } // anonymous namespace
911 
912 // ####################################################################################################
913 namespace
914 {
915  class rgbaToBayer : public cv::ParallelLoopBody
916  {
917  public:
918  rgbaToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
919  inImg(inputImage), outImg(outImage)
920  {
921  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
922  outlinesize = outw * 1; // 1 byte/pix for Bayer
923  }
924 
925  virtual void operator()(const cv::Range & range) const
926  {
927  for (int j = range.start; j < range.end; ++j)
928  {
929  int const inoff = j * inlinesize;
930  int const outoff = j * outlinesize;
931 
932  for (int i = 0; i < inImg.cols; i += 2)
933  {
934  int const in = inoff + i * 4;
935  int const out = outoff + i;
936 
937  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 5]; }
938  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 6]; }
939  }
940  }
941  }
942 
943  private:
944  cv::Mat const & inImg;
945  unsigned char * outImg;
946  int inlinesize, outlinesize;
947  };
948 
949  // ####################################################################################################
950  void convertCvRGBAtoBayer(cv::Mat const & src, jevois::RawImage & dst)
951  {
952  if (src.type() != CV_8UC4)
953  LFATAL("src must have type CV_8UC4 and RGBA pixels; your image has " << jevois::cvtypestr(src.type()));
954  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
955  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
956 
957  cv::parallel_for_(cv::Range(0, src.rows), rgbaToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
958  }
959 } // anonymous namespace
960 
961 // ####################################################################################################
962 namespace
963 {
964  class bgrToYUYV : public cv::ParallelLoopBody
965  {
966  public:
967  bgrToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
968  inImg(inputImage), outImg(outImage)
969  {
970  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
971  outlinesize = outw * 2; // 2 bytes/pix for YUYV
972  }
973 
974  virtual void operator()(const cv::Range & range) const
975  {
976  for (int j = range.start; j < range.end; ++j)
977  {
978  int const inoff = j * inlinesize;
979  int const outoff = j * outlinesize;
980 
981  for (int i = 0; i < inImg.cols; i += 2)
982  {
983  int mc = inoff + i * 3;
984  unsigned char const B1 = inImg.data[mc + 0];
985  unsigned char const G1 = inImg.data[mc + 1];
986  unsigned char const R1 = inImg.data[mc + 2];
987  unsigned char const B2 = inImg.data[mc + 3];
988  unsigned char const G2 = inImg.data[mc + 4];
989  unsigned char const R2 = inImg.data[mc + 5];
990 
991  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
992  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
993  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
994  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
995  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
996  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
997 
998  mc = outoff + i * 2;
999  outImg[mc + 0] = Y1;
1000  outImg[mc + 1] = U1;
1001  outImg[mc + 2] = Y2;
1002  outImg[mc + 3] = V2;
1003  }
1004  }
1005  }
1006 
1007  private:
1008  cv::Mat const & inImg;
1009  unsigned char * outImg;
1010  int inlinesize, outlinesize;
1011  };
1012 
1013  // ####################################################################################################
1014  void convertCvBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1015  {
1016  if (src.type() != CV_8UC3)
1017  LFATAL("src must have type CV_8UC3 and BGR pixels; your image has " << jevois::cvtypestr(src.type()));
1018  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1019  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1020 
1021  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1022  }
1023 } // anonymous namespace
1024 
1025 // ####################################################################################################
1026 void jevois::rawimage::convertCvBGRtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1027 {
1028  if (src.type() != CV_8UC3)
1029  LFATAL("src must have type CV_8UC3 and BGR pixels; your image has " << jevois::cvtypestr(src.type()));
1030  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1031 
1032  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.data, dst.cols));
1033 }
1034 
1035 // ####################################################################################################
1036 void jevois::rawimage::pasteBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
1037 {
1038  if (src.type() != CV_8UC3)
1039  LFATAL("src must have type CV_8UC3 and BGR pixels; your image has " << jevois::cvtypestr(src.type()));
1040  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1041  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
1042 
1043  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>() +
1044  (x + y * dst.width) * dst.bytesperpix(), dst.width));
1045 }
1046 
1047 // ####################################################################################################
1048 namespace
1049 {
1050  class rgbToYUYV : public cv::ParallelLoopBody
1051  {
1052  public:
1053  rgbToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1054  inImg(inputImage), outImg(outImage)
1055  {
1056  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
1057  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1058  }
1059 
1060  virtual void operator()(const cv::Range & range) const
1061  {
1062  for (int j = range.start; j < range.end; ++j)
1063  {
1064  int const inoff = j * inlinesize;
1065  int const outoff = j * outlinesize;
1066 
1067  for (int i = 0; i < inImg.cols; i += 2)
1068  {
1069  int mc = inoff + i * 3;
1070  unsigned char const R1 = inImg.data[mc + 0];
1071  unsigned char const G1 = inImg.data[mc + 1];
1072  unsigned char const B1 = inImg.data[mc + 2];
1073  unsigned char const R2 = inImg.data[mc + 3];
1074  unsigned char const G2 = inImg.data[mc + 4];
1075  unsigned char const B2 = inImg.data[mc + 5];
1076 
1077  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
1078  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
1079  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1080  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1081  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1082  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
1083 
1084  mc = outoff + i * 2;
1085  outImg[mc + 0] = Y1;
1086  outImg[mc + 1] = U1;
1087  outImg[mc + 2] = Y2;
1088  outImg[mc + 3] = V2;
1089  }
1090  }
1091  }
1092 
1093  private:
1094  cv::Mat const & inImg;
1095  unsigned char * outImg;
1096  int inlinesize, outlinesize;
1097  };
1098 
1099  // ####################################################################################################
1100  void convertCvRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1101  {
1102  if (src.type() != CV_8UC3)
1103  LFATAL("src must have type CV_8UC3 and RGB pixels; your image has " << jevois::cvtypestr(src.type()));
1104  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1105  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1106 
1107  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1108  }
1109 } // anonymous namespace
1110 
1111 // ####################################################################################################
1112 void jevois::rawimage::convertCvRGBtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1113 {
1114  if (src.type() != CV_8UC3)
1115  LFATAL("src must have type CV_8UC3 and RGB pixels; your image has " << jevois::cvtypestr(src.type()));
1116  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1117 
1118  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.data, dst.cols));
1119 }
1120 
1121 // ####################################################################################################
1122 void jevois::rawimage::pasteRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
1123 {
1124  if (src.type() != CV_8UC3)
1125  LFATAL("src must have type CV_8UC3 and RGB pixels; your image has " << jevois::cvtypestr(src.type()));
1126  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1127  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
1128 
1129  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>() +
1130  (x + y * dst.width) * dst.bytesperpix(), dst.width));
1131 }
1132 
1133 // ####################################################################################################
1134 namespace
1135 {
1136  class grayToYUYV : public cv::ParallelLoopBody
1137  {
1138  public:
1139  grayToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1140  inImg(inputImage), outImg(outImage)
1141  {
1142  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
1143  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1144  }
1145 
1146  virtual void operator()(const cv::Range & range) const
1147  {
1148  for (int j = range.start; j < range.end; ++j)
1149  {
1150  int const inoff = j * inlinesize;
1151  int const outoff = j * outlinesize;
1152 
1153  for (int i = 0; i < inImg.cols; ++i)
1154  {
1155  int mc = inoff + i;
1156  unsigned char const G = inImg.data[mc + 0];
1157 
1158  mc = outoff + i * 2;
1159  outImg[mc + 0] = G;
1160  outImg[mc + 1] = 0x80;
1161  }
1162  }
1163  }
1164 
1165  private:
1166  cv::Mat const & inImg;
1167  unsigned char * outImg;
1168  int inlinesize, outlinesize;
1169  };
1170 
1171  // ####################################################################################################
1172  void convertCvGRAYtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1173  {
1174  if (src.type() != CV_8UC1)
1175  LFATAL("src must have type CV_8UC1 and GRAY pixels; your image has " << jevois::cvtypestr(src.type()));
1176  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1177  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1178 
1179  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1180  }
1181 } // anonymous namespace
1182 
1183 // ####################################################################################################
1184 void jevois::rawimage::convertCvGRAYtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1185 {
1186  if (src.type() != CV_8UC1)
1187  LFATAL("src must have type CV_8UC1 and GRAY pixels; your image has " << jevois::cvtypestr(src.type()));
1188  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1189 
1190  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.data, dst.cols));
1191 }
1192 
1193 // ####################################################################################################
1194 namespace
1195 {
1196  class rgbaToYUYV : public cv::ParallelLoopBody
1197  {
1198  public:
1199  rgbaToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1200  inImg(inputImage), outImg(outImage)
1201  {
1202  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
1203  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1204  }
1205 
1206  virtual void operator()(const cv::Range & range) const
1207  {
1208  for (int j = range.start; j < range.end; ++j)
1209  {
1210  int const inoff = j * inlinesize;
1211  int const outoff = j * outlinesize;
1212 
1213  for (int i = 0; i < inImg.cols; i += 2)
1214  {
1215  int mc = inoff + i * 4;
1216  unsigned char const R1 = inImg.data[mc + 0];
1217  unsigned char const G1 = inImg.data[mc + 1];
1218  unsigned char const B1 = inImg.data[mc + 2];
1219  unsigned char const R2 = inImg.data[mc + 4];
1220  unsigned char const G2 = inImg.data[mc + 5];
1221  unsigned char const B2 = inImg.data[mc + 6];
1222 
1223  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
1224  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1225  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1226  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1227 
1228  mc = outoff + i * 2;
1229  outImg[mc + 0] = Y1;
1230  outImg[mc + 1] = U1;
1231  outImg[mc + 2] = Y2;
1232  outImg[mc + 3] = V2;
1233  }
1234  }
1235  }
1236 
1237  private:
1238  cv::Mat const & inImg;
1239  unsigned char * outImg;
1240  int inlinesize, outlinesize;
1241  };
1242 
1243  // ####################################################################################################
1244  void convertCvRGBAtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1245  {
1246  if (src.type() != CV_8UC4)
1247  LFATAL("src must have type CV_8UC4 and RGBA pixels; your image has " << jevois::cvtypestr(src.type()));
1248  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1249  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1250 
1251  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1252  }
1253 } // anonymous namespace
1254 
1255 // ####################################################################################################
1256 void jevois::rawimage::convertCvRGBAtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1257 {
1258  if (src.type() != CV_8UC4)
1259  LFATAL("src must have type CV_8UC4 and RGBA pixels; your image has " << jevois::cvtypestr(src.type()));
1260  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1261 
1262  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.data, dst.cols));
1263 }
1264 
1265 // ####################################################################################################
1266 void jevois::rawimage::convertCvBGRtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1267 {
1268  if (src.type() != CV_8UC3)
1269  LFATAL("src must have type CV_8UC3 and BGR pixels; your image has " << jevois::cvtypestr(src.type()));
1270  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1271 
1272  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1273  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1274 
1275  switch (dst.fmt)
1276  {
1277  case V4L2_PIX_FMT_SRGGB8: convertCvBGRtoBayer(src, dst); break;
1278  case V4L2_PIX_FMT_YUYV: convertCvBGRtoYUYV(src, dst); break;
1279  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, cv::COLOR_BGR2GRAY); break;
1280  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, cv::COLOR_BGR2BGR565); break;
1281  case V4L2_PIX_FMT_MJPEG: jevois::compressBGRtoJpeg(src, dst, quality); break;
1282  case V4L2_PIX_FMT_BGR24: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1283  case V4L2_PIX_FMT_RGB24: cv::cvtColor(src, dstcv, cv::COLOR_RGB2GRAY); break;
1284  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1285  }
1286 }
1287 
1288 // ####################################################################################################
1289 void jevois::rawimage::convertCvRGBtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1290 {
1291  if (src.type() != CV_8UC3)
1292  LFATAL("src must have type CV_8UC3 and RGB pixels; your image has " << jevois::cvtypestr(src.type()));
1293  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1294 
1295  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1296  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1297 
1298  switch (dst.fmt)
1299  {
1300  case V4L2_PIX_FMT_SRGGB8: convertCvRGBtoBayer(src, dst); break;
1301  case V4L2_PIX_FMT_YUYV: convertCvRGBtoYUYV(src, dst); break;
1302  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, cv::COLOR_RGB2GRAY); break;
1303  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, cv::COLOR_RGB2BGR565); break;
1304  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBtoJpeg(src, dst, quality); break;
1305  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, cv::COLOR_RGB2BGR); break;
1306  case V4L2_PIX_FMT_RGB24: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1307  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1308  }
1309 }
1310 
1311 // ####################################################################################################
1313 {
1314  if (src.type() != CV_8UC4)
1315  LFATAL("src must have type CV_8UC4 and RGBA pixels; your image has " << jevois::cvtypestr(src.type()));
1316  if (dst.fmt != V4L2_PIX_FMT_GREY) LFATAL("dst must have pixel type V4L2_PIX_FMT_GREY");
1317  int const w = src.cols, h = src.rows;
1318  if (int(dst.width) < w || int(dst.height) < 4 * h) LFATAL("dst must be at least as wide and 4x as tall as src");
1319 
1320  unsigned char const * sptr = src.data; unsigned char * dptr = dst.pixelsw<unsigned char>();
1321  int const stride = int(dst.width) - w;
1322 
1323  // Do R, G, B in 3 threads then A in the current thread:
1324  std::vector<std::future<void> > fut;
1325  for (int i = 0; i < 3; ++i) fut.push_back(jevois::async([&](int offset) {
1326  unsigned char const * s = sptr + offset; unsigned char * d = dptr + offset * w * h;
1327  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; } }, i));
1328 
1329  unsigned char const * s = sptr + 3; unsigned char * d = dptr + 3 * w * h;
1330  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; }
1331 
1332  // Wait for all threads to complete (those should never throw):
1333  for (auto & f : fut) f.get();
1334 }
1335 
1336 // ####################################################################################################
1337 void jevois::rawimage::convertCvRGBAtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1338 {
1339  if (src.type() != CV_8UC4)
1340  LFATAL("src must have type CV_8UC4 and RGBA pixels; your image has " << jevois::cvtypestr(src.type()));
1341  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1342 
1343  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1344 
1345  switch (dst.fmt)
1346  {
1347  case V4L2_PIX_FMT_SRGGB8: convertCvRGBAtoBayer(src, dst); break;
1348  case V4L2_PIX_FMT_YUYV: convertCvRGBAtoYUYV(src, dst); break;
1349  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, cv::COLOR_RGBA2GRAY); break;
1350  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, cv::COLOR_BGRA2BGR565); break;
1351  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBAtoJpeg(src, dst, quality); break;
1352  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, cv::COLOR_RGBA2BGR); break;
1353  case V4L2_PIX_FMT_RGB24: cv::cvtColor(src, dstcv, cv::COLOR_RGBA2RGB); break;
1354  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1355  }
1356 }
1357 
1358 // ####################################################################################################
1359 void jevois::rawimage::convertCvGRAYtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1360 {
1361  if (src.type() != CV_8UC1)
1362  LFATAL("src must have type CV_8UC1 and GRAY pixels; your image has " << jevois::cvtypestr(src.type()));
1363  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1364 
1365  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1366 
1367  switch (dst.fmt)
1368  {
1369  case V4L2_PIX_FMT_SRGGB8: convertCvGRAYtoBayer(src, dst); break;
1370  case V4L2_PIX_FMT_YUYV: convertCvGRAYtoYUYV(src, dst); break;
1371  case V4L2_PIX_FMT_GREY: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1372  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, cv::COLOR_GRAY2BGR565); break;
1373  case V4L2_PIX_FMT_MJPEG: jevois::compressGRAYtoJpeg(src, dst, quality); break;
1374  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, cv::COLOR_GRAY2BGR); break;
1375  case V4L2_PIX_FMT_RGB24: cv::cvtColor(src, dstcv, cv::COLOR_GRAY2RGB); break;
1376  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1377  }
1378 }
1379 
1380 // ####################################################################################################
1381 namespace
1382 {
1383  class hflipYUYV : public cv::ParallelLoopBody
1384  {
1385  public:
1386  hflipYUYV(unsigned char * outImage, size_t outw) :
1387  outImg(outImage), linesize(outw * 2) // 2 bytes/pix for YUYV
1388  { }
1389 
1390  virtual void operator()(const cv::Range & range) const
1391  {
1392  for (int j = range.start; j < range.end; ++j)
1393  {
1394  int const off = j * linesize;
1395 
1396  for (int i = 0; i < linesize / 2; i += 4)
1397  {
1398  unsigned char * ptr1 = outImg + off + i;
1399  unsigned char * ptr2 = outImg + off + linesize - 4 - i;
1400  std::swap(ptr1[0], ptr2[2]);
1401  std::swap(ptr1[1], ptr2[1]);
1402  std::swap(ptr1[2], ptr2[0]);
1403  std::swap(ptr1[3], ptr2[3]);
1404  }
1405  }
1406  }
1407 
1408  private:
1409  unsigned char * outImg;
1410  int linesize;
1411  };
1412 
1413 } // anonymous namespace
1414 
1415 // ####################################################################################################
1417 {
1418  if (img.fmt != V4L2_PIX_FMT_YUYV) LFATAL("img format must be V4L2_PIX_FMT_YUYV");
1419  cv::parallel_for_(cv::Range(0, img.height), hflipYUYV(img.pixelsw<unsigned char>(), img.width));
1420 }
1421 
1422 // ####################################################################################################
1423 #ifdef __ARM_NEON__
1424 namespace
1425 {
1426  class bayerToYUYV : public cv::ParallelLoopBody
1427  {
1428  public:
1429  bayerToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1430  inImg(inputImage), outImg(outImage)
1431  {
1432  inlinesize = inputImage.cols; // 1 bytes/pix for bayer
1433  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1434  }
1435 
1436  virtual void operator()(const cv::Range & range) const
1437  {
1438  uint16x8_t const masklo = vdupq_n_u16(255);
1439  uint8x16x3_t pix;
1440  const uint8x8_t u8_zero = vdup_n_u8(0);
1441  const uint16x8_t u16_rounding = vdupq_n_u16(128);
1442  const int16x8_t s16_rounding = vdupq_n_s16(128);
1443  const int8x16_t s8_rounding = vdupq_n_s8(128);
1444 
1445  for (int j = range.start; j < range.end; ++j)
1446  {
1447  int const inoff = j * inlinesize;
1448  int const outoff = j * outlinesize;
1449 
1450  unsigned char const * bayer = inImg.data + inoff;
1451  unsigned char const * bayer_end = bayer + inlinesize;
1452  int const bayer_step = inlinesize;
1453  unsigned char * dst = outImg + outoff;
1454 
1455  /* This code from opencv demosaicing:
1456  B G B G | B G B G | B G B G | B G B G
1457  G R G R | G R G R | G R G R | G R G R
1458  B G B G | B G B G | B G B G | B G B G */
1459 
1460  for( ; bayer <= bayer_end - 18; bayer += 14, dst += 28 )
1461  {
1462  uint16x8_t r0 = vld1q_u16((const ushort*)bayer);
1463  uint16x8_t r1 = vld1q_u16((const ushort*)(bayer + bayer_step));
1464  uint16x8_t r2 = vld1q_u16((const ushort*)(bayer + bayer_step*2));
1465 
1466  uint16x8_t b1 = vaddq_u16(vandq_u16(r0, masklo), vandq_u16(r2, masklo));
1467  uint16x8_t nextb1 = vextq_u16(b1, b1, 1);
1468  uint16x8_t b0 = vaddq_u16(b1, nextb1);
1469  // b0 b1 b2 ...
1470  uint8x8x2_t bb = vzip_u8(vrshrn_n_u16(b0, 2), vrshrn_n_u16(nextb1, 1));
1471  pix.val[2] = vcombine_u8(bb.val[0], bb.val[1]);
1472 
1473  uint16x8_t g0 = vaddq_u16(vshrq_n_u16(r0, 8), vshrq_n_u16(r2, 8));
1474  uint16x8_t g1 = vandq_u16(r1, masklo);
1475  g0 = vaddq_u16(g0, vaddq_u16(g1, vextq_u16(g1, g1, 1)));
1476  g1 = vextq_u16(g1, g1, 1);
1477  // g0 g1 g2 ...
1478  uint8x8x2_t gg = vzip_u8(vrshrn_n_u16(g0, 2), vmovn_u16(g1));
1479  pix.val[1] = vcombine_u8(gg.val[0], gg.val[1]);
1480 
1481  r0 = vshrq_n_u16(r1, 8);
1482  r1 = vaddq_u16(r0, vextq_u16(r0, r0, 1));
1483  // r0 r1 r2 ...
1484  uint8x8x2_t rr = vzip_u8(vmovn_u16(r0), vrshrn_n_u16(r1, 1));
1485  pix.val[0] = vcombine_u8(rr.val[0], rr.val[1]);
1486 
1487 
1488  // Ok, we have rgb values in pix, now convert to YUV:
1489  // code from: https://github.com/yszheda/rgb2yuv-neon/blob/master/yuv444.cpp
1490  uint8x8_t high_r = vget_high_u8(pix.val[2]);
1491  uint8x8_t low_r = vget_low_u8(pix.val[2]);
1492  uint8x8_t high_g = vget_high_u8(pix.val[1]);
1493  uint8x8_t low_g = vget_low_u8(pix.val[1]);
1494  uint8x8_t high_b = vget_high_u8(pix.val[0]);
1495  uint8x8_t low_b = vget_low_u8(pix.val[0]);
1496  int16x8_t signed_high_r = vreinterpretq_s16_u16(vaddl_u8(high_r, u8_zero));
1497  int16x8_t signed_low_r = vreinterpretq_s16_u16(vaddl_u8(low_r, u8_zero));
1498  int16x8_t signed_high_g = vreinterpretq_s16_u16(vaddl_u8(high_g, u8_zero));
1499  int16x8_t signed_low_g = vreinterpretq_s16_u16(vaddl_u8(low_g, u8_zero));
1500  int16x8_t signed_high_b = vreinterpretq_s16_u16(vaddl_u8(high_b, u8_zero));
1501  int16x8_t signed_low_b = vreinterpretq_s16_u16(vaddl_u8(low_b, u8_zero));
1502 
1503  // NOTE:
1504  // declaration may not appear after executable statement in block
1505  uint16x8_t high_y;
1506  uint16x8_t low_y;
1507  uint8x8_t scalar = vdup_n_u8(76);
1508  int16x8_t high_u;
1509  int16x8_t low_u;
1510  int16x8_t signed_scalar = vdupq_n_s16(-43);
1511  int16x8_t high_v;
1512  int16x8_t low_v;
1513  uint8x16x3_t pixel_yuv;
1514  int8x16_t u;
1515  int8x16_t v;
1516 
1517  // 1. Multiply transform matrix (Y′: unsigned, U/V: signed)
1518  high_y = vmull_u8(high_r, scalar);
1519  low_y = vmull_u8(low_r, scalar);
1520 
1521  high_u = vmulq_s16(signed_high_r, signed_scalar);
1522  low_u = vmulq_s16(signed_low_r, signed_scalar);
1523 
1524  signed_scalar = vdupq_n_s16(127);
1525  high_v = vmulq_s16(signed_high_r, signed_scalar);
1526  low_v = vmulq_s16(signed_low_r, signed_scalar);
1527 
1528  scalar = vdup_n_u8(150);
1529  high_y = vmlal_u8(high_y, high_g, scalar);
1530  low_y = vmlal_u8(low_y, low_g, scalar);
1531 
1532  signed_scalar = vdupq_n_s16(-84);
1533  high_u = vmlaq_s16(high_u, signed_high_g, signed_scalar);
1534  low_u = vmlaq_s16(low_u, signed_low_g, signed_scalar);
1535 
1536  signed_scalar = vdupq_n_s16(-106);
1537  high_v = vmlaq_s16(high_v, signed_high_g, signed_scalar);
1538  low_v = vmlaq_s16(low_v, signed_low_g, signed_scalar);
1539 
1540  scalar = vdup_n_u8(29);
1541  high_y = vmlal_u8(high_y, high_b, scalar);
1542  low_y = vmlal_u8(low_y, low_b, scalar);
1543 
1544  signed_scalar = vdupq_n_s16(127);
1545  high_u = vmlaq_s16(high_u, signed_high_b, signed_scalar);
1546  low_u = vmlaq_s16(low_u, signed_low_b, signed_scalar);
1547 
1548  signed_scalar = vdupq_n_s16(-21);
1549  high_v = vmlaq_s16(high_v, signed_high_b, signed_scalar);
1550  low_v = vmlaq_s16(low_v, signed_low_b, signed_scalar);
1551  // 2. Scale down (">>8") to 8-bit values with rounding ("+128") (Y′: unsigned, U/V: signed)
1552  // 3. Add an offset to the values to eliminate any negative values (all results are 8-bit unsigned)
1553 
1554  high_y = vaddq_u16(high_y, u16_rounding);
1555  low_y = vaddq_u16(low_y, u16_rounding);
1556 
1557  high_u = vaddq_s16(high_u, s16_rounding);
1558  low_u = vaddq_s16(low_u, s16_rounding);
1559 
1560  high_v = vaddq_s16(high_v, s16_rounding);
1561  low_v = vaddq_s16(low_v, s16_rounding);
1562 
1563  pixel_yuv.val[0] = vcombine_u8(vqshrn_n_u16(low_y, 8), vqshrn_n_u16(high_y, 8));
1564 
1565  u = vcombine_s8(vqshrn_n_s16(low_u, 8), vqshrn_n_s16(high_u, 8));
1566 
1567  v = vcombine_s8(vqshrn_n_s16(low_v, 8), vqshrn_n_s16(high_v, 8));
1568 
1569  u = vaddq_s8(u, s8_rounding);
1570  pixel_yuv.val[1] = vreinterpretq_u8_s8(u);
1571 
1572  v = vaddq_s8(v, s8_rounding);
1573  pixel_yuv.val[2] = vreinterpretq_u8_s8(v);
1574 
1575  // Store
1576 
1577  //FIXME: instead of storing all 3x16 YUV values, we need to interleave into YUYV...
1578  vst3q_u8(dst, pixel_yuv);
1579  }
1580  }
1581  }
1582 
1583  private:
1584  cv::Mat const & inImg;
1585  unsigned char * outImg;
1586  int inlinesize, outlinesize;
1587  };
1588 } // anonymous namespace
1589 #endif
1590 
1591 
1592 // ####################################################################################################
1594 {
1595  if (src.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("src format must be V4L2_PIX_FMT_SRGGB8");
1596  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1597  if (dst.width != src.width || dst.height < src.height) LFATAL("src and dst dims must match");
1598 
1599  auto cvsrc = jevois::rawimage::cvImage(src);
1600 
1601 #ifdef FIXME__ARM_NEON__
1602  // FIXME: Neon code not working yet, needs more work...
1603  cv::parallel_for_(cv::Range(0, cvsrc.rows), bayerToYUYV(cvsrc, dst.pixelsw<unsigned char>(), dst.width));
1604 #else
1605  auto cvdst = jevois::rawimage::cvImage(dst);
1606  cv::Mat xx;
1607  cv::cvtColor(cvsrc, xx, cv::COLOR_BayerBG2BGR);
1608  cv::parallel_for_(cv::Range(0, xx.rows), bgrToYUYV(xx, cvdst.data, cvdst.cols));
1609 #endif
1610 }
1611 
1612 // ####################################################################################################
1614 {
1615  if (src.fmt != V4L2_PIX_FMT_GREY) LFATAL("src format must be V4L2_PIX_FMT_GREY");
1616  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1617  if (dst.width != src.width || dst.height < src.height) LFATAL("src and dst dims must match");
1618 
1619  auto cvsrc = jevois::rawimage::cvImage(src);
1620  convertCvGRAYtoYUYV(cvsrc, dst);
1621 }
1622 
1623 // ####################################################################################################
1624 cv::Mat jevois::rescaleCv(cv::Mat const & img, cv::Size const & newdims)
1625 {
1626  cv::Mat scaled;
1627 
1628  if (newdims.width == img.cols && newdims.height == img.rows)
1629  scaled = img;
1630  else if (newdims.width > img.cols || newdims.height > img.rows)
1631  cv::resize(img, scaled, newdims, 0, 0, cv::INTER_LINEAR);
1632  else
1633  cv::resize(img, scaled, newdims, 0, 0, cv::INTER_AREA);
1634 
1635  return scaled;
1636 }
jevois::font::font8x13bold
const unsigned char font8x13bold[95][104]
Definition: Font8x13bold.C:8
jevois::rawimage::convertBayerToYUYV
void convertBayerToYUYV(RawImage const &src, RawImage &dst)
Convert from Bayer to YUYV, only used internally by Camera class.
Definition: RawImageOps.C:1593
jevois::rawimage::Font15x28
@ Font15x28
Definition: RawImageOps.H:163
jevois::font::font15x28
const unsigned char font15x28[95][420]
Definition: Font15x28.C:8
jevois::rawimage::drawDisk
void drawDisk(RawImage &img, int x, int y, unsigned int rad, unsigned int col)
Draw a disk in a YUYV image.
Definition: RawImageOps.C:465
jevois::font::font9x15bold
const unsigned char font9x15bold[95][135]
Definition: Font9x15bold.C:8
jevois::RawImage::pixels
const T * pixels() const
Shortcut access to pixels, read-only.
jevois::rawimage::convertCvRGBAtoCvYUYV
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:1256
jevois::rawimage::Font20x38
@ Font20x38
Definition: RawImageOps.H:165
jevois::rawimage::Font9x15bold
@ Font9x15bold
Definition: RawImageOps.H:158
jevois::RawImage::bytesperpix
unsigned int bytesperpix() const
Helper function to get the number of bytes/pixel given the RawImage pixel format.
Definition: RawImage.C:34
Async.H
jevois::rawimage::convertCvRGBtoCvYUYV
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:1112
RawImageOps.H
jevois::font::font14x26
const unsigned char font14x26[95][364]
Definition: Font14x26.C:8
jevois::rescaleCv
cv::Mat rescaleCv(cv::Mat const &img, cv::Size const &newdims)
Rescale an OpenCV image, choosing the right kind of interpolation.
Definition: RawImageOps.C:1624
jevois::rawimage::convertCvRGBtoRawImage
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:1289
jevois::rawimage::Font14x26
@ Font14x26
Definition: RawImageOps.H:162
jevois::cvtypestr
std::string cvtypestr(unsigned int cvtype)
Convert cv::Mat::type() code to to a string (e.g., CV_8UC1, CV_32SC3, etc)
Definition: Utils.C:57
Jpeg.H
jevois::RawImage::bytesize
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
jevois::rawimage::convertToCvRGB
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:307
jevois::font::font5x7
const unsigned char font5x7[95][35]
Definition: Font5x7.C:7
VideoBuf.H
jevois::rawimage::Font
Font
Available fonts for writeText()
Definition: RawImageOps.H:152
jevois::RawImage
A raw image as coming from a V4L2 Camera and/or being sent out to a USB Gadget.
Definition: RawImage.H:110
jevois::compressBGRtoJpeg
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
jevois::rawimage::drawCircle
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:483
jevois::compressRGBtoJpeg
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
jevois::rawimage::convertToCvBGR
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:282
jevois::rawimage::Font6x10
@ Font6x10
Definition: RawImageOps.H:155
jevois::font::font6x10
const unsigned char font6x10[95][60]
Definition: Font6x10.C:8
jevois::rawimage::convertToCvGray
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:246
jevois::rawimage::roipaste
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:425
jevois::RawImage::width
unsigned int width
Image width in pixels.
Definition: RawImage.H:145
jevois::RawImage::pixelsw
T * pixelsw()
Shortcut access to pixels, read-write.
jevois::font::font12x22
const unsigned char font12x22[95][264]
Definition: Font12x22.C:8
jevois::rawimage::writeText
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:689
jevois::rawimage::convertCvGRAYtoRawImage
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:1359
jevois
Definition: Concepts.dox:1
F
float F
Definition: GUIhelper.C:2150
Log.H
jevois::rawimage::Font8x13bold
@ Font8x13bold
Definition: RawImageOps.H:157
jevois::rawimage::Font12x22
@ Font12x22
Definition: RawImageOps.H:161
jevois::font::font11x22
const unsigned char font11x22[95][242]
Definition: Font11x22.C:8
jevois::v4l2BytesPerPix
unsigned int v4l2BytesPerPix(unsigned int fcc)
Return the number of bytes per pixel for a given V4L2_PIX_FMT_...
Definition: Utils.C:137
jevois::compressRGBAtoJpeg
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
jevois::compressGRAYtoJpeg
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
jevois::rawimage::Font5x7
@ Font5x7
Definition: RawImageOps.H:154
jevois::fccstr
std::string fccstr(unsigned int fcc)
Convert a V4L2 four-cc code (V4L2_PIX_FMT_...) to a 4-char string.
Definition: Utils.C:44
jevois::rawimage::pasteRGBtoYUYV
void pasteRGBtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a RGB byte image into a YUYV image.
Definition: RawImageOps.C:1122
jevois::font::font16x29
const unsigned char font16x29[95][464]
Definition: Font16x29.C:8
jevois::rawimage::drawFilledRect
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:646
jevois::rawimage::hFlipYUYV
void hFlipYUYV(RawImage &img)
Flip a YUYV RawImage horizontally while preserving color information.
Definition: RawImageOps.C:1416
jevois::rawimage::convertCvRGBAtoRawImage
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:1337
LFATAL
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level.
Definition: Log.H:217
jevois::rawimage::clipLine
bool clipLine(int wxmin, int wymin, int wxmax, int wymax, int &x1, int &y1, int &x2, int &y2)
Clip a line to fit inside a viewport [wxmin...wxmax[ x [wymin...wymax[.
Definition: RawImageOps.C:533
jevois::rawimage::convertCvBGRtoRawImage
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:1266
jevois::rawimage::unpackCvRGBAtoGrayRawImage
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:1312
jevois::async
std::future< std::invoke_result_t< std::decay_t< Function >, std::decay_t< Args >... > > async(Function &&f, Args &&... args)
Async execution using a thread pool.
jevois::RawImage::height
unsigned int height
Image height in pixels.
Definition: RawImage.H:146
jevois::rawimage::Font16x29
@ Font16x29
Definition: RawImageOps.H:164
jevois::RawImage::coordsOk
bool coordsOk(int x, int y) const
Helper function to check that coords are within image bounds.
Definition: RawImage.C:85
jevois::rawimage::Font11x22
@ Font11x22
Definition: RawImageOps.H:160
jevois::rawimage::cvImage
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:31
jevois::font::font7x13
const unsigned char font7x13[95][91]
Definition: Font7x13.C:8
jevois::rawimage::pasteBGRtoYUYV
void pasteBGRtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a BGR byte image into a YUYV image.
Definition: RawImageOps.C:1036
jevois::font::font20x38
const unsigned char font20x38[95][760]
Definition: Font20x38.C:8
jevois::rawimage::paste
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
Paste an image within another of same pixel type.
Definition: RawImageOps.C:403
jevois::rawimage::drawRect
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:607
jevois::RawImage::fmt
unsigned int fmt
Pixel format as a V4L2_PIX_FMT_XXX.
Definition: RawImage.H:147
Utils.H
jevois::RawImage::buf
std::shared_ptr< VideoBuf > buf
The pixel data buffer.
Definition: RawImage.H:149
jevois::rawimage::convertToCvRGBA
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:332
h
int h
Definition: GUIhelper.C:2150
jevois::rawimage::convertCvBGRtoCvYUYV
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:1026
jevois::rawimage::Font7x13
@ Font7x13
Definition: RawImageOps.H:156
jevois::rawimage::drawLine
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:564
jevois::font::font10x20
const unsigned char font10x20[95][200]
Definition: Font10x20.C:8
jevois::rawimage::Font10x20
@ Font10x20
Definition: RawImageOps.H:159
G
E G[999]
Definition: GUIhelper.C:2150
jevois::rawimage::pasteGreyToYUYV
void pasteGreyToYUYV(cv::Mat const &src, RawImage &dest, int dx, int dy)
Paste a grey byte image into a YUYV image.
Definition: RawImageOps.C:448
jevois::rawimage::convertGreyToYUYV
void convertGreyToYUYV(RawImage const &src, RawImage &dst)
Convert from Grey (monochrome) to YUYV, only used internally by Camera class.
Definition: RawImageOps.C:1613
jevois::rawimage::byteSwap
void byteSwap(RawImage &img)
Swap pairs of bytes in a RawImage.
Definition: RawImageOps.C:368
jevois::rawimage::convertCvGRAYtoCvYUYV
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:1184