Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
tutorial-grabber-structure-core.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/vpOccipitalStructure.h>
8 
9 void usage(const char *argv[], int error)
10 {
11  std::cout << "SYNOPSIS" << std::endl
12  << " " << argv[0] << " [--depth-fps <6|15|30|60>]"
13  << " [--depth-fps <6|15|30|60>]"
14  << " [--sxga]"
15  << " [--no-frame-sync]"
16  << " [--record <mode>]"
17  << " [--no-display]"
18  << " [--help] [-h]" << std::endl
19  << std::endl;
20  std::cout << "DESCRIPTION" << std::endl
21  << " --visible-fps <6|15|30|60>" << std::endl
22  << " Visible camera (gray or color) frames per second." << std::endl
23  << " Default: 30." << std::endl
24  << std::endl
25  << " --depth-fps <6|15|30|60>" << std::endl
26  << " Depth camera frames per second." << std::endl
27  << " Default: 30." << std::endl
28  << std::endl
29  << " --sxga" << std::endl
30  << " If available, output 1280x960 high resolution depth array." << std::endl
31  << std::endl
32  << " --no-frame-sync" << std::endl
33  << " If available, disable frame synchronization." << std::endl
34  << std::endl
35  << " --record <mode>" << std::endl
36  << " Allowed values for mode are:" << std::endl
37  << " 0: record all the captures images (continuous mode)," << std::endl
38  << " 1: record only images selected by a user click (single shot mode)." << std::endl
39  << " Default mode: 0" << std::endl
40  << std::endl
41  << " --no-display" << std::endl
42  << " Disable displaying captured images." << std::endl
43  << " When used and sequence name specified, record mode is internally set to 1 (continuous mode)."
44  << std::endl
45  << std::endl
46  << " --help, -h" << std::endl
47  << " Print this helper message." << std::endl
48  << std::endl;
49  std::cout << "USAGE" << std::endl
50  << " Example to visualize images:" << std::endl
51  << " " << argv[0] << std::endl
52  << std::endl
53  << " Example to record a sequence of images:" << std::endl
54  << " " << argv[0] << " --record 0" << std::endl
55  << std::endl
56  << " Example to record a sequence of images at different frame rates:" << std::endl
57  << " " << argv[0] << " --record 0 --depth-fps 15 --visible-fps 10 --no-frame-sync" << std::endl
58  << std::endl
59  << " Example to record single shot images:\n"
60  << " " << argv[0] << " --record 1" << std::endl
61  << std::endl;
62 
63  if (error) {
64  std::cout << "Error" << std::endl
65  << " "
66  << "Unsupported parameter " << argv[error] << std::endl;
67  }
68 }
69 
73 int main(int argc, const char *argv[])
74 {
75 #if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && defined(VISP_HAVE_THREADS)
76  try {
77  std::string opt_seqname_visible = "visible-%04d.png", opt_seqname_depth = "depth-%04d.png";
78  int opt_record_mode = 0;
79  int opt_depth_fps = 30, opt_visible_fps = opt_depth_fps; // frame synchronization by default.
80  bool opt_sxga = false; // Used for high resolution depth array (true => 1280x960).
81  bool opt_frame_sync = true; // Used to set/unset frame synchronization (default: true).
82  bool opt_display = true;
83 
84  for (int i = 1; i < argc; i++) {
85  if (std::string(argv[i]) == "--depth-fps") {
86  opt_depth_fps = std::atoi(argv[i + 1]);
87  i++;
88  }
89  else if (std::string(argv[i]) == "--visible-fps") {
90  opt_visible_fps = std::atoi(argv[i + 1]);
91  i++;
92  }
93  else if (std::string(argv[i]) == "--sxga") {
94  opt_sxga = true;
95  }
96  else if (std::string(argv[i]) == "--no-frame-sync") {
97  opt_frame_sync = false;
98  }
99  else if (std::string(argv[i]) == "--record") {
100  opt_record_mode = std::atoi(argv[i + 1]);
101  i++;
102  }
103  else if (std::string(argv[i]) == "--no-display") {
104  opt_display = false;
105  }
106  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
107  usage(argv, 0);
108  return EXIT_SUCCESS;
109  }
110  else {
111  usage(argv, i);
112  return EXIT_FAILURE;
113  }
114  }
115 
116  if (!opt_display) {
117  opt_record_mode = 0;
118  }
119 
120  std::cout << "Depth framerate : " << opt_depth_fps << std::endl;
121  std::cout << "Visible framerate: " << opt_visible_fps << std::endl;
122  std::cout << "Display : " << (opt_display ? "enabled" : "disabled") << std::endl;
123 
124  std::string text_record_mode =
125  std::string("Record mode: ") + (opt_record_mode ? std::string("single") : std::string("continuous"));
126 
127  std::cout << text_record_mode << std::endl;
128  std::cout << "Visible record name: " << opt_seqname_visible << std::endl;
129  std::cout << "Depth record name: " << opt_seqname_depth << std::endl;
130 
131  vpImage<vpRGBa> I_color, I_depth;
132  vpImage<float> I_depth_raw;
133 
135 
136  // There's an issue in the firmware when visible fps is set between 1 Hz and 2 Hz.
137  // The visible frame is damaged. The depth frame can be streamed at 1Hz without any problem.
138  if (opt_visible_fps < 2) {
139  opt_visible_fps = 2;
140  }
141 
142  ST::CaptureSessionSettings settings;
143  settings.source = ST::CaptureSessionSourceId::StructureCore;
144  settings.structureCore.visibleEnabled = true;
145  settings.frameSyncEnabled = opt_frame_sync;
146  settings.structureCore.depthFramerate = opt_depth_fps;
147  settings.structureCore.visibleFramerate = opt_visible_fps;
148  if (opt_sxga)
149  settings.structureCore.depthResolution = ST::StructureCoreDepthResolution::SXGA;
150  settings.applyExpensiveCorrection = true; // Apply a correction and clean filter to the depth before streaming.
151 
152  bool is_open = g.open(settings);
153 
154  if (is_open) {
155  // Wait some time to at least have 1 frame of each enabled stream (worst case scenario fps = 1Hz).
156  vpTime::wait(1000);
160 
161  vpDisplay *display_visible = nullptr, *display_depth = nullptr;
162  if (opt_display) {
163 #if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
164  std::cout << "No image viewer is available..." << std::endl;
165  opt_display = false;
166 #endif
167  }
168  if (opt_display) {
169 #ifdef VISP_HAVE_X11
170  display_visible = new vpDisplayX(I_color, 10, 10, "Visible image");
171  display_depth = new vpDisplayX(I_depth, 10 + I_color.getWidth(), 10, "Depth image");
172 #elif defined(VISP_HAVE_GDI)
173  display_visible = new vpDisplayGDI(I_color, 10, 10, "Visible image");
174  display_depth = new vpDisplayGDI(I_depth, 10 + I_color.getWidth(), 10, "Depth image");
175 #elif defined(HAVE_OPENCV_HIGHGUI)
176  display_visible = new vpDisplayOpenCV(I_color, 10, 10, "Visible image");
177  display_depth = new vpDisplayOpenCV(I_depth, 10 + I_color.getWidth(), 10, "Depth image");
178 #endif
179  }
180 
181  vpImageQueue<vpRGBa> image_queue_visible(opt_seqname_visible, opt_record_mode);
182  std::thread image_visible_storage_thread;
183 
184  vpImageStorageWorker<vpRGBa> image_visible_storage_worker(std::ref(image_queue_visible));
185  image_visible_storage_thread = std::thread(&vpImageStorageWorker<vpRGBa>::run, &image_visible_storage_worker);
186 
187  vpImageQueue<vpRGBa> image_queue_depth(opt_seqname_depth, opt_record_mode);
188  vpImageStorageWorker<vpRGBa> image_depth_storage_worker(std::ref(image_queue_depth));
189  std::thread image_depth_storage_thread(&vpImageStorageWorker<vpRGBa>::run, &image_depth_storage_worker);
190 
191  bool quit = false;
192  double t;
193  while (!quit) {
194  t = vpTime::measureTimeMs();
195 
196  g.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap);
197 
198  vpDisplay::display(I_color);
199  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
200  vpDisplay::display(I_depth);
201 
202  quit = image_queue_visible.record(I_color);
203  quit |= image_queue_depth.record(I_depth, nullptr, image_queue_visible.getRecordingTrigger(), true);
204 
205  std::stringstream ss;
206  ss << "Acquisition time: " << std::setprecision(3) << vpTime::measureTimeMs() - t << " ms";
207  vpDisplay::displayText(I_depth, I_depth.getHeight() - 20, 10, ss.str(), vpColor::red);
208  vpDisplay::flush(I_color);
209  vpDisplay::flush(I_depth);
210  }
211  image_queue_visible.cancel();
212  image_queue_depth.cancel();
213  image_visible_storage_thread.join();
214  image_depth_storage_thread.join();
215 
216  if (display_visible) {
217  delete display_visible;
218  }
219  if (display_depth) {
220  delete display_depth;
221  }
222  }
223  }
224  catch (const vpException &e) {
225  std::cout << "Catch an exception: " << e << std::endl;
226  }
227 #else
228  (void)argc;
229  (void)argv;
230 #if !(defined(VISP_HAVE_OCCIPITAL_STRUCTURE))
231  std::cout << "Install libStructure, configure and build ViSP again to use this example" << std::endl;
232 #endif
233 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
234  std::cout << "This tutorial should be built with c++11 support" << std::endl;
235 #endif
236 #endif
237 }
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
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
unsigned int getWidth() const
Definition: vpImage.h:245
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:184
unsigned int getHeight(vpOccipitalStructureStream stream_type)
void acquire(vpImage< unsigned char > &gray, bool undistorted=false, double *ts=nullptr)
unsigned int getWidth(vpOccipitalStructureStream stream_type)
bool open(const ST::CaptureSessionSettings &settings)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()