Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
grabV4l2MultiCpp11Thread.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
36
44#include <iostream>
45
46#include <visp3/core/vpConfig.h>
47
48#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_V4L2) && \
49 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
50
51#include <condition_variable>
52#include <iostream>
53#include <limits>
54#include <mutex>
55#include <queue>
56#include <thread>
57
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpImageFilter.h>
60#include <visp3/core/vpIoTools.h>
61#include <visp3/core/vpTime.h>
62#include <visp3/gui/vpDisplayGTK.h>
63#include <visp3/gui/vpDisplayX.h>
64#include <visp3/io/vpParseArgv.h>
65#include <visp3/io/vpVideoWriter.h>
66#include <visp3/sensor/vpV4l2Grabber.h>
67
68#define GETOPTARGS "d:oh"
69
70namespace
71{
72
73void usage(const char *name, const char *badparam)
74{
75 fprintf(stdout, "\n\
76SYNOPSIS:\n\
77 %s [-d <device count>] [-o] [-h]\n\
78\n\
79DESCRIPTION:\n\
80 Capture multiple camera streams and save the stream without slowing down the acquisition.\n\
81 \n\
82OPTIONS: \n\
83 -d <device count> \n\
84 Open the specified number of camera streams.\n\
85 \n\
86 -o \n\
87 Save each stream in a dedicated folder.\n\
88 \n\
89 -h \n\
90 Print the help.\n\n",
91 name);
92
93 if (badparam)
94 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
95}
96
97bool getOptions(int argc, char **argv, unsigned int &deviceCount, bool &saveVideo)
98{
99 const char *optarg;
100 const char **argv1 = (const char **)argv;
101 int c;
102 while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
103
104 switch (c) {
105 case 'd':
106 deviceCount = (unsigned int)atoi(optarg);
107 break;
108 case 'o':
109 saveVideo = true;
110 break;
111 case 'h':
112 usage(argv[0], NULL);
113 return false;
114 break;
115
116 default:
117 usage(argv[0], optarg);
118 return false;
119 break;
120 }
121 }
122
123 if ((c == 1) || (c == -1)) {
124 // standalone param or error
125 usage(argv[0], NULL);
126 std::cerr << "ERROR: " << std::endl;
127 std::cerr << " Bad argument " << optarg << std::endl << std::endl;
128 return false;
129 }
130
131 return true;
132}
133
134// Code adapted from the original author Dan MaĊĦek to be compatible with ViSP
135// image
136class FrameQueue
137{
138
139public:
140 struct cancelled
141 { };
142
143 FrameQueue()
144 : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits<size_t>::max()), m_mutex()
145 { }
146
147 void cancel()
148 {
149 std::lock_guard<std::mutex> lock(m_mutex);
150 m_cancelled = true;
151 m_cond.notify_all();
152 }
153
154 // Push the image to save in the queue (FIFO)
155 void push(const vpImage<vpRGBa> &image)
156 {
157 std::lock_guard<std::mutex> lock(m_mutex);
158
159 m_queueColor.push(image);
160
161 // Pop extra images in the queue
162 while (m_queueColor.size() > m_maxQueueSize) {
163 m_queueColor.pop();
164 }
165
166 m_cond.notify_one();
167 }
168
169 // Pop the image to save from the queue (FIFO)
170 vpImage<vpRGBa> pop()
171 {
172 std::unique_lock<std::mutex> lock(m_mutex);
173
174 while (m_queueColor.empty()) {
175 if (m_cancelled) {
176 throw cancelled();
177 }
178
179 m_cond.wait(lock);
180
181 if (m_cancelled) {
182 throw cancelled();
183 }
184 }
185
186 vpImage<vpRGBa> image(m_queueColor.front());
187 m_queueColor.pop();
188
189 return image;
190 }
191
192 void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
193
194private:
195 bool m_cancelled;
196 std::condition_variable m_cond;
197 std::queue<vpImage<vpRGBa> > m_queueColor;
198 size_t m_maxQueueSize;
199 std::mutex m_mutex;
200};
201
202class StorageWorker
203{
204
205public:
206 StorageWorker(FrameQueue &queue, const std::string &filename, unsigned int width, unsigned int height)
207 : m_queue(queue), m_filename(filename), m_width(width), m_height(height)
208 { }
209
210 // Thread main loop
211 void run()
212 {
213 vpImage<vpRGBa> O_color(m_height, m_width);
214
215 vpVideoWriter writer;
216 if (!m_filename.empty()) {
217 writer.setFileName(m_filename);
218 writer.open(O_color);
219 }
220
221 try {
222 for (;;) {
223 vpImage<vpRGBa> image(m_queue.pop());
224
225 if (!m_filename.empty()) {
226 writer.saveFrame(image);
227 }
228 }
229 }
230 catch (FrameQueue::cancelled &) {
231 }
232 }
233
234private:
235 FrameQueue &m_queue;
236 std::string m_filename;
237 unsigned int m_width;
238 unsigned int m_height;
239};
240
241class ShareImage
242{
243
244private:
245 bool m_cancelled;
246 std::condition_variable m_cond;
247 std::mutex m_mutex;
248 unsigned char *m_pImgData;
249 unsigned int m_totalSize;
250
251public:
252 struct cancelled
253 { };
254
255 ShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) { }
256
257 virtual ~ShareImage()
258 {
259 if (m_pImgData != NULL) {
260 delete [] m_pImgData;
261 }
262 }
263
264 void cancel()
265 {
266 std::lock_guard<std::mutex> lock(m_mutex);
267 m_cancelled = true;
268 m_cond.notify_all();
269 }
270
271 // Get the image to display
272 void getImage(unsigned char *const imageData, const unsigned int totalSize)
273 {
274 std::unique_lock<std::mutex> lock(m_mutex);
275
276 if (m_cancelled) {
277 throw cancelled();
278 }
279
280 m_cond.wait(lock);
281
282 if (m_cancelled) {
283 throw cancelled();
284 }
285
286 // Copy to imageData
287 if (totalSize <= m_totalSize) {
288 memcpy(imageData, m_pImgData, totalSize * sizeof(unsigned char));
289 }
290 else {
291 std::cerr << "totalSize <= m_totalSize !" << std::endl;
292 }
293 }
294
295 bool isCancelled()
296 {
297 std::lock_guard<std::mutex> lock(m_mutex);
298 return m_cancelled;
299 }
300
301 // Set the image to display
302 void setImage(const unsigned char *const imageData, const unsigned int totalSize)
303 {
304 std::lock_guard<std::mutex> lock(m_mutex);
305
306 if (m_pImgData == NULL || m_totalSize != totalSize) {
307 m_totalSize = totalSize;
308
309 if (m_pImgData != NULL) {
310 delete [] m_pImgData;
311 }
312
313 m_pImgData = new unsigned char[m_totalSize];
314 }
315
316 // Copy from imageData
317 memcpy(m_pImgData, imageData, m_totalSize * sizeof(unsigned char));
318
319 m_cond.notify_one();
320 }
321};
322
323void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image)
324{
325 vpImage<vpRGBa> local_img;
326
327 // Open the camera stream
328 pGrabber->open(local_img);
329
330 while (true) {
331 if (share_image.isCancelled()) {
332 break;
333 }
334
335 pGrabber->acquire(local_img);
336
337 // Update share_image
338 share_image.setImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4);
339 }
340}
341
342void display(unsigned int width, unsigned int height, int win_x, int win_y, unsigned int deviceId,
343 ShareImage &share_image, FrameQueue &queue, bool save)
344{
345 vpImage<vpRGBa> local_img(height, width);
346
347#if defined(VISP_HAVE_X11)
348 vpDisplayX display;
349#elif defined(VISP_HAVE_GTK)
350 vpDisplayGTK display;
351#endif
352
353 // Init Display
354 {
355 std::stringstream ss;
356 ss << "Camera stream " << deviceId;
357 display.init(local_img, win_x, win_y, ss.str());
358 }
359
360 try {
362
363 vpImage<unsigned char> I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width);
364 vpImage<unsigned char> I_red_gaussian(height, width), I_green_gaussian(height, width),
365 I_blue_gaussian(height, width);
366 vpImage<double> I_red_gaussian_double, I_green_gaussian_double, I_blue_gaussian_double;
367
368 bool exit = false, gaussian_blur = false;
369 while (!exit) {
370 double t = vpTime::measureTimeMs();
371
372 // Get image
373 share_image.getImage((unsigned char *)local_img.bitmap, local_img.getSize() * 4);
374
375 // Apply gaussian blur to simulate a computation on the image
376 if (gaussian_blur) {
377 // Split channels
378 vpImageConvert::split(local_img, &I_red, &I_green, &I_blue, &I_alpha);
379 vpImageConvert::convert(I_red, I_red_gaussian_double);
380 vpImageConvert::convert(I_green, I_green_gaussian_double);
381 vpImageConvert::convert(I_blue, I_blue_gaussian_double);
382
383 vpImageFilter::gaussianBlur(I_red_gaussian_double, I_red_gaussian_double, 21);
384 vpImageFilter::gaussianBlur(I_green_gaussian_double, I_green_gaussian_double, 21);
385 vpImageFilter::gaussianBlur(I_blue_gaussian_double, I_blue_gaussian_double, 21);
386
387 vpImageConvert::convert(I_red_gaussian_double, I_red_gaussian);
388 vpImageConvert::convert(I_green_gaussian_double, I_green_gaussian);
389 vpImageConvert::convert(I_blue_gaussian_double, I_blue_gaussian);
390
391 vpImageConvert::merge(&I_red_gaussian, &I_green_gaussian, &I_blue_gaussian, NULL, local_img);
392 }
393
394 t = vpTime::measureTimeMs() - t;
395 std::stringstream ss;
396 ss << "Time: " << t << " ms";
397
398 vpDisplay::display(local_img);
399
400 vpDisplay::displayText(local_img, 20, 20, ss.str(), vpColor::red);
401 vpDisplay::displayText(local_img, 40, 20, "Left click to quit, right click for Gaussian blur.", vpColor::red);
402
403 vpDisplay::flush(local_img);
404
405 if (save) {
406 queue.push(local_img);
407 }
408
409 if (vpDisplay::getClick(local_img, button, false)) {
410 switch (button) {
412 gaussian_blur = !gaussian_blur;
413 break;
414
415 default:
416 exit = true;
417 break;
418 }
419 }
420 }
421 }
422 catch (ShareImage::cancelled &) {
423 std::cout << "Cancelled!" << std::endl;
424 }
425
426 share_image.cancel();
427}
428
429} // Namespace
430
431int main(int argc, char *argv [])
432{
433 unsigned int deviceCount = 1;
434 unsigned int cameraScale = 1; // 640x480
435 bool saveVideo = false;
436
437 // Read the command line options
438 if (!getOptions(argc, argv, deviceCount, saveVideo)) {
439 return EXIT_FAILURE;
440 }
441
442 std::vector<vpV4l2Grabber *> grabbers;
443
444 const unsigned int offsetX = 100, offsetY = 100;
445 for (unsigned int devicedId = 0; devicedId < deviceCount; devicedId++) {
446 try {
447 vpV4l2Grabber *pGrabber = new vpV4l2Grabber;
448 std::stringstream ss;
449 ss << "/dev/video" << devicedId;
450 pGrabber->setDevice(ss.str());
451 pGrabber->setScale(cameraScale);
452
453 grabbers.push_back(pGrabber);
454 }
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
static const vpColor red
Definition vpColor.h:211
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
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 emitted by ViSP classes.
Definition vpException.h:59
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< FilterType > &GI, unsigned int size=7, FilterType sigma=0., bool normalize=true)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getSize() const
Definition vpImage.h:223
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
static void makeDirectory(const std::string &dirname)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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 double measureTimeMs()
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")