Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
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 
10 void 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 
76 int main(int argc, const char *argv[])
77 {
78 #if defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_THREADS)
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  }
92  else if (std::string(argv[i]) == "--seqname") {
93  opt_seqname = std::string(argv[i + 1]);
94  i++;
95  }
96  else if (std::string(argv[i]) == "--width") {
97  opt_width = std::atoi(argv[i + 1]);
98  i++;
99  }
100  else if (std::string(argv[i]) == "--height") {
101  opt_height = std::atoi(argv[i + 1]);
102  i++;
103  }
104  else if (std::string(argv[i]) == "--record") {
105  opt_record_mode = std::atoi(argv[i + 1]);
106  i++;
107  }
108  else if (std::string(argv[i]) == "--no-display") {
109  opt_display = false;
110  }
111  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
112  usage(argv, 0);
113  return EXIT_SUCCESS;
114  }
115  else {
116  usage(argv, i);
117  return EXIT_FAILURE;
118  }
119  }
120 
121  if ((!opt_display) && (!opt_seqname.empty())) {
122  opt_record_mode = 0;
123  }
124 
125  if (opt_fps != 6 && opt_fps != 15 && opt_fps != 30 && opt_fps != 60) {
126  opt_fps = 30; // Default
127  }
128  std::cout << "Resolution : " << opt_width << " x " << opt_height << std::endl;
129  std::cout << "Recording : " << (opt_seqname.empty() ? "disabled" : "enabled") << std::endl;
130  std::cout << "Framerate : " << opt_fps << std::endl;
131  std::cout << "Display : " << (opt_display ? "enabled" : "disabled") << std::endl;
132 
133  std::string text_record_mode =
134  std::string("Record mode: ") + (opt_record_mode ? std::string("single") : std::string("continuous"));
135 
136  if (!opt_seqname.empty()) {
137  std::cout << text_record_mode << std::endl;
138  std::cout << "Record name: " << opt_seqname << std::endl;
139  }
140  vpImage<vpRGBa> I;
141 
142 #ifdef VISP_HAVE_REALSENSE2
143  std::cout << "SDK : Realsense 2" << std::endl;
144  vpRealSense2 g;
145  rs2::config config;
146  config.disable_stream(RS2_STREAM_DEPTH);
147  config.disable_stream(RS2_STREAM_INFRARED);
148  config.enable_stream(RS2_STREAM_COLOR, opt_width, opt_height, RS2_FORMAT_RGBA8, opt_fps);
149  g.open(config);
150 #else
151  std::cout << "SDK : Realsense 1" << std::endl;
152  vpRealSense g;
153  g.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(opt_width, opt_height, rs::format::rgba8, 60));
154  g.open();
155 #endif
156  g.acquire(I);
157 
158  std::cout << "Image size : " << I.getWidth() << " " << I.getHeight() << std::endl;
159 
160  vpDisplay *d = nullptr;
161  if (opt_display) {
162 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
163  std::cout << "No image viewer is available..." << std::endl;
164  opt_display = false;
165 #endif
166  }
167  if (opt_display) {
168 #ifdef VISP_HAVE_X11
169  d = new vpDisplayX(I);
170 #elif defined(VISP_HAVE_GDI)
171  d = new vpDisplayGDI(I);
172 #elif defined(HAVE_OPENCV_HIGHGUI)
173  d = new vpDisplayOpenCV(I);
174 #endif
175  }
176 
177  vpImageQueue<vpRGBa> image_queue(opt_seqname, opt_record_mode);
178  vpImageStorageWorker<vpRGBa> image_storage_worker(std::ref(image_queue));
179  std::thread image_storage_thread(&vpImageStorageWorker<vpRGBa>::run, &image_storage_worker);
180 
181  bool quit = false;
182  while (!quit) {
183  double t = vpTime::measureTimeMs();
184  g.acquire(I);
185 
187 
188  quit = image_queue.record(I);
189 
190  std::stringstream ss;
191  ss << "Acquisition time: " << std::setprecision(3) << vpTime::measureTimeMs() - t << " ms";
192  vpDisplay::displayText(I, I.getHeight() - 20, 10, ss.str(), vpColor::red);
193  vpDisplay::flush(I);
194  }
195  image_queue.cancel();
196  image_storage_thread.join();
197 
198  if (d) {
199  delete d;
200  }
201  }
202  catch (const vpException &e) {
203  std::cout << "Catch an exception: " << e << std::endl;
204  }
205 #else
206  (void)argc;
207  (void)argv;
208 #if !(defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2))
209  std::cout << "Install librealsense version > 2.31.0, configure and build ViSP again to use this example" << std::endl;
210 #endif
211 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
212  std::cout << "This tutorial should be built with c++11 support" << std::endl;
213 #endif
214 #endif
215 }
static const vpColor red
Definition: vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
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:128
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
unsigned int getWidth() const
Definition: vpImage.h:249
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)
void acquire(std::vector< vpColVector > &pointcloud)
VISP_EXPORT double measureTimeMs()