Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-grabber-realsense.cpp
1
2#include <visp3/core/vpImage.h>
3#include <visp3/gui/vpDisplayGDI.h>
4#include <visp3/gui/vpDisplayOpenCV.h>
5#include <visp3/gui/vpDisplayX.h>
6#include <visp3/io/vpImageStorageWorker.h>
7#include <visp3/sensor/vpRealSense.h>
8#include <visp3/sensor/vpRealSense2.h>
9
10void usage(const char *argv[], int error)
11{
12 std::cout << "SYNOPSIS" << std::endl
13 << " " << argv[0] << " [--fps <6|15|30|60>]"
14 << " [--width <image width>]"
15 << " [--height <image height>]"
16 << " [--seqname <sequence name>]"
17 << " [--record <mode>]"
18 << " [--no-display]"
19 << " [--help] [-h]" << std::endl
20 << std::endl;
21 std::cout << "DESCRIPTION" << std::endl
22 << " --fps <6|15|30|60>" << std::endl
23 << " Frames per second." << std::endl
24 << " Default: 30." << std::endl
25 << std::endl
26 << " --width <image width>" << std::endl
27 << " Default: 640." << std::endl
28 << std::endl
29 << " --height <image height>" << std::endl
30 << " Default: 480." << std::endl
31 << std::endl
32 << " --seqname <sequence name>" << std::endl
33 << " Name of the sequence of image to create (ie: /tmp/image%04d.jpg)." << std::endl
34 << " Default: empty." << std::endl
35 << std::endl
36 << " --record <mode>" << std::endl
37 << " Allowed values for mode are:" << std::endl
38 << " 0: record all the captures images (continuous mode)," << std::endl
39 << " 1: record only images selected by a user click (single shot mode)." << std::endl
40 << " Default mode: 0" << std::endl
41 << std::endl
42 << " --no-display" << std::endl
43 << " Disable displaying captured images." << std::endl
44 << " When used and sequence name specified, record mode is internally set to 1 (continuous mode)."
45 << std::endl
46 << std::endl
47 << " --help, -h" << std::endl
48 << " Print this helper message." << std::endl
49 << std::endl;
50 std::cout << "USAGE" << std::endl
51 << " Example to visualize images:" << std::endl
52 << " " << argv[0] << std::endl
53 << std::endl
54 << " Examples to record a sequence of successive images in 640x480 resolution:" << std::endl
55 << " " << argv[0] << " --seqname I%04d.png" << std::endl
56 << " " << argv[0] << " --seqname folder/I%04d.png --record 0" << std::endl
57 << std::endl
58 << " Examples to record single shot 640x480 images:\n"
59 << " " << argv[0] << " --seqname I%04d.png --record 1\n"
60 << " " << argv[0] << " --seqname folder/I%04d.png --record 1" << std::endl
61 << std::endl
62 << " Examples to record single shot 1280x720 images:\n"
63 << " " << argv[0] << " --seqname I%04d.png --record 1 --width 1280 --height 720" << std::endl
64 << std::endl;
65
66 if (error) {
67 std::cout << "Error" << std::endl
68 << " "
69 << "Unsupported parameter " << argv[error] << std::endl;
70 }
71}
72
76int main(int argc, const char *argv[])
77{
78#if defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
79 try {
80 std::string opt_seqname;
81 int opt_record_mode = 0;
82 int opt_fps = 30;
83 bool opt_display = true;
84 unsigned int opt_width = 640;
85 unsigned int opt_height = 480;
86
87 for (int i = 1; i < argc; i++) {
88 if (std::string(argv[i]) == "--fps") {
89 opt_fps = std::atoi(argv[i + 1]);
90 i++;
91 } else if (std::string(argv[i]) == "--seqname") {
92 opt_seqname = std::string(argv[i + 1]);
93 i++;
94 } else if (std::string(argv[i]) == "--width") {
95 opt_width = std::atoi(argv[i + 1]);
96 i++;
97 } else if (std::string(argv[i]) == "--height") {
98 opt_height = std::atoi(argv[i + 1]);
99 i++;
100 } else if (std::string(argv[i]) == "--record") {
101 opt_record_mode = std::atoi(argv[i + 1]);
102 i++;
103 } else if (std::string(argv[i]) == "--no-display") {
104 opt_display = false;
105 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
106 usage(argv, 0);
107 return EXIT_SUCCESS;
108 } else {
109 usage(argv, i);
110 return EXIT_FAILURE;
111 }
112 }
113
114 if ((!opt_display) && (!opt_seqname.empty())) {
115 opt_record_mode = 0;
116 }
117
118 if (opt_fps != 6 && opt_fps != 15 && opt_fps != 30 && opt_fps != 60) {
119 opt_fps = 30; // Default
120 }
121 std::cout << "Resolution : " << opt_width << " x " << opt_height << std::endl;
122 std::cout << "Recording : " << (opt_seqname.empty() ? "disabled" : "enabled") << std::endl;
123 std::cout << "Framerate : " << opt_fps << std::endl;
124 std::cout << "Display : " << (opt_display ? "enabled" : "disabled") << std::endl;
125
126 std::string text_record_mode =
127 std::string("Record mode: ") + (opt_record_mode ? std::string("single") : std::string("continuous"));
128
129 if (!opt_seqname.empty()) {
130 std::cout << text_record_mode << std::endl;
131 std::cout << "Record name: " << opt_seqname << std::endl;
132 }
134
135#ifdef VISP_HAVE_REALSENSE2
136 std::cout << "SDK : Realsense 2" << std::endl;
137 vpRealSense2 g;
138 rs2::config config;
139 config.disable_stream(RS2_STREAM_DEPTH);
140 config.disable_stream(RS2_STREAM_INFRARED);
141 config.enable_stream(RS2_STREAM_COLOR, opt_width, opt_height, RS2_FORMAT_RGBA8, opt_fps);
142 g.open(config);
143#else
144 std::cout << "SDK : Realsense 1" << std::endl;
145 vpRealSense g;
146 g.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(opt_width, opt_height, rs::format::rgba8, 60));
147 g.open();
148#endif
149 g.acquire(I);
150
151 std::cout << "Image size : " << I.getWidth() << " " << I.getHeight() << std::endl;
152
153 vpDisplay *d = NULL;
154 if (opt_display) {
155#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
156 std::cout << "No image viewer is available..." << std::endl;
157 opt_display = false;
158#endif
159 }
160 if (opt_display) {
161#ifdef VISP_HAVE_X11
162 d = new vpDisplayX(I);
163#elif defined(VISP_HAVE_GDI)
164 d = new vpDisplayGDI(I);
165#elif defined(HAVE_OPENCV_HIGHGUI)
166 d = new vpDisplayOpenCV(I);
167#endif
168 }
169
170 vpImageQueue<vpRGBa> image_queue(opt_seqname, opt_record_mode);
171 vpImageStorageWorker<vpRGBa> image_storage_worker(std::ref(image_queue));
172 std::thread image_storage_thread(&vpImageStorageWorker<vpRGBa>::run, &image_storage_worker);
173
174 bool quit = false;
175 while (!quit) {
176 double t = vpTime::measureTimeMs();
177 g.acquire(I);
178
180
181 quit = image_queue.record(I);
182
183 std::stringstream ss;
184 ss << "Acquisition time: " << std::setprecision(3) << vpTime::measureTimeMs() - t << " ms";
185 vpDisplay::displayText(I, I.getHeight() - 20, 10, ss.str(), vpColor::red);
187 }
188 image_queue.cancel();
189 image_storage_thread.join();
190
191 if (d) {
192 delete d;
193 }
194 } catch (const vpException &e) {
195 std::cout << "Catch an exception: " << e << std::endl;
196 }
197#else
198 (void)argc;
199 (void)argv;
200#if !(defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2))
201 std::cout << "Install librealsense version > 2.31.0, configure and build ViSP again to use this example" << std::endl;
202#endif
203#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
204 std::cout << "This tutorial should be built with c++11 support" << std::endl;
205#endif
206#endif
207}
static const vpColor red
Definition vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
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
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
bool open(const rs2::config &cfg=rs2::config())
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
VISP_EXPORT double measureTimeMs()