JeVois  1.7
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 namespace
500 {
501  // Liang-Barsky algo from http://hinjang.com/articles/04.html#eight
502  inline bool isZero(double a)
503  { return (a < 0.0001 && a > -0.0001 ); }
504 
505  bool clipT(double num, double denom, double & tE, double & tL)
506  {
507  if (isZero(denom)) return (num <= 0.0);
508 
509  double t = num / denom;
510 
511  if (denom > 0.0) {
512  if (t > tL) return false;
513  if (t > tE) tE = t;
514  } else {
515  if (t < tE) return false;
516  if (t < tL) tL = t;
517  }
518  return true;
519  }
520 }
521 
522 // ####################################################################################################
523 // Liang-Barsky algo from http://hinjang.com/articles/04.html#eight
524 bool jevois::rawimage::clipLine(int wxmin, int wymin, int wxmax, int wymax, int & x1, int & y1, int & x2, int & y2)
525 {
526  // This algo does not handle lines completely outside the window? quick test here that should work for most lines (but
527  // not all, may need to fix later):
528  if (x1 < wxmin && x2 < wxmin) return false;
529  if (x1 >= wxmax && x2 >= wxmax) return false;
530  if (y1 < wymin && y2 < wymin) return false;
531  if (y1 >= wymax && y2 >= wymax) return false;
532 
533  int const toofar = 5000;
534  if (x1 < -toofar || x1 > toofar || y1 < -toofar || y1 > toofar) return false;
535  if (x2 < -toofar || x2 > toofar || y2 < -toofar || y2 > toofar) return false;
536 
537  --wxmax; --wymax; // exclude right and bottom edges of the window
538 
539  double dx = x2 - x1, dy = y2 - y1;
540  if (isZero(dx) && isZero(dy)) return true;
541 
542  double tE = 0.0, tL = 1.0;
543 
544  if (clipT(wxmin - x1, dx, tE, tL) && clipT(x1 - wxmax, -dx, tE, tL) &&
545  clipT(wymin - y1, dy, tE, tL) && clipT(y1 - wymax, -dy, tE, tL))
546  {
547  if (tL < 1) { x2 = x1 + tL * dx; y2 = y1 + tL * dy; }
548  if (tE > 0) { x1 += tE * dx; y1 += tE * dy; }
549  }
550 
551  return true;
552 }
553 
554 // ####################################################################################################
555 void jevois::rawimage::drawLine(jevois::RawImage & img, int x1, int y1, int x2, int y2, unsigned int thick,
556  unsigned int col)
557 {
558  // First clip the line so we don't waste time trying to sometimes draw very long lines that may result from singular
559  // 3D projections:
560  if (jevois::rawimage::clipLine(0, 0, img.width, img.height, x1, y1, x2, y2) == false) return; // line fully outside
561 
562  // From the iLab Neuromorphic Vision C++ Toolkit
563  // from Graphics Gems / Paul Heckbert
564  int const dx = x2 - x1; int const ax = std::abs(dx) << 1; int const sx = dx < 0 ? -1 : 1;
565  int const dy = y2 - y1; int const ay = std::abs(dy) << 1; int const sy = dy < 0 ? -1 : 1;
566  int const w = img.width; int const h = img.height;
567  int x = x1, y = y1;
568 
569  if (ax > ay)
570  {
571  int d = ay - (ax >> 1);
572  for (;;)
573  {
574  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
575 
576  if (x == x2) return;
577  if (d >= 0) { y += sy; d -= ax; }
578  x += sx; d += ay;
579  }
580  }
581  else
582  {
583  int d = ax - (ay >> 1);
584  for (;;)
585  {
586  if (x >= 0 && x < w && y >= 0 && y < h) jevois::rawimage::drawDisk(img, x, y, thick, col);
587  if (y == y2) return;
588  if (d >= 0) { x += sx; d -= ay; }
589  y += sy; d += ax;
590  }
591  }
592 }
593 
594 // ####################################################################################################
595 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
596  unsigned int thick, unsigned int col)
597 {
598  if (thick == 0)
599  jevois::rawimage::drawRect(img, x, y, w, h, col);
600  else
601  {
602  // Draw so that the lines are drawn on top of the bottom-right corner at (x+w-1, y+h-1):
603  if (w) --w;
604  if (h) --h;
605  jevois::rawimage::drawLine(img, x, y, x+w, y, thick, col);
606  jevois::rawimage::drawLine(img, x, y+h, x+w, y+h, thick, col);
607  jevois::rawimage::drawLine(img, x, y, x, y+h, thick, col);
608  jevois::rawimage::drawLine(img, x+w, y, x+w, y+h, thick, col);
609  }
610 }
611 // ####################################################################################################
612 void jevois::rawimage::drawRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
613  unsigned int col)
614 {
615  if (w == 0) w = 1;
616  if (h == 0) h = 1;
617  if (x > int(img.width)) x = img.width;
618  if (y > int(img.height)) y = img.height;
619  if (x + w > img.width) w = img.width - x;
620  if (y + h > img.height) h = img.height - y;
621 
622  unsigned int const imgw = img.width;
623  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
624 
625  // Two horizontal lines:
626  unsigned int const offy = (h-1) * imgw;
627  for (unsigned int xx = 0; xx < w; ++xx) { b[xx] = col; b[xx + offy] = col; }
628 
629  // Two vertical lines:
630  unsigned int const offx = w-1;
631  for (unsigned int yy = 0; yy < h * imgw; yy += imgw) { b[yy] = col; b[yy + offx] = col; }
632 }
633 // ####################################################################################################
634 void jevois::rawimage::drawFilledRect(jevois::RawImage & img, int x, int y, unsigned int w, unsigned int h,
635  unsigned int col)
636 {
637  if (w == 0) w = 1;
638  if (h == 0) h = 1;
639  if (x > int(img.width)) x = img.width;
640  if (y > int(img.height)) y = img.height;
641  if (x + w > img.width) w = img.width - x;
642  if (y + h > img.height) h = img.height - y;
643 
644  unsigned int const stride = img.width - w;
645  unsigned short * b = img.pixelsw<unsigned short>() + x + y * img.width;
646 
647  for (unsigned int yy = 0; yy < h; ++yy)
648  {
649  for (unsigned int xx = 0; xx < w; ++xx) *b++ = col;
650  b += stride;
651  }
652 }
653 
654 // ####################################################################################################
655 // Font pattern definitions:
656 namespace jevois
657 {
658  namespace font
659  {
660  extern const unsigned char font10x20[95][200];
661  extern const unsigned char font11x22[95][242];
662  extern const unsigned char font12x22[95][264];
663  extern const unsigned char font14x26[95][364];
664  extern const unsigned char font15x28[95][420];
665  extern const unsigned char font16x29[95][464];
666  extern const unsigned char font20x38[95][760];
667  extern const unsigned char font5x7[95][35];
668  extern const unsigned char font6x10[95][60];
669  extern const unsigned char font7x13[95][91];
670  extern const unsigned char font8x13bold[95][104];
671  extern const unsigned char font9x15bold[95][135];
672  } // namespace font
673 } // namespace jevois
674 
675 
676 // ####################################################################################################
677 void jevois::rawimage::writeText(jevois::RawImage & img, std::string const & txt, int x, int y, unsigned int col,
679 {
680  jevois::rawimage::writeText(img, txt.c_str(), x, y, col, font);
681 }
682 
683 // ####################################################################################################
684 void jevois::rawimage::writeText(jevois::RawImage & img, char const * txt, int x, int y, unsigned int col,
686 {
687  int len = int(strlen(txt));
688  unsigned int const imgw = img.width;
689 
690  int fontw, fonth; unsigned char const * fontptr;
691  switch (font)
692  {
693  case Font5x7: fontw = 5; fonth = 7; fontptr = &jevois::font::font5x7[0][0]; break;
694  case Font6x10: fontw = 6; fonth = 10; fontptr = &jevois::font::font6x10[0][0]; break;
695  case Font7x13: fontw = 7; fonth = 13; fontptr = &jevois::font::font7x13[0][0]; break;
696  case Font8x13bold: fontw = 8; fonth = 13; fontptr = &jevois::font::font8x13bold[0][0]; break;
697  case Font9x15bold: fontw = 9; fonth = 15; fontptr = &jevois::font::font9x15bold[0][0]; break;
698  case Font10x20: fontw = 10; fonth = 20; fontptr = &jevois::font::font10x20[0][0]; break;
699  case Font11x22: fontw = 11; fonth = 22; fontptr = &jevois::font::font11x22[0][0]; break;
700  case Font12x22: fontw = 12; fonth = 22; fontptr = &jevois::font::font12x22[0][0]; break;
701  case Font14x26: fontw = 14; fonth = 26; fontptr = &jevois::font::font14x26[0][0]; break;
702  case Font15x28: fontw = 15; fonth = 28; fontptr = &jevois::font::font15x28[0][0]; break;
703  case Font16x29: fontw = 16; fonth = 29; fontptr = &jevois::font::font16x29[0][0]; break;
704  case Font20x38: fontw = 20; fonth = 38; fontptr = &jevois::font::font20x38[0][0]; break;
705  default: LFATAL("Invalid font");
706  }
707 
708  // Clip the text so that it does not go outside the image:
709  if (y < 0 || y + fonth > int(img.height)) return;
710  while (x + len * fontw > int(imgw)) { --len; if (len <= 0) return; }
711 
712  // Be nice and handle various pixel formats:
713  switch (img.bytesperpix())
714  {
715  case 2:
716  {
717  unsigned short * b = img.pixelsw<unsigned short>() + x + y * imgw;
718 
719  for (int i = 0; i < len; ++i)
720  {
721  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
722  unsigned char const * ptr = fontptr + fontw * fonth * idx;
723  unsigned short * bb = b;
724  for (int yy = 0; yy < fonth; ++yy)
725  {
726  // Draw one line of this letter, note the transparent background:
727  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
728  bb += imgw - fontw;
729  }
730  b += fontw;
731  }
732  }
733  break;
734 
735  case 1:
736  {
737  unsigned char * b = img.pixelsw<unsigned char>() + x + y * imgw;
738 
739  for (int i = 0; i < len; ++i)
740  {
741  int idx = txt[i] - 32; if (idx >= 95) idx = 0;
742  unsigned char const * ptr = fontptr + fontw * fonth * idx;
743  unsigned char * bb = b;
744  for (int yy = 0; yy < fonth; ++yy)
745  {
746  // Draw one line of this letter, note the transparent background:
747  for (int xx = 0; xx < fontw; ++xx) if (*ptr++) ++bb; else *bb++ = col;
748  bb += imgw - fontw;
749  }
750  b += fontw;
751  }
752  }
753  break;
754 
755  default:
756  LFATAL("Sorry, only 1 and 2 bytes/pixel images are supported for now");
757  }
758 }
759 
760 // ####################################################################################################
761 namespace
762 {
763  class bgrToBayer : public cv::ParallelLoopBody
764  {
765  public:
766  bgrToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
767  inImg(inputImage), outImg(outImage)
768  {
769  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
770  outlinesize = outw * 1; // 1 byte/pix for Bayer
771  }
772 
773  virtual void operator()(const cv::Range & range) const
774  {
775  for (int j = range.start; j < range.end; ++j)
776  {
777  int const inoff = j * inlinesize;
778  int const outoff = j * outlinesize;
779 
780  for (int i = 0; i < inImg.cols; i += 2)
781  {
782  int const in = inoff + i * 3;
783  int const out = outoff + i;
784 
785  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 2]; outImg[out + 1] = inImg.data[in + 4]; }
786  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 3]; }
787  }
788  }
789  }
790 
791  private:
792  cv::Mat const & inImg;
793  unsigned char * outImg;
794  int inlinesize, outlinesize;
795  };
796 
797  // ####################################################################################################
798  void convertCvBGRtoBayer(cv::Mat const & src, jevois::RawImage & dst)
799  {
800  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
801  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
802  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
803 
804  cv::parallel_for_(cv::Range(0, src.rows), bgrToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
805  }
806 } // anonymous namespace
807 
808 // ####################################################################################################
809 namespace
810 {
811  class rgbToBayer : public cv::ParallelLoopBody
812  {
813  public:
814  rgbToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
815  inImg(inputImage), outImg(outImage)
816  {
817  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
818  outlinesize = outw * 1; // 1 byte/pix for Bayer
819  }
820 
821  virtual void operator()(const cv::Range & range) const
822  {
823  for (int j = range.start; j < range.end; ++j)
824  {
825  int const inoff = j * inlinesize;
826  int const outoff = j * outlinesize;
827 
828  for (int i = 0; i < inImg.cols; i += 2)
829  {
830  int const in = inoff + i * 3;
831  int const out = outoff + i;
832 
833  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 4]; }
834  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 5]; }
835  }
836  }
837  }
838 
839  private:
840  cv::Mat const & inImg;
841  unsigned char * outImg;
842  int inlinesize, outlinesize;
843  };
844 
845  // ####################################################################################################
846  void convertCvRGBtoBayer(cv::Mat const & src, jevois::RawImage & dst)
847  {
848  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
849  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
850  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
851 
852  cv::parallel_for_(cv::Range(0, src.rows), rgbToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
853  }
854 } // anonymous namespace
855 
856 // ####################################################################################################
857 namespace
858 {
859  class grayToBayer : public cv::ParallelLoopBody
860  {
861  public:
862  grayToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
863  inImg(inputImage), outImg(outImage)
864  {
865  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
866  outlinesize = outw * 1; // 1 byte/pix for Bayer
867  }
868 
869  virtual void operator()(const cv::Range & range) const
870  {
871  for (int j = range.start; j < range.end; ++j)
872  {
873  int const inoff = j * inlinesize;
874  int const outoff = j * outlinesize;
875 
876  memcpy(&outImg[outoff], &inImg.data[inoff], inlinesize);
877  }
878  }
879 
880  private:
881  cv::Mat const & inImg;
882  unsigned char * outImg;
883  int inlinesize, outlinesize;
884  };
885 
886  // ####################################################################################################
887  void convertCvGRAYtoBayer(cv::Mat const & src, jevois::RawImage & dst)
888  {
889  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
890  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
891  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
892 
893  cv::parallel_for_(cv::Range(0, src.rows), grayToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
894  }
895 } // anonymous namespace
896 
897 // ####################################################################################################
898 namespace
899 {
900  class rgbaToBayer : public cv::ParallelLoopBody
901  {
902  public:
903  rgbaToBayer(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
904  inImg(inputImage), outImg(outImage)
905  {
906  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
907  outlinesize = outw * 1; // 1 byte/pix for Bayer
908  }
909 
910  virtual void operator()(const cv::Range & range) const
911  {
912  for (int j = range.start; j < range.end; ++j)
913  {
914  int const inoff = j * inlinesize;
915  int const outoff = j * outlinesize;
916 
917  for (int i = 0; i < inImg.cols; i += 2)
918  {
919  int const in = inoff + i * 4;
920  int const out = outoff + i;
921 
922  if ( (j & 1) == 0) { outImg[out + 0] = inImg.data[in + 0]; outImg[out + 1] = inImg.data[in + 5]; }
923  else { outImg[out + 0] = inImg.data[in + 1]; outImg[out + 1] = inImg.data[in + 6]; }
924  }
925  }
926  }
927 
928  private:
929  cv::Mat const & inImg;
930  unsigned char * outImg;
931  int inlinesize, outlinesize;
932  };
933 
934  // ####################################################################################################
935  void convertCvRGBAtoBayer(cv::Mat const & src, jevois::RawImage & dst)
936  {
937  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
938  if (dst.fmt != V4L2_PIX_FMT_SRGGB8) LFATAL("dst format must be V4L2_PIX_FMT_SRGGB8");
939  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
940 
941  cv::parallel_for_(cv::Range(0, src.rows), rgbaToBayer(src, dst.pixelsw<unsigned char>(), dst.width));
942  }
943 } // anonymous namespace
944 
945 // ####################################################################################################
946 namespace
947 {
948  class bgrToYUYV : public cv::ParallelLoopBody
949  {
950  public:
951  bgrToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
952  inImg(inputImage), outImg(outImage)
953  {
954  inlinesize = inputImage.cols * 3; // 3 bytes/pix for BGR
955  outlinesize = outw * 2; // 2 bytes/pix for YUYV
956  }
957 
958  virtual void operator()(const cv::Range & range) const
959  {
960  for (int j = range.start; j < range.end; ++j)
961  {
962  int const inoff = j * inlinesize;
963  int const outoff = j * outlinesize;
964 
965  for (int i = 0; i < inImg.cols; i += 2)
966  {
967  int mc = inoff + i * 3;
968  unsigned char const B1 = inImg.data[mc + 0];
969  unsigned char const G1 = inImg.data[mc + 1];
970  unsigned char const R1 = inImg.data[mc + 2];
971  unsigned char const B2 = inImg.data[mc + 3];
972  unsigned char const G2 = inImg.data[mc + 4];
973  unsigned char const R2 = inImg.data[mc + 5];
974 
975  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
976  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
977  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
978  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
979  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
980  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
981 
982  mc = outoff + i * 2;
983  outImg[mc + 0] = Y1;
984  outImg[mc + 1] = U1;
985  outImg[mc + 2] = Y2;
986  outImg[mc + 3] = V2;
987  }
988  }
989  }
990 
991  private:
992  cv::Mat const & inImg;
993  unsigned char * outImg;
994  int inlinesize, outlinesize;
995  };
996 
997  // ####################################################################################################
998  void convertCvBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
999  {
1000  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
1001  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1002  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1003 
1004  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1005  }
1006 } // anonymous namespace
1007 
1008 // ####################################################################################################
1009 void jevois::rawimage::convertCvBGRtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1010 {
1011  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
1012  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1013 
1014  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.data, dst.cols));
1015 }
1016 
1017 // ####################################################################################################
1018 void jevois::rawimage::pasteBGRtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
1019 {
1020  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
1021  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1022  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
1023 
1024  cv::parallel_for_(cv::Range(0, src.rows), bgrToYUYV(src, dst.pixelsw<unsigned char>() +
1025  (x + y * dst.width) * dst.bytesperpix(), dst.width));
1026 }
1027 
1028 // ####################################################################################################
1029 namespace
1030 {
1031  class rgbToYUYV : public cv::ParallelLoopBody
1032  {
1033  public:
1034  rgbToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1035  inImg(inputImage), outImg(outImage)
1036  {
1037  inlinesize = inputImage.cols * 3; // 3 bytes/pix for RGB
1038  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1039  }
1040 
1041  virtual void operator()(const cv::Range & range) const
1042  {
1043  for (int j = range.start; j < range.end; ++j)
1044  {
1045  int const inoff = j * inlinesize;
1046  int const outoff = j * outlinesize;
1047 
1048  for (int i = 0; i < inImg.cols; i += 2)
1049  {
1050  int mc = inoff + i * 3;
1051  unsigned char const R1 = inImg.data[mc + 0];
1052  unsigned char const G1 = inImg.data[mc + 1];
1053  unsigned char const B1 = inImg.data[mc + 2];
1054  unsigned char const R2 = inImg.data[mc + 3];
1055  unsigned char const G2 = inImg.data[mc + 4];
1056  unsigned char const B2 = inImg.data[mc + 5];
1057 
1058  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
1059  //float const V1 = (0.439F * R1) - (0.368F * G1) - (0.071F * B1) + 128.0F;
1060  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1061  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1062  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1063  //float const U2 = -(0.148F * R2) - (0.291F * G2) + (0.439F * B2) + 128.0F;
1064 
1065  mc = outoff + i * 2;
1066  outImg[mc + 0] = Y1;
1067  outImg[mc + 1] = U1;
1068  outImg[mc + 2] = Y2;
1069  outImg[mc + 3] = V2;
1070  }
1071  }
1072  }
1073 
1074  private:
1075  cv::Mat const & inImg;
1076  unsigned char * outImg;
1077  int inlinesize, outlinesize;
1078  };
1079 
1080  // ####################################################################################################
1081  void convertCvRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1082  {
1083  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1084  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1085  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1086 
1087  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1088  }
1089 } // anonymous namespace
1090 
1091 // ####################################################################################################
1092 void jevois::rawimage::convertCvRGBtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1093 {
1094  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1095  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1096 
1097  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.data, dst.cols));
1098 }
1099 
1100 // ####################################################################################################
1101 void jevois::rawimage::pasteRGBtoYUYV(cv::Mat const & src, jevois::RawImage & dst, int x, int y)
1102 {
1103  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1104  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1105  if (x + src.cols > int(dst.width) || y + src.rows > int(dst.height)) LFATAL("src does not fit within dst");
1106 
1107  cv::parallel_for_(cv::Range(0, src.rows), rgbToYUYV(src, dst.pixelsw<unsigned char>() +
1108  (x + y * dst.width) * dst.bytesperpix(), dst.width));
1109 }
1110 
1111 // ####################################################################################################
1112 namespace
1113 {
1114  class grayToYUYV : public cv::ParallelLoopBody
1115  {
1116  public:
1117  grayToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1118  inImg(inputImage), outImg(outImage)
1119  {
1120  inlinesize = inputImage.cols * 1; // 1 bytes/pix for GRAY
1121  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1122  }
1123 
1124  virtual void operator()(const cv::Range & range) const
1125  {
1126  for (int j = range.start; j < range.end; ++j)
1127  {
1128  int const inoff = j * inlinesize;
1129  int const outoff = j * outlinesize;
1130 
1131  for (int i = 0; i < inImg.cols; ++i)
1132  {
1133  int mc = inoff + i;
1134  unsigned char const G = inImg.data[mc + 0];
1135 
1136  mc = outoff + i * 2;
1137  outImg[mc + 0] = G;
1138  outImg[mc + 1] = 0x80;
1139  }
1140  }
1141  }
1142 
1143  private:
1144  cv::Mat const & inImg;
1145  unsigned char * outImg;
1146  int inlinesize, outlinesize;
1147  };
1148 
1149  // ####################################################################################################
1150  void convertCvGRAYtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1151  {
1152  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
1153  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1154  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1155 
1156  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1157  }
1158 } // anonymous namespace
1159 
1160 // ####################################################################################################
1161 void jevois::rawimage::convertCvGRAYtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1162 {
1163  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and GRAY pixels");
1164  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1165 
1166  cv::parallel_for_(cv::Range(0, src.rows), grayToYUYV(src, dst.data, dst.cols));
1167 }
1168 
1169 // ####################################################################################################
1170 namespace
1171 {
1172  class rgbaToYUYV : public cv::ParallelLoopBody
1173  {
1174  public:
1175  rgbaToYUYV(cv::Mat const & inputImage, unsigned char * outImage, size_t outw) :
1176  inImg(inputImage), outImg(outImage)
1177  {
1178  inlinesize = inputImage.cols * 4; // 4 bytes/pix for RGBA
1179  outlinesize = outw * 2; // 2 bytes/pix for YUYV
1180  }
1181 
1182  virtual void operator()(const cv::Range & range) const
1183  {
1184  for (int j = range.start; j < range.end; ++j)
1185  {
1186  int const inoff = j * inlinesize;
1187  int const outoff = j * outlinesize;
1188 
1189  for (int i = 0; i < inImg.cols; i += 2)
1190  {
1191  int mc = inoff + i * 4;
1192  unsigned char const R1 = inImg.data[mc + 0];
1193  unsigned char const G1 = inImg.data[mc + 1];
1194  unsigned char const B1 = inImg.data[mc + 2];
1195  unsigned char const R2 = inImg.data[mc + 4];
1196  unsigned char const G2 = inImg.data[mc + 5];
1197  unsigned char const B2 = inImg.data[mc + 6];
1198 
1199  float const Y1 = (0.257F * R1) + (0.504F * G1) + (0.098F * B1) + 16.0F;
1200  float const U1 = -(0.148F * R1) - (0.291F * G1) + (0.439F * B1) + 128.0F;
1201  float const Y2 = (0.257F * R2) + (0.504F * G2) + (0.098F * B2) + 16.0F;
1202  float const V2 = (0.439F * R2) - (0.368F * G2) - (0.071F * B2) + 128.0F;
1203 
1204  mc = outoff + i * 2;
1205  outImg[mc + 0] = Y1;
1206  outImg[mc + 1] = U1;
1207  outImg[mc + 2] = Y2;
1208  outImg[mc + 3] = V2;
1209  }
1210  }
1211  }
1212 
1213  private:
1214  cv::Mat const & inImg;
1215  unsigned char * outImg;
1216  int inlinesize, outlinesize;
1217  };
1218 
1219  // ####################################################################################################
1220  void convertCvRGBAtoYUYV(cv::Mat const & src, jevois::RawImage & dst)
1221  {
1222  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1223  if (dst.fmt != V4L2_PIX_FMT_YUYV) LFATAL("dst format must be V4L2_PIX_FMT_YUYV");
1224  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1225 
1226  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.pixelsw<unsigned char>(), dst.width));
1227  }
1228 } // anonymous namespace
1229 
1230 // ####################################################################################################
1231 void jevois::rawimage::convertCvRGBAtoCvYUYV(cv::Mat const & src, cv::Mat & dst)
1232 {
1233  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1234  dst = cv::Mat(src.rows, src.cols, CV_8UC2);
1235 
1236  cv::parallel_for_(cv::Range(0, src.rows), rgbaToYUYV(src, dst.data, dst.cols));
1237 }
1238 
1239 // ####################################################################################################
1240 void jevois::rawimage::convertCvBGRtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1241 {
1242  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and BGR pixels");
1243  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1244 
1245  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1246  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1247 
1248  switch (dst.fmt)
1249  {
1250  case V4L2_PIX_FMT_SRGGB8: convertCvBGRtoBayer(src, dst); break;
1251  case V4L2_PIX_FMT_YUYV: convertCvBGRtoYUYV(src, dst); break;
1252  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_BGR2GRAY); break;
1253  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_BGR2BGR565); break;
1254  case V4L2_PIX_FMT_MJPEG: jevois::compressBGRtoJpeg(src, dst, quality); break;
1255  case V4L2_PIX_FMT_BGR24: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1256  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1257  }
1258 }
1259 
1260 // ####################################################################################################
1261 void jevois::rawimage::convertCvRGBtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1262 {
1263  if (src.type() != CV_8UC3) LFATAL("src must have type CV_8UC3 and RGB pixels");
1264  if (int(dst.width) != src.cols || int(dst.height) < src.rows) LFATAL("src and dst dims must match");
1265 
1266  // Note how the destination opencv image dstcv here is just a shell, the actual pixel data is in dst:
1267  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1268 
1269  switch (dst.fmt)
1270  {
1271  case V4L2_PIX_FMT_SRGGB8: convertCvRGBtoBayer(src, dst); break;
1272  case V4L2_PIX_FMT_YUYV: convertCvRGBtoYUYV(src, dst); break;
1273  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_RGB2GRAY); break;
1274  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_RGB2BGR565); break;
1275  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBtoJpeg(src, dst, quality); break;
1276  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_RGB2BGR); break;
1277  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1278  }
1279 }
1280 
1281 // ####################################################################################################
1283 {
1284  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1285  if (dst.fmt != V4L2_PIX_FMT_GREY) LFATAL("dst must have pixel type V4L2_PIX_FMT_GREY");
1286  int const w = src.cols, h = src.rows;
1287  if (int(dst.width) < w || int(dst.height) < 4 * h) LFATAL("dst must be at least as wide and 4x as tall as src");
1288 
1289  unsigned char const * sptr = src.data; unsigned char * dptr = dst.pixelsw<unsigned char>();
1290  int const stride = int(dst.width) - w;
1291 
1292  // Do R, G, B in 3 threads then A in the current thread:
1293  std::vector<std::future<void> > fut;
1294  for (int i = 0; i < 3; ++i) fut.push_back(std::async(std::launch::async, [&](int offset) {
1295  unsigned char const * s = sptr + offset; unsigned char * d = dptr + offset * w * h;
1296  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; } }, i));
1297 
1298  unsigned char const * s = sptr + 3; unsigned char * d = dptr + 3 * w * h;
1299  for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { *d++ = *s; s += 4; } d += stride; }
1300 
1301  // Wait for all threads to complete (those should never throw):
1302  for (auto & f : fut) f.get();
1303 }
1304 
1305 // ####################################################################################################
1306 void jevois::rawimage::convertCvRGBAtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1307 {
1308  if (src.type() != CV_8UC4) LFATAL("src must have type CV_8UC4 and RGBA pixels");
1309  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1310 
1311  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1312 
1313  switch (dst.fmt)
1314  {
1315  case V4L2_PIX_FMT_SRGGB8: convertCvRGBAtoBayer(src, dst); break;
1316  case V4L2_PIX_FMT_YUYV: convertCvRGBAtoYUYV(src, dst); break;
1317  case V4L2_PIX_FMT_GREY: cv::cvtColor(src, dstcv, CV_RGBA2GRAY); break;
1318  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_BGRA2BGR565); break;
1319  case V4L2_PIX_FMT_MJPEG: jevois::compressRGBAtoJpeg(src, dst, quality); break;
1320  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_RGBA2BGR); break;
1321  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1322  }
1323 }
1324 
1325 // ####################################################################################################
1326 void jevois::rawimage::convertCvGRAYtoRawImage(cv::Mat const & src, RawImage & dst, int quality)
1327 {
1328  if (src.type() != CV_8UC1) LFATAL("src must have type CV_8UC1 and Gray pixels");
1329  if (int(dst.width) != src.cols || int(dst.height) != src.rows) LFATAL("src and dst dims must match");
1330 
1331  cv::Mat dstcv = jevois::rawimage::cvImage(dst);
1332 
1333  switch (dst.fmt)
1334  {
1335  case V4L2_PIX_FMT_SRGGB8: convertCvGRAYtoBayer(src, dst); break;
1336  case V4L2_PIX_FMT_YUYV: convertCvGRAYtoYUYV(src, dst); break;
1337  case V4L2_PIX_FMT_GREY: memcpy(dst.pixelsw<void>(), src.data, dst.width * dst.height * dst.bytesperpix()); break;
1338  case V4L2_PIX_FMT_RGB565: cv::cvtColor(src, dstcv, CV_GRAY2BGR565); break;
1339  case V4L2_PIX_FMT_MJPEG: jevois::compressGRAYtoJpeg(src, dst, quality); break;
1340  case V4L2_PIX_FMT_BGR24: cv::cvtColor(src, dstcv, CV_GRAY2BGR); break;
1341  default: LFATAL("Unsupported output pixel format " << jevois::fccstr(dst.fmt) << std::hex <<' '<< dst.fmt);
1342  }
1343 }
1344 
1345 // ####################################################################################################
1346 namespace
1347 {
1348  class hflipYUYV : public cv::ParallelLoopBody
1349  {
1350  public:
1351  hflipYUYV(unsigned char * outImage, size_t outw) :
1352  outImg(outImage), linesize(outw * 2) // 2 bytes/pix for YUYV
1353  { }
1354 
1355  virtual void operator()(const cv::Range & range) const
1356  {
1357  for (int j = range.start; j < range.end; ++j)
1358  {
1359  int const off = j * linesize;
1360 
1361  for (int i = 0; i < linesize / 2; i += 4)
1362  {
1363  unsigned char * ptr1 = outImg + off + i;
1364  unsigned char * ptr2 = outImg + off + linesize - 4 - i;
1365  std::swap(ptr1[0], ptr2[2]);
1366  std::swap(ptr1[1], ptr2[1]);
1367  std::swap(ptr1[2], ptr2[0]);
1368  std::swap(ptr1[3], ptr2[3]);
1369  }
1370  }
1371  }
1372 
1373  private:
1374  unsigned char * outImg;
1375  int linesize;
1376  };
1377 
1378 } // anonymous namespace
1379 
1380 // ####################################################################################################
1382 {
1383  if (img.fmt != V4L2_PIX_FMT_YUYV) LFATAL("img format must be V4L2_PIX_FMT_YUYV");
1384  cv::parallel_for_(cv::Range(0, img.height), hflipYUYV(img.pixelsw<unsigned char>(), img.width));
1385 }
1386 
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:677
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:1231
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:555
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:152
void hFlipYUYV(RawImage &img)
Flip a YUYV RawImage horizontally while preserving color information.
Definition: RawImageOps.C:1381
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:1009
void pasteBGRtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a BGR byte image into a YUYV image.
Definition: RawImageOps.C:1018
std::string fccstr(unsigned int fcc)
Convert a V4L2 four-cc code (V4L2_PIX_FMT_...) to a 4-char string.
Definition: Utils.C:37
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:524
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:1240
#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:1092
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:634
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:1306
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:1282
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:1326
void pasteRGBtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
Paste a RGB byte image into a YUYV image.
Definition: RawImageOps.C:1101
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:1161
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:595
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:1261
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