api_how_to.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #pragma once
5 
6 #include <iostream>
7 #include <iomanip>
8 #include <map>
9 #include <utility>
10 #include <vector>
11 #include <librealsense2/rs.hpp>
12 #include "helper.h"
13 
14 using namespace helper;
15 
19 class how_to
20 {
21 public:
23  {
24  // First, create a rs2::context.
25  // The context represents the current platform with respect to connected devices
27 
28  // Using the context we can get all connected devices in a device list
30 
31  rs2::device selected_device;
32  if (devices.size() == 0)
33  {
34  std::cerr << "No device connected, please connect a RealSense device" << std::endl;
35 
36  //To help with the boilerplate code of waiting for a device to connect
37  //The SDK provides the rs2::device_hub class
38  rs2::device_hub device_hub(ctx);
39 
40  //Using the device_hub we can block the program until a device connects
41  selected_device = device_hub.wait_for_device();
42  }
43  else
44  {
45  std::cout << "Found the following devices:\n" << std::endl;
46 
47  // device_list is a "lazy" container of devices which allows
48  //The device list provides 2 ways of iterating it
49  //The first way is using an iterator (in this case hidden in the Range-based for loop)
50  int index = 0;
51  for (rs2::device device : devices)
52  {
53  std::cout << " " << index++ << " : " << get_device_name(device) << std::endl;
54  }
55 
56  uint32_t selected_device_index = get_user_selection("Select a device by index: ");
57 
58  // The second way is using the subscript ("[]") operator:
59  if (selected_device_index >= devices.size())
60  {
61  throw std::out_of_range("Selected device index is out of range");
62  }
63 
64  // Update the selected device
65  selected_device = devices[selected_device_index];
66  }
67 
68  return selected_device;
69  }
70 
72  {
73  // Each device provides some information on itself
74  // The different types of available information are represented using the "RS2_CAMERA_INFO_*" enum
75 
76  std::cout << "Device information: " << std::endl;
77  //The following code shows how to enumerate all of the RS2_CAMERA_INFO
78  //Note that all enum types in the SDK start with the value of zero and end at the "*_COUNT" value
79  for (int i = 0; i < static_cast<int>(RS2_CAMERA_INFO_COUNT); i++)
80  {
81  rs2_camera_info info_type = static_cast<rs2_camera_info>(i);
82  //SDK enum types can be streamed to get a string that represents them
83  std::cout << " " << std::left << std::setw(20) << info_type << " : ";
84 
85  //A device might not support all types of RS2_CAMERA_INFO.
86  //To prevent throwing exceptions from the "get_info" method we first check if the device supports this type of info
87  if (dev.supports(info_type))
88  std::cout << dev.get_info(info_type) << std::endl;
89  else
90  std::cout << "N/A" << std::endl;
91  }
92  }
93 
95  {
96  // Each device provides some information on itself, such as name:
97  std::string name = "Unknown Device";
99  name = dev.get_info(RS2_CAMERA_INFO_NAME);
100 
101  // and the serial number of the device:
102  std::string sn = "########";
105 
106  return name + " " + sn;
107  }
108 
110  {
111  // Sensors support additional information, such as a human readable name
112  if (sensor.supports(RS2_CAMERA_INFO_NAME))
113  return sensor.get_info(RS2_CAMERA_INFO_NAME);
114  else
115  return "Unknown Sensor";
116  }
117 
119  {
120  // A rs2::device is a container of rs2::sensors that have some correlation between them.
121  // For example:
122  // * A device where all sensors are on a single board
123  // * A Robot with mounted sensors that share calibration information
124 
125  // Given a device, we can query its sensors using:
126  std::vector<rs2::sensor> sensors = dev.query_sensors();
127 
128  std::cout << "Device consists of " << sensors.size() << " sensors:\n" << std::endl;
129  int index = 0;
130  // We can now iterate the sensors and print their names
131  for (rs2::sensor sensor : sensors)
132  {
133  std::cout << " " << index++ << " : " << get_sensor_name(sensor) << std::endl;
134  }
135 
136  uint32_t selected_sensor_index = get_user_selection("Select a sensor by index: ");
137 
138  // The second way is using the subscript ("[]") operator:
139  if (selected_sensor_index >= sensors.size())
140  {
141  throw std::out_of_range("Selected sensor index is out of range");
142  }
143 
144  return sensors[selected_sensor_index];
145  }
146 
148  {
149  // Sensors usually have several options to control their properties
150  // such as Exposure, Brightness etc.
151 
152  std::cout << "Sensor supports the following options:\n" << std::endl;
153 
154  // The following loop shows how to iterate over all available options
155  // Starting from 0 until RS2_OPTION_COUNT (exclusive)
156  for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
157  {
158  rs2_option option_type = static_cast<rs2_option>(i);
159  //SDK enum types can be streamed to get a string that represents them
160  std::cout << " " << i << ": " << option_type;
161 
162  // To control an option, use the following api:
163 
164  // First, verify that the sensor actually supports this option
165  if (sensor.supports(option_type))
166  {
167  std::cout << std::endl;
168 
169  // Get a human readable description of the option
170  const char* description = sensor.get_option_description(option_type);
171  std::cout << " Description : " << description << std::endl;
172 
173  // Get the current value of the option
174  float current_value = sensor.get_option(option_type);
175  std::cout << " Current Value : " << current_value << std::endl;
176 
177  //To change the value of an option, please follow the change_sensor_option() function
178  }
179  else
180  {
181  std::cout << " is not supported" << std::endl;
182  }
183  }
184 
185  uint32_t selected_sensor_option = get_user_selection("Select an option by index: ");
186  if (selected_sensor_option >= static_cast<int>(RS2_OPTION_COUNT))
187  {
188  throw std::out_of_range("Selected option is out of range");
189  }
190  return static_cast<rs2_option>(selected_sensor_option);
191  }
192 
193  static float get_depth_units(const rs2::sensor& sensor)
194  {
195  //A Depth stream contains an image that is composed of pixels with depth information.
196  //The value of each pixel is the distance from the camera, in some distance units.
197  //To get the distance in units of meters, each pixel's value should be multiplied by the sensor's depth scale
198  //Here is the way to grab this scale value for a "depth" sensor:
199  if (rs2::depth_sensor dpt_sensor = sensor.as<rs2::depth_sensor>())
200  {
201  float scale = dpt_sensor.get_depth_scale();
202  std::cout << "Scale factor for depth sensor is: " << scale << std::endl;
203  return scale;
204  }
205  else
206  throw std::runtime_error("Given sensor is not a depth sensor");
207  }
208 
210  {
211  // A sensor's stream (rs2::stream_profile) is in general a stream of data with no specific type.
212  // For video streams (streams of images), the sensor that produces the data has a lens and thus has properties such
213  // as a focal point, distortion, and principal point.
214  // To get these intrinsics parameters, we need to take a stream and first check if it is a video stream
215  if (auto video_stream = stream.as<rs2::video_stream_profile>())
216  {
217  try
218  {
219  //If the stream is indeed a video stream, we can now simply call get_intrinsics()
220  rs2_intrinsics intrinsics = video_stream.get_intrinsics();
221 
222  auto principal_point = std::make_pair(intrinsics.ppx, intrinsics.ppy);
223  auto focal_length = std::make_pair(intrinsics.fx, intrinsics.fy);
224  rs2_distortion model = intrinsics.model;
225 
226  std::cout << "Principal Point : " << principal_point.first << ", " << principal_point.second << std::endl;
227  std::cout << "Focal Length : " << focal_length.first << ", " << focal_length.second << std::endl;
228  std::cout << "Distortion Model : " << model << std::endl;
229  std::cout << "Distortion Coefficients : [" << intrinsics.coeffs[0] << "," << intrinsics.coeffs[1] << "," <<
230  intrinsics.coeffs[2] << "," << intrinsics.coeffs[3] << "," << intrinsics.coeffs[4] << "]" << std::endl;
231  }
232  catch (const std::exception& e)
233  {
234  std::cerr << "Failed to get intrinsics for the given stream. " << e.what() << std::endl;
235  }
236  }
237  else if (auto motion_stream = stream.as<rs2::motion_stream_profile>())
238  {
239  try
240  {
241  //If the stream is indeed a motion stream, we can now simply call get_motion_intrinsics()
242  rs2_motion_device_intrinsic intrinsics = motion_stream.get_motion_intrinsics();
243 
244  std::cout << " Scale X cross axis cross axis Bias X \n";
245  std::cout << " cross axis Scale Y cross axis Bias Y \n";
246  std::cout << " cross axis cross axis Scale Z Bias Z \n";
247  for (int i = 0; i < 3; i++)
248  {
249  for (int j = 0; j < 4; j++)
250  {
251  std::cout << intrinsics.data[i][j] << " ";
252  }
253  std::cout << "\n";
254  }
255 
256  std::cout << "Variance of noise for X, Y, Z axis \n";
257  for (int i = 0; i < 3; i++)
258  std::cout << intrinsics.noise_variances[i] << " ";
259  std::cout << "\n";
260 
261  std::cout << "Variance of bias for X, Y, Z axis \n";
262  for (int i = 0; i < 3; i++)
263  std::cout << intrinsics.bias_variances[i] << " ";
264  std::cout << "\n";
265  }
266  catch (const std::exception& e)
267  {
268  std::cerr << "Failed to get intrinsics for the given stream. " << e.what() << std::endl;
269  }
270  }
271  else
272  {
273  std::cerr << "Given stream profile has no intrinsics data" << std::endl;
274  }
275  }
276 
277  static void get_extrinsics(const rs2::stream_profile& from_stream, const rs2::stream_profile& to_stream)
278  {
279  // If the device/sensor that you are using contains more than a single stream, and it was calibrated
280  // then the SDK provides a way of getting the transformation between any two streams (if such exists)
281  try
282  {
283  // Given two streams, use the get_extrinsics_to() function to get the transformation from the stream to the other stream
284  rs2_extrinsics extrinsics = from_stream.get_extrinsics_to(to_stream);
285  std::cout << "Translation Vector : [" << extrinsics.translation[0] << "," << extrinsics.translation[1] << "," << extrinsics.translation[2] << "]\n";
286  std::cout << "Rotation Matrix : [" << extrinsics.rotation[0] << "," << extrinsics.rotation[3] << "," << extrinsics.rotation[6] << "]\n";
287  std::cout << " : [" << extrinsics.rotation[1] << "," << extrinsics.rotation[4] << "," << extrinsics.rotation[7] << "]\n";
288  std::cout << " : [" << extrinsics.rotation[2] << "," << extrinsics.rotation[5] << "," << extrinsics.rotation[8] << "]" << std::endl;
289  }
290  catch (const std::exception& e)
291  {
292  std::cerr << "Failed to get extrinsics for the given streams. " << e.what() << std::endl;
293  }
294  }
295 
296  static void change_sensor_option(const rs2::sensor& sensor, rs2_option option_type)
297  {
298  // Sensors usually have several options to control their properties
299  // such as Exposure, Brightness etc.
300 
301  // To control an option, use the following api:
302 
303  // First, verify that the sensor actually supports this option
304  if (!sensor.supports(option_type))
305  {
306  std::cerr << "This option is not supported by this sensor" << std::endl;
307  return;
308  }
309 
310  // Each option provides its rs2::option_range to provide information on how it can be changed
311  // To get the supported range of an option we do the following:
312 
313  std::cout << "Supported range for option " << option_type << ":" << std::endl;
314 
315  rs2::option_range range = sensor.get_option_range(option_type);
316  float default_value = range.def;
317  float maximum_supported_value = range.max;
318  float minimum_supported_value = range.min;
319  float difference_to_next_value = range.step;
320  std::cout << " Min Value : " << minimum_supported_value << std::endl;
321  std::cout << " Max Value : " << maximum_supported_value << std::endl;
322  std::cout << " Default Value : " << default_value << std::endl;
323  std::cout << " Step : " << difference_to_next_value << std::endl;
324 
325  bool change_option = false;
326  change_option = prompt_yes_no("Change option's value?");
327 
328  if (change_option)
329  {
330  std::cout << "Enter the new value for this option: ";
331  float requested_value;
332  std::cin >> requested_value;
333  std::cout << std::endl;
334 
335  // To set an option to a different value, we can call set_option with a new value
336  try
337  {
338  sensor.set_option(option_type, requested_value);
339  }
340  catch (const rs2::error& e)
341  {
342  // Some options can only be set while the camera is streaming,
343  // and generally the hardware might fail so it is good practice to catch exceptions from set_option
344  std::cerr << "Failed to set option " << option_type << ". (" << e.what() << ")" << std::endl;
345  }
346  }
347  }
348 
350  {
351  // A Sensor is an object that is capable of streaming one or more types of data.
352  // For example:
353  // * A stereo sensor with Left and Right Infrared streams that
354  // creates a stream of depth images
355  // * A motion sensor with an Accelerometer and Gyroscope that
356  // provides a stream of motion information
357 
358  // Using the sensor we can get all of its streaming profiles
359  std::vector<rs2::stream_profile> stream_profiles = sensor.get_stream_profiles();
360 
361  // Usually a sensor provides one or more streams which are identifiable by their stream_type and stream_index
362  // Each of these streams can have several profiles (e.g FHD/HHD/VGA/QVGA resolution, or 90/60/30 fps, etc..)
363  //The following code shows how to go over a sensor's stream profiles, and group the profiles by streams.
364  std::map<std::pair<rs2_stream, int>, int> unique_streams;
365  for (auto&& sp : stream_profiles)
366  {
367  unique_streams[std::make_pair(sp.stream_type(), sp.stream_index())]++;
368  }
369  std::cout << "Sensor consists of " << unique_streams.size() << " streams: " << std::endl;
370  for (size_t i = 0; i < unique_streams.size(); i++)
371  {
372  auto it = unique_streams.begin();
373  std::advance(it, i);
374  std::cout << " - " << it->first.first << " #" << it->first.second << std::endl;
375  }
376 
377  //Next, we go over all the stream profiles and print the details of each one
378  std::cout << "Sensor provides the following stream profiles:" << std::endl;
379  int profile_num = 0;
380  for (rs2::stream_profile stream_profile : stream_profiles)
381  {
382  // A Stream is an abstraction for a sequence of data items of a
383  // single data type, which are ordered according to their time
384  // of creation or arrival.
385  // The stream's data types are represented using the rs2_stream
386  // enumeration
387  rs2_stream stream_data_type = stream_profile.stream_type();
388 
389  // The rs2_stream provides only types of data which are
390  // supported by the RealSense SDK
391  // For example:
392  // * rs2_stream::RS2_STREAM_DEPTH describes a stream of depth images
393  // * rs2_stream::RS2_STREAM_COLOR describes a stream of color images
394  // * rs2_stream::RS2_STREAM_INFRARED describes a stream of infrared images
395 
396  // As mentioned, a sensor can have multiple streams.
397  // In order to distinguish between streams with the same
398  // stream type we can use the following methods:
399 
400  // 1) Each stream type can have multiple occurances.
401  // All streams, of the same type, provided from a single
402  // device have distinct indices:
403  int stream_index = stream_profile.stream_index();
404 
405  // 2) Each stream has a user-friendly name.
406  // The stream's name is not promised to be unique,
407  // rather a human readable description of the stream
408  std::string stream_name = stream_profile.stream_name();
409 
410  // 3) Each stream in the system, which derives from the same
411  // rs2::context, has a unique identifier
412  // This identifier is unique across all streams, regardless of the stream type.
413  int unique_stream_id = stream_profile.unique_id(); // The unique identifier can be used for comparing two streams
414  std::cout << std::setw(3) << profile_num << ": " << stream_data_type << " #" << stream_index;
415 
416  // As noted, a stream is an abstraction.
417  // In order to get additional data for the specific type of a
418  // stream, a mechanism of "Is" and "As" is provided:
419  if (stream_profile.is<rs2::video_stream_profile>()) //"Is" will test if the type tested is of the type given
420  {
421  // "As" will try to convert the instance to the given type
422  rs2::video_stream_profile video_stream_profile = stream_profile.as<rs2::video_stream_profile>();
423 
424  // After using the "as" method we can use the new data type
425  // for additinal operations:
426  std::cout << " (Video Stream: " << video_stream_profile.format() << " " <<
427  video_stream_profile.width() << "x" << video_stream_profile.height() << "@ " << video_stream_profile.fps() << "Hz)";
428  }
429  std::cout << std::endl;
430  profile_num++;
431  }
432 
433  uint32_t selected_profile_index = get_user_selection("Please select the desired streaming profile: ");
434  if (selected_profile_index >= stream_profiles.size())
435  {
436  throw std::out_of_range("Requested profile index is out of range");
437  }
438 
439  return stream_profiles[selected_profile_index];
440  }
441 
442  static void start_streaming_a_profile(const rs2::sensor& sensor, const rs2::stream_profile& stream_profile)
443  {
444  // The sensor controls turning the streaming on and off
445  // To start streaming, two calls must be made with the following order:
446  // 1) open(stream_profiles_to_open)
447  // 2) start(function_to_handle_frames)
448 
449  // Open can be called with a single profile, or with a collection of profiles
450  // Calling open() tries to get exclusive access to the sensor.
451  // Opening a sensor may have side effects such as actually
452  // running, consume power, produce data, etc.
453  sensor.open(stream_profile);
454 
455  std::ostringstream oss;
456  oss << "Displaying profile " << stream_profile.stream_name();
457 
458  // In order to begin getting data from the sensor, we need to register a callback to handle frames (data)
459  // To register a callback, the sensor's start() method should be invoked.
460  // The start() method takes any type of callable object that takes a frame as its parameter
461  // NOTE:
462  // * Since a sensor can stream multiple streams, and start()
463  // takes a single handler, multiple types of frames can
464  // arrive to the handler.
465  // * Different streams' frames arrive on different threads.
466  // This behavior requires the provided frame handler to the
467  // start method to be re-entrant
468 
469  // In this example we have created a class to handle the frames,
470  // and we capture it by reference inside a C++11 lambda which is passed to the start() function
471  helper::frame_viewer display(oss.str());
472  sensor.start([&](rs2::frame f) { display(f); });
473 
474  // At this point, frames will asynchronously arrive to the callback handler
475  // This thread will continue to run in parallel.
476  // To prevent this thread from returning, we block it using the helper wait() function
477  std::cout << "Streaming profile: " << stream_profile.stream_name() << ". Close display window to continue..." << std::endl;
478  display.wait();
479 
480  // To stop streaming, we simply need to call the sensor's stop method
481  // After returning from the call to stop(), no frames will arrive from this sensor
482  sensor.stop();
483 
484  // To complete the stop operation, and release access of the device, we need to call close() per sensor
485  sensor.close();
486  }
487 };
bool prompt_yes_no(const std::string &prompt_msg)
Definition: helper.h:14
rs2_camera_info
Read-only strings that can be queried from the device. Not all information attributes are available o...
Definition: rs_sensor.h:22
::realsense_legacy_msgs::extrinsics_< std::allocator< void > > extrinsics
Definition: extrinsics.h:59
Definition: helper.h:12
GLuint const GLchar * name
std::vector< sensor > query_sensors() const
Definition: rs_device.hpp:25
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
static std::string get_sensor_name(const rs2::sensor &sensor)
Definition: api_how_to.h:109
rs2_distortion
Distortion model: defines how pixel coordinates should be mapped to sensor coordinates.
Definition: rs_types.h:45
float translation[3]
Definition: rs_sensor.h:99
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:10806
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
Definition: rs_frame.hpp:148
device_list query_devices() const
Definition: rs_context.hpp:112
uint32_t size() const
Definition: rs_device.hpp:711
static std::string get_device_name(const rs2::device &dev)
Definition: api_how_to.h:94
float coeffs[5]
Definition: rs_types.h:67
std::string stream_name() const
Definition: rs_frame.hpp:113
GLsizei const GLchar *const * string
GLuint GLuint stream
Definition: glext.h:1790
e
Definition: rmse.py:177
float rotation[9]
Definition: rs_sensor.h:98
static void start_streaming_a_profile(const rs2::sensor &sensor, const rs2::stream_profile &stream_profile)
Definition: api_how_to.h:442
GLuint index
static rs2::sensor get_a_sensor_from_a_device(const rs2::device &dev)
Definition: api_how_to.h:118
GLdouble f
std::ostream & cout()
static void print_device_information(const rs2::device &dev)
Definition: api_how_to.h:71
const char * get_option_description(rs2_option option) const
Definition: rs_options.hpp:32
unsigned int uint32_t
Definition: stdint.h:80
devices
Definition: test-fg.py:9
const char * get_info(rs2_camera_info info) const
Definition: rs_device.hpp:79
T as() const
Definition: rs_sensor.hpp:333
static rs2::stream_profile choose_a_streaming_profile(const rs2::sensor &sensor)
Definition: api_how_to.h:349
GLint j
GLint left
Definition: glext.h:1963
bool supports(rs2_camera_info info) const
Definition: rs_sensor.hpp:125
static void change_sensor_option(const rs2::sensor &sensor, rs2_option option_type)
Definition: api_how_to.h:296
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:165
dictionary intrinsics
Definition: t265_stereo.py:142
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
uint32_t get_user_selection(const std::string &prompt_message)
Definition: helper.h:26
bool supports(rs2_camera_info info) const
Definition: rs_device.hpp:66
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
std::pair< std::string, std::string > get_device_name(const device &dev)
rs2_format format() const
Definition: rs_frame.hpp:44
std::string get_sensor_name(rs2::video_stream_profile c, rs2::video_stream_profile d)
rs2_distortion model
Definition: rs_types.h:66
static void get_extrinsics(const rs2::stream_profile &from_stream, const rs2::stream_profile &to_stream)
Definition: api_how_to.h:277
static void get_field_of_view(const rs2::stream_profile &stream)
Definition: api_how_to.h:209
static auto it
static rs2_option get_sensor_option(const rs2::sensor &sensor)
Definition: api_how_to.h:147
Video stream intrinsics.
Definition: rs_types.h:58
void open(const stream_profile &profile) const
Definition: rs_sensor.hpp:111
std::ostream & cerr()
void display(void)
Definition: boing.c:194
GLsizei range
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:103
int i
void close() const
Definition: rs_sensor.hpp:173
option_range get_option_range(rs2_option option) const
Definition: rs_options.hpp:84
const char * get_info(rs2_camera_info info) const
Definition: rs_sensor.hpp:138
unsigned char advance
device wait_for_device() const
Definition: rs_context.hpp:240
static rs2::device get_a_realsense_device()
Definition: api_how_to.h:22
std::vector< stream_profile > get_stream_profiles() const
Definition: rs_sensor.hpp:219
void set_option(rs2_option option, float value) const
Definition: rs_options.hpp:99
float get_option(rs2_option option) const
Definition: rs_options.hpp:72
auto device
Definition: pyrs_net.cpp:17
int fps() const
Definition: rs_frame.hpp:49
void start(T callback) const
Definition: rs_sensor.hpp:185
static float get_depth_units(const rs2::sensor &sensor)
Definition: api_how_to.h:193
void stop() const
Definition: rs_sensor.hpp:195


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:45:06