Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
tutorial-grabber-realsense-T265.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/vpRealSense2.h>
8 
9 void usage(const char *argv[], int error)
10 {
11  std::cout << "SYNOPSIS" << std::endl
12  << " " << argv[0] << " [--fps <6|15|30|60>]"
13  << " [--record <mode>]"
14  << " [--no-display]"
15  << " [--help] [-h]" << std::endl
16  << std::endl;
17  std::cout << "DESCRIPTION" << std::endl
18  << " --fps <6|15|30|60>" << std::endl
19  << " Frames per second." << std::endl
20  << " Default: 30." << std::endl
21  << std::endl
22  << " --record <mode>" << std::endl
23  << " Allowed values for mode are:" << std::endl
24  << " 0: record all the captures images (continuous mode)," << std::endl
25  << " 1: record only images selected by a user click (single shot mode)." << std::endl
26  << " Default mode: 0" << std::endl
27  << std::endl
28  << " --no-display" << std::endl
29  << " Disable displaying captured images." << std::endl
30  << " When used and sequence name specified, record mode is internally set to 1 (continuous mode)."
31  << std::endl
32  << std::endl
33  << " --help, -h" << std::endl
34  << " Print this helper message." << std::endl
35  << std::endl;
36  std::cout << "USAGE" << std::endl
37  << " Example to visualize images:" << std::endl
38  << " " << argv[0] << std::endl
39  << std::endl
40  << " Example to record a sequence of images:" << std::endl
41  << " " << argv[0] << " --record 0" << std::endl
42  << std::endl
43  << " Example to record single shot images:\n"
44  << " " << argv[0] << " --record 1" << std::endl
45  << std::endl;
46 
47  if (error) {
48  std::cout << "Error" << std::endl
49  << " "
50  << "Unsupported parameter " << argv[error] << std::endl;
51  }
52 }
53 
57 int main(int argc, const char *argv[])
58 {
59 #if defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0)) && defined(VISP_HAVE_THREADS)
60  try {
61  std::string opt_seqname_left = "left-%04d.png", opt_seqname_right = "right-%04d.png";
62  int opt_record_mode = 0;
63  int opt_fps = 30;
64  bool opt_display = true;
65 
66  for (int i = 1; i < argc; i++) {
67  if (std::string(argv[i]) == "--fps") {
68  opt_fps = std::atoi(argv[i + 1]);
69  i++;
70  }
71  else if (std::string(argv[i]) == "--record") {
72  opt_record_mode = std::atoi(argv[i + 1]);
73  i++;
74  }
75  else if (std::string(argv[i]) == "--no-display") {
76  opt_display = false;
77  }
78  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
79  usage(argv, 0);
80  return EXIT_SUCCESS;
81  }
82  else {
83  usage(argv, i);
84  return EXIT_FAILURE;
85  }
86  }
87 
88  if (!opt_display) {
89  opt_record_mode = 0;
90  }
91 
92  std::cout << "Framerate : " << opt_fps << std::endl;
93  std::cout << "Display : " << (opt_display ? "enabled" : "disabled") << std::endl;
94 
95  std::string text_record_mode =
96  std::string("Record mode: ") + (opt_record_mode ? std::string("single") : std::string("continuous"));
97 
98  std::cout << text_record_mode << std::endl;
99  std::cout << "Left record name: " << opt_seqname_left << std::endl;
100  std::cout << "Right record name: " << opt_seqname_right << std::endl;
101 
102  vpImage<unsigned char> I_left, I_right;
103 
104  vpRealSense2 g;
105  rs2::config config;
106  config.enable_stream(RS2_STREAM_FISHEYE, 1);
107  config.enable_stream(RS2_STREAM_FISHEYE, 2);
108  g.open(config);
109 
110  g.acquire(&I_left, &I_right);
111 
112  std::cout << "Image size : " << I_left.getWidth() << " " << I_right.getHeight() << std::endl;
113 
114  vpDisplay *display_left = nullptr, *display_right = nullptr;
115  if (opt_display) {
116 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
117  std::cout << "No image viewer is available..." << std::endl;
118  opt_display = false;
119 #endif
120  }
121  if (opt_display) {
122 #ifdef VISP_HAVE_X11
123  display_left = new vpDisplayX(I_left, 10, 10, "Left image");
124  display_right = new vpDisplayX(I_right, I_left.getWidth(), 10, "Right image");
125 #elif defined(VISP_HAVE_GDI)
126  display_left = new vpDisplayGDI(I_left, 10, 10, "Left image");
127  display_right = new vpDisplayGDI(I_right, I_left.getWidth(), 10, "Right image");
128 #elif defined(HAVE_OPENCV_HIGHGUI)
129  display_left = new vpDisplayOpenCV(I_left, 10, 10, "Left image");
130  display_right = new vpDisplayOpenCV(I_right, I_left.getWidth(), 10, "Right image");
131 #endif
132  }
133 
134  vpImageQueue<unsigned char> image_queue_left(opt_seqname_left, opt_record_mode);
135  vpImageQueue<unsigned char> image_queue_right(opt_seqname_right, opt_record_mode);
136  vpImageStorageWorker<unsigned char> image_left_storage_worker(std::ref(image_queue_left));
137  vpImageStorageWorker<unsigned char> image_right_storage_worker(std::ref(image_queue_right));
138  std::thread image_left_storage_thread(&vpImageStorageWorker<unsigned char>::run, &image_left_storage_worker);
139  std::thread image_right_storage_thread(&vpImageStorageWorker<unsigned char>::run, &image_right_storage_worker);
140 
141  bool quit = false;
142  while (!quit) {
143  double t = vpTime::measureTimeMs();
144 
145  g.acquire(&I_left, &I_right);
146 
147  vpDisplay::display(I_left);
148  vpDisplay::display(I_right);
149 
150  quit = image_queue_left.record(I_left);
151  quit |= image_queue_right.record(I_right, nullptr, image_queue_left.getRecordingTrigger(), true);
152 
153  std::stringstream ss;
154  ss << "Acquisition time: " << std::setprecision(3) << vpTime::measureTimeMs() - t << " ms";
155  vpDisplay::displayText(I_left, I_left.getHeight() - 20, 10, ss.str(), vpColor::red);
156  vpDisplay::flush(I_left);
157  vpDisplay::flush(I_right);
158  }
159  image_queue_left.cancel();
160  image_queue_right.cancel();
161  image_left_storage_thread.join();
162  image_right_storage_thread.join();
163 
164  if (display_left) {
165  delete display_left;
166  }
167  if (display_right) {
168  delete display_right;
169  }
170  }
171  catch (const vpException &e) {
172  std::cout << "Catch an exception: " << e << std::endl;
173  }
174 #else
175  (void)argc;
176  (void)argv;
177 #if !(defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0)))
178  std::cout << "Install librealsense version > 2.31.0, configure and build ViSP again to use this example" << std::endl;
179 #endif
180 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
181  std::cout << "This tutorial should be built with c++11 support" << std::endl;
182 #endif
183 #endif
184 }
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:245
unsigned int getHeight() const
Definition: vpImage.h:184
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()