Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
grabV4l2MultiCpp11Thread.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Acquire images using 1394 device with cfox (MAC OSX) and display it
33  * using GTK or GTK.
34  *
35  * Authors:
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
47 #include <iostream>
48 
49 #include <visp3/core/vpConfig.h>
50 
51 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_V4L2) && \
52  (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
53 
54 #include <condition_variable>
55 #include <iostream>
56 #include <limits>
57 #include <mutex>
58 #include <queue>
59 #include <thread>
60 
61 #include <visp3/core/vpDisplay.h>
62 #include <visp3/core/vpImageFilter.h>
63 #include <visp3/core/vpIoTools.h>
64 #include <visp3/core/vpTime.h>
65 #include <visp3/gui/vpDisplayGTK.h>
66 #include <visp3/gui/vpDisplayX.h>
67 #include <visp3/io/vpParseArgv.h>
68 #include <visp3/io/vpVideoWriter.h>
69 #include <visp3/sensor/vpV4l2Grabber.h>
70 
71 #define GETOPTARGS "d:oh"
72 
73 namespace
74 {
75 
76 void usage(const char *name, const char *badparam)
77 {
78  fprintf(stdout, "\n\
79 SYNOPSIS:\n\
80  %s [-d <device count>] [-o] [-h]\n\
81 \n\
82 DESCRIPTION:\n\
83  Capture multiple camera streams and save the stream without slowing down the acquisition.\n\
84  \n\
85 OPTIONS: \n\
86  -d <device count> \n\
87  Open the specified number of camera streams.\n\
88  \n\
89  -o \n\
90  Save each stream in a dedicated folder.\n\
91  \n\
92  -h \n\
93  Print the help.\n\n", name);
94 
95  if (badparam)
96  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
97 }
98 
99 bool getOptions(int argc, char **argv, unsigned int &deviceCount, bool &saveVideo)
100 {
101  const char *optarg;
102  const char **argv1 = (const char **)argv;
103  int c;
104  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
105 
106  switch (c) {
107  case 'd':
108  deviceCount = (unsigned int)atoi(optarg);
109  break;
110  case 'o':
111  saveVideo = true;
112  break;
113  case 'h':
114  usage(argv[0], NULL);
115  return false;
116  break;
117 
118  default:
119  usage(argv[0], optarg);
120  return false;
121  break;
122  }
123  }
124 
125  if ((c == 1) || (c == -1)) {
126  // standalone param or error
127  usage(argv[0], NULL);
128  std::cerr << "ERROR: " << std::endl;
129  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
130  return false;
131  }
132 
133  return true;
134 }
135 
136 // Code adapted from the original author Dan MaĊĦek to be compatible with ViSP
137 // image
138 class FrameQueue
139 {
140 
141 public:
142  struct cancelled {
143  };
144 
145  FrameQueue()
146  : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits<size_t>::max()), m_mutex()
147  {
148  }
149 
150  void cancel()
151  {
152  std::lock_guard<std::mutex> lock(m_mutex);
153  m_cancelled = true;
154  m_cond.notify_all();
155  }
156 
157  // Push the image to save in the queue (FIFO)
158  void push(const vpImage<vpRGBa> &image)
159  {
160  std::lock_guard<std::mutex> lock(m_mutex);
161 
162  m_queueColor.push(image);
163 
164  // Pop extra images in the queue
165  while (m_queueColor.size() > m_maxQueueSize) {
166  m_queueColor.pop();
167  }
168 
169  m_cond.notify_one();
170  }
171 
172  // Pop the image to save from the queue (FIFO)
173  vpImage<vpRGBa> pop()
174  {
175  std::unique_lock<std::mutex> lock(m_mutex);
176 
177  while (m_queueColor.empty()) {
178  if (m_cancelled) {
179  throw cancelled();
180  }
181 
182  m_cond.wait(lock);
183 
184  if (m_cancelled) {
185  throw cancelled();
186  }
187  }
188 
189  vpImage<vpRGBa> image(m_queueColor.front());
190  m_queueColor.pop();
191 
192  return image;
193  }
194 
195  void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
196 
197 private:
198  bool m_cancelled;
199  std::condition_variable m_cond;
200  std::queue<vpImage<vpRGBa> > m_queueColor;
201  size_t m_maxQueueSize;
202  std::mutex m_mutex;
203 };
204 
205 class StorageWorker
206 {
207 
208 public:
209  StorageWorker(FrameQueue &queue, const std::string &filename, unsigned int width, unsigned int height)
210  : m_queue(queue), m_filename(filename), m_width(width), m_height(height)
211  {
212  }
213 
214  // Thread main loop
215  void run()
216  {
217  vpImage<vpRGBa> O_color(m_height, m_width);
218 
219  vpVideoWriter writer;
220  if (!m_filename.empty()) {
221  writer.setFileName(m_filename);
222  writer.open(O_color);
223  }
224 
225  try {
226  for (;;) {
227  vpImage<vpRGBa> image(m_queue.pop());
228 
229  if (!m_filename.empty()) {
230  writer.saveFrame(image);
231  }
232  }
233  } catch (FrameQueue::cancelled &) {
234  }
235  }
236 
237 private:
238  FrameQueue &m_queue;
239  std::string m_filename;
240  unsigned int m_width;
241  unsigned int m_height;
242 };
243 
244 class ShareImage
245 {
246 
247 private:
248  bool m_cancelled;
249  std::condition_variable m_cond;
250  std::mutex m_mutex;
251  unsigned char *m_pImgData;
252  unsigned int m_totalSize;
253 
254 public:
255  struct cancelled {
256  };
257 
258  ShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) {}
259 
260  virtual ~ShareImage()
261  {
262  if (m_pImgData != NULL) {
263  delete[] m_pImgData;
264  }
265  }
266 
267  void cancel()
268  {
269  std::lock_guard<std::mutex> lock(m_mutex);
270  m_cancelled = true;
271  m_cond.notify_all();
272  }
273 
274  // Get the image to display
275  void getImage(unsigned char *const imageData, const unsigned int totalSize)
276  {
277  std::unique_lock<std::mutex> lock(m_mutex);
278 
279  if (m_cancelled) {
280  throw cancelled();
281  }
282 
283  m_cond.wait(lock);
284 
285  if (m_cancelled) {
286  throw cancelled();
287  }
288 
289  // Copy to imageData
290  if (totalSize <= m_totalSize) {
291  memcpy(imageData, m_pImgData, totalSize * sizeof(unsigned char));
292  } else {
293  std::cerr << "totalSize <= m_totalSize !" << std::endl;
294  }
295  }
296 
297  bool isCancelled()
298  {
299  std::lock_guard<std::mutex> lock(m_mutex);
300  return m_cancelled;
301  }
302 
303  // Set the image to display
304  void setImage(const unsigned char *const imageData, const unsigned int totalSize)
305  {
306  std::lock_guard<std::mutex> lock(m_mutex);
307 
308  if (m_pImgData == NULL || m_totalSize != totalSize) {
309  m_totalSize = totalSize;
310 
311  if (m_pImgData != NULL) {
312  delete[] m_pImgData;
313  }
314 
315  m_pImgData = new unsigned char[m_totalSize];
316  }
317 
318  // Copy from imageData
319  memcpy(m_pImgData, imageData, m_totalSize * sizeof(unsigned char));
320 
321  m_cond.notify_one();
322  }
323 };
324 
325 void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image)
326 {
327  vpImage<vpRGBa> local_img;
328 
329  // Open the camera stream
330  pGrabber->open(local_img);
331 
332  while (true) {
333  if (share_image.isCancelled()) {
334  break;
335  }
336 
337  pGrabber->acquire(local_img);
338 
339  // Update share_image
340  share_image.setImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4);
341  }
342 }
343 
344 void display(unsigned int width, unsigned int height, int win_x, int win_y,
345  unsigned int deviceId, ShareImage &share_image, FrameQueue &queue, bool save)
346 {
347  vpImage<vpRGBa> local_img(height, width);
348 
349 #if defined VISP_HAVE_X11
350  vpDisplayX display;
351 #elif defined VISP_HAVE_GTK
352  vpDisplayGTK display;
353 #endif
354 
355  // Init Display
356  {
357  std::stringstream ss;
358  ss << "Camera stream " << deviceId;
359  display.init(local_img, win_x, win_y, ss.str());
360  }
361 
362  try {
364 
365  vpImage<unsigned char> I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width);
366  vpImage<unsigned char> I_red_gaussian(height, width), I_green_gaussian(height, width),
367  I_blue_gaussian(height, width);
368  vpImage<double> I_red_gaussian_double, I_green_gaussian_double, I_blue_gaussian_double;
369 
370  bool exit = false, gaussian_blur = false;
371  while (!exit) {
372  double t = vpTime::measureTimeMs();
373 
374  // Get image
375  share_image.getImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4);
376 
377  // Apply gaussian blur to simulate a computation on the image
378  if (gaussian_blur) {
379  // Split channels
380  vpImageConvert::split(local_img, &I_red, &I_green, &I_blue, &I_alpha);
381  vpImageConvert::convert(I_red, I_red_gaussian_double);
382  vpImageConvert::convert(I_green, I_green_gaussian_double);
383  vpImageConvert::convert(I_blue, I_blue_gaussian_double);
384 
385  vpImageFilter::gaussianBlur(I_red_gaussian_double, I_red_gaussian_double, 21);
386  vpImageFilter::gaussianBlur(I_green_gaussian_double, I_green_gaussian_double, 21);
387  vpImageFilter::gaussianBlur(I_blue_gaussian_double, I_blue_gaussian_double, 21);
388 
389  vpImageConvert::convert(I_red_gaussian_double, I_red_gaussian);
390  vpImageConvert::convert(I_green_gaussian_double, I_green_gaussian);
391  vpImageConvert::convert(I_blue_gaussian_double, I_blue_gaussian);
392 
393  vpImageConvert::merge(&I_red_gaussian, &I_green_gaussian, &I_blue_gaussian, NULL, local_img);
394  }
395 
396  t = vpTime::measureTimeMs() - t;
397  std::stringstream ss;
398  ss << "Time: " << t << " ms";
399 
400  vpDisplay::display(local_img);
401 
402  vpDisplay::displayText(local_img, 20, 20, ss.str(), vpColor::red);
403  vpDisplay::displayText(local_img, 40, 20, "Left click to quit, right click for Gaussian blur.", vpColor::red);
404 
405  vpDisplay::flush(local_img);
406 
407  if (save) {
408  queue.push(local_img);
409  }
410 
411  if (vpDisplay::getClick(local_img, button, false)) {
412  switch (button) {
414  gaussian_blur = !gaussian_blur;
415  break;
416 
417  default:
418  exit = true;
419  break;
420  }
421  }
422  }
423  } catch (ShareImage::cancelled &) {
424  std::cout << "Cancelled!" << std::endl;
425  }
426 
427  share_image.cancel();
428 }
429 
430 } // Namespace
431 
432 int main(int argc, char *argv[])
433 {
434  unsigned int deviceCount = 1;
435  unsigned int cameraScale = 1; // 640x480
436  bool saveVideo = false;
437 
438  // Read the command line options
439  if (!getOptions(argc, argv, deviceCount, saveVideo)) {
440  return (-1);
441  }
442 
443  std::vector<vpV4l2Grabber *> grabbers;
444 
445  const unsigned int offsetX = 100, offsetY = 100;
446  for (unsigned int devicedId = 0; devicedId < deviceCount; devicedId++) {
447  try {
448  vpV4l2Grabber *pGrabber = new vpV4l2Grabber;
449  std::stringstream ss;
450  ss << "/dev/video" << devicedId;
451  pGrabber->setDevice(ss.str());
452  pGrabber->setScale(cameraScale);
453 
454  grabbers.push_back(pGrabber);
455  } catch (const vpException &e) {
456  std::cerr << "Exception: " << e.what() << std::endl;
457  }
458  }
459 
460  std::cout << "Grabbers: " << grabbers.size() << std::endl;
461 
462  std::vector<ShareImage> share_images(grabbers.size());
463  std::vector<std::thread> capture_threads;
464  std::vector<std::thread> display_threads;
465 
466  // Synchronized queues for each camera stream
467  std::vector<FrameQueue> save_queues(grabbers.size());
468  std::vector<StorageWorker> storages;
469  std::vector<std::thread> storage_threads;
470 
471  std::string parent_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
472  for (size_t deviceId = 0; deviceId < grabbers.size(); deviceId++) {
473  // Start the capture thread for the current camera stream
474  capture_threads.emplace_back(capture, grabbers[deviceId], std::ref(share_images[deviceId]));
475  int win_x = deviceId * offsetX, win_y = deviceId * offsetY;
476 
477  // Start the display thread for the current camera stream
478  display_threads.emplace_back(display, grabbers[deviceId]->getWidth(), grabbers[deviceId]->getHeight(), win_x, win_y,
479  deviceId, std::ref(share_images[deviceId]), std::ref(save_queues[deviceId]),
480  saveVideo);
481 
482  if (saveVideo) {
483  std::stringstream ss;
484  ss << parent_directory << "/Camera_Stream" << deviceId;
485  std::cout << "Create directory: " << ss.str() << std::endl;
486  vpIoTools::makeDirectory(ss.str());
487  ss << "/%06d.png";
488  std::string filename = ss.str();
489 
490  storages.emplace_back(std::ref(save_queues[deviceId]), std::cref(filename), grabbers[deviceId]->getWidth(),
491  grabbers[deviceId]->getHeight());
492  }
493  }
494 
495  if (saveVideo) {
496  for (auto &s : storages) {
497  // Start the storage thread for the current camera stream
498  storage_threads.emplace_back(&StorageWorker::run, &s);
499  }
500  }
501 
502  // Join all the worker threads, waiting for them to finish
503  for (auto &ct : capture_threads) {
504  ct.join();
505  }
506 
507  for (auto &dt : display_threads) {
508  dt.join();
509  }
510 
511  // Clean first the grabbers to avoid camera problems when cancelling the
512  // storage threads in the terminal
513  for (auto &g : grabbers) {
514  delete g;
515  }
516 
517  if (saveVideo) {
518  std::cout << "\nWaiting for finishing thread to write images..." << std::endl;
519  }
520 
521  // We're done reading, cancel all the queues
522  for (auto &qu : save_queues) {
523  qu.cancel();
524  }
525 
526  // Join all the worker threads, waiting for them to finish
527  for (auto &st : storage_threads) {
528  st.join();
529  }
530 
531  return EXIT_SUCCESS;
532 }
533 #else
534 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
535 int main()
536 {
537  std::cout << "You do not have X11, or GTK functionalities to display images..." << std::endl;
538  std::cout << "Tip if you are on a unix-like system:" << std::endl;
539  std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
540  std::cout << "Tip if you are on a windows-like system:" << std::endl;
541  std::cout << "- Install GTK, configure again ViSP using cmake and build again this example" << std::endl;
542  return EXIT_SUCCESS;
543 }
544 #elif !defined(VISP_HAVE_V4L2)
545 int main()
546 {
547  std::cout << "You do not have Video 4 Linux 2 functionality enabled" << std::endl;
548  std::cout << "Tip if you are on a unix-like system:" << std::endl;
549  std::cout << "- Install libv4l2, configure again ViSP using cmake and build again this example" << std::endl;
550  return EXIT_SUCCESS;
551 }
552 #else
553 int main()
554 {
555  std::cout << "You do not build ViSP with c++11 or higher compiler flag" << std::endl;
556  std::cout << "Tip:" << std::endl;
557  std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
558  return EXIT_SUCCESS;
559 }
560 #endif
561 #endif
562 
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
void acquire(vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void open(vpImage< unsigned char > &I)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
void setDevice(const std::string &devname)
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void split(const vpImage< vpRGBa > &src, vpImage< unsigned char > *pR, vpImage< unsigned char > *pG, vpImage< unsigned char > *pB, vpImage< unsigned char > *pa=NULL)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Definition: vpTime.cpp:358
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:217
static void display(const vpImage< unsigned char > &I)
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void gaussianBlur(const vpImage< unsigned char > &I, vpImage< double > &GI, unsigned int size=7, double sigma=0., bool normalize=true)
Class that enables to write easily a video file or a sequence of images.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:134
void saveFrame(vpImage< vpRGBa > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void open(vpImage< vpRGBa > &I)
const char * what() const
static void merge(const vpImage< unsigned char > *R, const vpImage< unsigned char > *G, const vpImage< unsigned char > *B, const vpImage< unsigned char > *a, vpImage< vpRGBa > &RGBa)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
unsigned int getSize() const
Definition: vpImage.h:227
void setFileName(const std::string &filename)