Visual Servoing Platform version 3.5.0
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
73namespace
74{
75
76void usage(const char *name, const char *badparam)
77{
78 fprintf(stdout, "\n\
79SYNOPSIS:\n\
80 %s [-d <device count>] [-o] [-h]\n\
81\n\
82DESCRIPTION:\n\
83 Capture multiple camera streams and save the stream without slowing down the acquisition.\n\
84 \n\
85OPTIONS: \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
99bool 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
138class FrameQueue
139{
140
141public:
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
197private:
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
205class StorageWorker
206{
207
208public:
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
237private:
238 FrameQueue &m_queue;
239 std::string m_filename;
240 unsigned int m_width;
241 unsigned int m_height;
242};
243
244class ShareImage
245{
246
247private:
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
254public:
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
325void 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
344void 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
432int 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))
535int 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)
545int 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
553int 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 const vpColor red
Definition: vpColor.h:217
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * what() const
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 merge(const vpImage< unsigned char > *R, const vpImage< unsigned char > *G, const vpImage< unsigned char > *B, const vpImage< unsigned char > *a, vpImage< vpRGBa > &RGBa)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void gaussianBlur(const vpImage< unsigned char > &I, vpImage< double > &GI, unsigned int size=7, double sigma=0., bool normalize=true)
unsigned int getSize() const
Definition: vpImage.h:227
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)
Class that enables to write easily a video file or a sequence of images.
void saveFrame(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void open(vpImage< vpRGBa > &I)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()