rs-enumerate-devices.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #include <librealsense2/rs.hpp>
5 #include <librealsense2/rsutil.h>
6 #include <iostream>
7 #include <iomanip>
8 #include <map>
9 #include <set>
10 #include <cstring>
11 #include <cmath>
12 #include <limits>
13 #include <thread>
14 
15 #include "tclap/CmdLine.h"
16 
17 using namespace std;
18 using namespace TCLAP;
19 using namespace rs2;
20 
21 std::vector<std::string> tokenize_floats(string input, char separator) {
22  std::vector<std::string> tokens;
23  stringstream ss(input);
24  string token;
25 
26  while (std::getline(ss, token, separator)) {
27  tokens.push_back(token);
28  }
29 
30  return tokens;
31 }
32 
34 {
35  stringstream ss;
36  ss << " Rotation Matrix:\n";
37 
38  // Align displayed data along decimal point
39  for (auto i = 0; i < 3; ++i)
40  {
41  for (auto j = 0; j < 3; ++j)
42  {
43  std::ostringstream oss;
44  oss << extrinsics.rotation[j * 3 + i];
45  auto tokens = tokenize_floats(oss.str().c_str(), '.');
46  ss << right << setw(4) << tokens[0];
47  if (tokens.size() > 1)
48  ss << "." << left << setw(12) << tokens[1];
49  }
50  ss << endl;
51  }
52 
53  ss << "\n Translation Vector: ";
54  for (auto i = 0u; i < sizeof(extrinsics.translation) / sizeof(extrinsics.translation[0]); ++i)
55  ss << setprecision(15) << extrinsics.translation[i] << " ";
56 
57  cout << ss.str() << endl << endl;
58 }
59 
61 {
62  stringstream ss;
63  ss << "Bias Variances: \t";
64 
65  for (auto i = 0u; i < sizeof(intrinsics.bias_variances) / sizeof(intrinsics.bias_variances[0]); ++i)
66  ss << setprecision(15) << std::fixed << intrinsics.bias_variances[i] << " ";
67 
68  ss << "\nNoise Variances: \t";
69  for (auto i = 0u; i < sizeof(intrinsics.noise_variances) / sizeof(intrinsics.noise_variances[0]); ++i)
70  ss << setprecision(15) << std::fixed << intrinsics.noise_variances[i] << " ";
71 
72  ss << "\nSensitivity : " << std::endl;
73  for (auto i = 0u; i < sizeof(intrinsics.data) / sizeof(intrinsics.data[0]); ++i)
74  {
75  for (auto j = 0u; j < sizeof(intrinsics.data[0]) / sizeof(intrinsics.data[0][0]); ++j)
76  ss << std::right << std::setw(13) << setprecision(6) << intrinsics.data[i][j] << " ";
77  ss << "\n";
78  }
79 
80  cout << ss.str() << endl << endl;
81 }
82 
84 {
85  stringstream ss;
86  ss << left << setw(14) << " Width: " << "\t" << intrinsics.width << endl <<
87  left << setw(14) << " Height: " << "\t" << intrinsics.height << endl <<
88  left << setw(14) << " PPX: " << "\t" << setprecision(15) << intrinsics.ppx << endl <<
89  left << setw(14) << " PPY: " << "\t" << setprecision(15) << intrinsics.ppy << endl <<
90  left << setw(14) << " Fx: " << "\t" << setprecision(15) << intrinsics.fx << endl <<
91  left << setw(14) << " Fy: " << "\t" << setprecision(15) << intrinsics.fy << endl <<
92  left << setw(14) << " Distortion: " << "\t" << rs2_distortion_to_string(intrinsics.model) << endl <<
93  left << setw(14) << " Coeffs: ";
94 
95  for (auto i = 0u; i < sizeof(intrinsics.coeffs) / sizeof(intrinsics.coeffs[0]); ++i)
96  ss << "\t" << setprecision(15) << intrinsics.coeffs[i] << " ";
97 
98  float fov[2];
99  rs2_fov(&intrinsics, fov);
100  ss << endl << left << setw(14) << " FOV (deg): " << "\t" << setprecision(4) << fov[0] << " x " << fov[1];
101 
102  cout << ss.str() << endl << endl;
103 }
104 
106 {
107  bool ret = false;
108  try
109  {
110  intrinsics = profile.get_intrinsics();
111  ret = true;
112  }
113  catch (...)
114  {
115  }
116 
117  return ret;
118 }
119 
121 {
122  bool ret = false;
123  try
124  {
125  intrinsics = profile.get_motion_intrinsics();
126  ret = true;
127  }
128  catch (...)
129  {
130  }
131  return ret;
132 }
133 
137  int width;
138  int height;
139  string stream_name;
140 
142  {
143  return (std::make_tuple(stream, stream_index, width, height) < std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height));
144  }
145 };
146 
150 
151  bool operator <(const stream_and_index& obj) const
152  {
153  return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index));
154  }
155 };
156 
158  const rs2_intrinsics& rhs)
159 {
160  return lhs.width == rhs.width &&
161  lhs.height == rhs.height &&
162  (fabs(lhs.ppx - rhs.ppx) < std::numeric_limits<float>::min()) &&
163  (fabs(lhs.ppy - rhs.ppy) < std::numeric_limits<float>::min()) &&
164  (fabs(lhs.fx - rhs.fx) < std::numeric_limits<float>::min()) &&
165  (fabs(lhs.fy - rhs.fy) < std::numeric_limits<float>::min()) &&
166  lhs.model == rhs.model &&
167  !std::memcmp(lhs.coeffs, rhs.coeffs, sizeof(rhs.coeffs));
168 }
169 
171  const rs2_motion_device_intrinsic& rhs)
172 {
173  return !std::memcmp(&lhs, &rhs, sizeof(rs2_motion_device_intrinsic));
174 }
175 
176 string get_str_formats(const set<rs2_format>& formats)
177 {
178  stringstream ss;
179  for (auto format = formats.begin(); format != formats.end(); ++format)
180  {
181  ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ? "" : "/");
182  }
183  return ss.str();
184 }
185 
186 int main(int argc, char** argv) try
187 {
188  CmdLine cmd("librealsense rs-enumerate-devices tool", ' ', RS2_API_VERSION_STR);
189 
190  SwitchArg short_view_arg("s", "short", "Provide a one-line summary of the devices");
191  SwitchArg compact_view_arg("S", "compact", "Provide a short summary of the devices");
192  SwitchArg show_options_arg("o", "option", "Show all the supported options per subdevice");
193  SwitchArg show_calibration_data_arg("c", "calib_data", "Show extrinsic and intrinsic of all subdevices");
194  SwitchArg show_defaults("d", "defaults", "Show the default streams configuration");
195  ValueArg<string> show_playback_device_arg("p", "playback_device", "Inspect and enumerate playback device (from file)",
196  false, "", "Playback device - ROSBag record full path");
197  cmd.add(short_view_arg);
198  cmd.add(compact_view_arg);
199  cmd.add(show_options_arg);
200  cmd.add(show_calibration_data_arg);
201  cmd.add(show_defaults);
202  cmd.add(show_playback_device_arg);
203 
204  cmd.parse(argc, argv);
205 
206 #ifdef BUILD_EASYLOGGINGPP
208 #endif
209 
210  bool short_view = short_view_arg.getValue();
211  bool compact_view = compact_view_arg.getValue();
212  bool show_options = show_options_arg.getValue();
213  bool show_calibration_data = show_calibration_data_arg.getValue();
214  bool show_modes = !(compact_view || short_view);
215  auto playback_dev_file = show_playback_device_arg.getValue();
216 
217  if ((short_view || compact_view) && (show_options || show_calibration_data))
218  {
219  std::stringstream s;
220  s << "rs-enumerate-devices ";
221  for (int i = 1; i < argc; ++i)
222  s << argv[i] << ' ';
223  std::cout << "Warning: The command line \"" << s.str().c_str()
224  <<"\" has conflicting requests.\n The flags \"-s\" / \"-S\" are compatible with \"-p\" only,"
225  << " other options will be ignored.\nRefer to the tool's documentation for details\n" << std::endl;
226  }
227 
228  // Obtain a list of devices currently present on the system
229  context ctx;
230  rs2::device d;
231  if (!playback_dev_file.empty())
232  d = ctx.load_device(playback_dev_file.data());
233 
234  auto devices_list = ctx.query_devices();
235  size_t device_count = devices_list.size();
236  if (!device_count)
237  {
238  cout << "No device detected. Is it plugged in?\n";
239  return EXIT_SUCCESS;
240  }
241 
242  // Retrieve the viable devices
243  std::vector<rs2::device> devices;
244 
245  for ( auto i = 0u; i < device_count; i++)
246  {
247  try
248  {
249  auto dev = devices_list[i];
250  devices.emplace_back(dev);
251  }
252  catch (const std::exception & e)
253  {
254  std::cout << "Could not create device - " << e.what() <<" . Check SDK logs for details" << std::endl;
255  }
256  catch(...)
257  {
258  std::cout << "Failed to created device. Check SDK logs for details" << std::endl;
259  }
260  }
261 
262  if (short_view || compact_view)
263  {
264  cout << left << setw(30) << "Device Name"
265  << setw(20) << "Serial Number"
266  << setw(20) << "Firmware Version"
267  << endl;
268 
269  for (auto i = 0u; i < devices.size(); ++i)
270  {
271  auto dev = devices[i];
272 
273  cout << left << setw(30) << dev.get_info(RS2_CAMERA_INFO_NAME)
274  << setw(20) << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER)
275  << setw(20) << dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION)
276  << endl;
277  }
278 
279  show_options = show_calibration_data = show_modes = false;
280  }
281 
282  for (auto i = 0u; i < devices.size(); ++i)
283  {
284 
285  auto dev = devices[i];
286 
287  if (!short_view)
288  {
289  // Show which options are supported by this device
290  cout << "Device info: \n";
291  if (auto pb_dev = dev.as<playback>())
292  cout << "Playback Device (" << pb_dev.file_name() << ")" << std::endl;
293 
294  for (auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j)
295  {
296  auto param = static_cast<rs2_camera_info>(j);
297  if (dev.supports(param))
298  cout << " " << left << setw(30) << rs2_camera_info_to_string(rs2_camera_info(param))
299  << ": \t" << dev.get_info(param) << endl;
300  }
301 
302  cout << endl;
303  }
304 
305  if (show_defaults.getValue())
306  {
307  if (dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER))
308  {
309  cout << "Default streams:" << endl;
310  config cfg;
312  pipeline p;
313  auto profile = cfg.resolve(p);
314  for (auto&& sp : profile.get_streams())
315  {
316  cout << " " << sp.stream_name() << " as " << sp.format() << " at " << sp.fps() << " Hz";
317  if (auto vp = sp.as<video_stream_profile>())
318  {
319  cout << "; Resolution: " << vp.width() << "x" << vp.height();
320  }
321  cout << endl;
322  }
323  }
324  else
325  {
326  cout << "Cannot list default streams since the device does not provide a serial number!" << endl;
327  }
328  cout << endl;
329  }
330 
331  if (show_options)
332  {
333  for (auto&& sensor : dev.query_sensors())
334  {
335  cout << "Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) << endl;
336 
337  cout << setw(35) << " Supported options:" << setw(10) << "min" << setw(10)
338  << " max" << setw(6) << " step" << setw(10) << " default" << endl;
339  for (auto j = 0; j < RS2_OPTION_COUNT; ++j)
340  {
341  auto opt = static_cast<rs2_option>(j);
342  if (sensor.supports(opt))
343  {
344  try
345  {
346  auto range = sensor.get_option_range(opt);
347  cout << " " << left << setw(30) << opt << " : "
348  << setw(5) << range.min << "... " << setw(12) << range.max
349  << setw(6) << range.step << setw(10) << range.def << "\n";
350  }
351  catch (const error & e)
352  {
353  cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << endl;
354  }
355  }
356  }
357  cout << endl;
358  }
359  }
360 
361  if (show_modes)
362  {
363  for (auto&& sensor : dev.query_sensors())
364  {
365  cout << "Stream Profiles supported by " << sensor.get_info(RS2_CAMERA_INFO_NAME) << endl;
366 
367  cout << " Supported modes:\n" << setw(16) << " stream" << setw(16)
368  << " resolution" << setw(10) << " fps" << setw(10) << " format" << endl;
369  // Show which streams are supported by this device
370  for (auto&& profile : sensor.get_stream_profiles())
371  {
372  if (auto video = profile.as<video_stream_profile>())
373  {
374  cout << " " << profile.stream_name() << "\t " << video.width() << "x"
375  << video.height() << "\t@ " << profile.fps() << setw(6) << "Hz\t" << profile.format() << endl;
376  }
377  else
378  {
379  cout << " " << profile.stream_name() << "\t N/A\t\t@ " << profile.fps()
380  << setw(6) << "Hz\t" << profile.format() << endl;
381  }
382  }
383 
384  cout << endl;
385  }
386  }
387 
388  // Print Intrinsics
389  if (show_calibration_data)
390  {
391  std::map<stream_and_index, stream_profile> streams;
392  std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_intrinsics>>> intrinsics_map;
393  std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_motion_device_intrinsic>>> motion_intrinsics_map;
394  for (auto&& sensor : dev.query_sensors())
395  {
396  // Intrinsics
397  for (auto&& profile : sensor.get_stream_profiles())
398  {
399  if (auto video = profile.as<video_stream_profile>())
400  {
401  if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
402  {
403  streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
404  }
405 
407  stream_and_resolution stream_res{ profile.stream_type(), profile.stream_index(), video.width(), video.height(), profile.stream_name() };
408  if (safe_get_intrinsics(video, intrinsics))
409  {
410  auto it = std::find_if((intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(), [&](const std::pair<std::set<rs2_format>, rs2_intrinsics>& kvp) {
411  return intrinsics == kvp.second;
412  });
413  if (it == (intrinsics_map[stream_res]).end())
414  {
415  (intrinsics_map[stream_res]).push_back({ {profile.format()}, intrinsics });
416  }
417  else
418  {
419  it->first.insert(profile.format()); // If the intrinsics are equals, add the profile format to format set
420  }
421  }
422  }
423  else
424  {
426  {
427  if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
428  {
429  streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
430  }
431 
433  stream_and_resolution stream_res{ profile.stream_type(), profile.stream_index(), motion.stream_type(), motion.stream_index(), profile.stream_name() };
435  {
436  auto it = std::find_if((motion_intrinsics_map[stream_res]).begin(), (motion_intrinsics_map[stream_res]).end(),
437  [&](const std::pair<std::set<rs2_format>, rs2_motion_device_intrinsic>& kvp)
438  {
439  return motion_intrinsics == kvp.second;
440  });
441  if (it == (motion_intrinsics_map[stream_res]).end())
442  {
443  (motion_intrinsics_map[stream_res]).push_back({ {profile.format()}, motion_intrinsics });
444  }
445  else
446  {
447  it->first.insert(profile.format()); // If the intrinsics are equals, add the profile format to format set
448  }
449  }
450  }
451  else
452  {
454  {
455  if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
456  {
457  streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
458  }
459  }
460  else
461  {
462  cout << " Unhandled profile encountered: " << profile.stream_name() << "/" << profile.format() << std::endl;
463  }
464  }
465  }
466  }
467  }
468 
469  if (intrinsics_map.size())
470  {
471  cout << "Intrinsic Parameters:\n" << endl;
472  for (auto& kvp : intrinsics_map)
473  {
474  auto stream_res = kvp.first;
475  for (auto& intrinsics : kvp.second)
476  {
477  auto formats = "{" + get_str_formats(intrinsics.first) + "}";
478  cout << " Intrinsic of \"" << stream_res.stream_name << "\" / " << stream_res.width << "x"
479  << stream_res.height << " / " << formats << endl;
480  if (intrinsics.second == rs2_intrinsics{})
481  {
482  cout << "Intrinsic NOT available!\n\n";
483  }
484  else
485  {
486  print(intrinsics.second);
487  }
488  }
489  }
490  }
491 
492  if (motion_intrinsics_map.size())
493  {
494  cout << "Motion Intrinsic Parameters:\n" << endl;
495  for (auto& kvp : motion_intrinsics_map)
496  {
497  auto stream_res = kvp.first;
498  for (auto& intrinsics : kvp.second)
499  {
500  auto formats = get_str_formats(intrinsics.first);
501  cout << "Motion Intrinsic of \"" << stream_res.stream_name << "\"\t " << formats << endl;
502  if (intrinsics.second == rs2_motion_device_intrinsic{})
503  {
504  cout << "Intrinsic NOT available!\n\n";
505  }
506  else
507  {
508  print(intrinsics.second);
509  }
510  }
511  }
512  }
513 
514  if (streams.size())
515  {
516  // Print Extrinsics
517  cout << "\nExtrinsic Parameters:" << endl;
519  for (auto kvp1 = streams.begin(); kvp1 != streams.end(); ++kvp1)
520  {
521  for (auto kvp2 = streams.begin(); kvp2 != streams.end(); ++kvp2)
522  {
523  cout << "Extrinsic from \"" << kvp1->second.stream_name() << "\"\t " <<
524  "To" << "\t \"" << kvp2->second.stream_name() << "\" :\n";
525  try
526  {
527  extrinsics = kvp1->second.get_extrinsics_to(kvp2->second);
528  print(extrinsics);
529  }
530  catch (...)
531  {
532  cout << "N/A\n";
533  }
534  }
535  }
536  }
537  }
538  }
539 
540  return EXIT_SUCCESS;
541 }
542 catch (const error & e)
543 {
544  cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << endl;
545  return EXIT_FAILURE;
546 }
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
GLuint GLuint end
::realsense_legacy_msgs::extrinsics_< std::allocator< void > > extrinsics
Definition: extrinsics.h:59
std::vector< std::string > tokenize_floats(string input, char separator)
string get_str_formats(const set< rs2_format > &formats)
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
GLdouble s
rs2_motion_device_intrinsic get_motion_intrinsics() const
Definition: rs_frame.hpp:304
GLfloat GLfloat p
Definition: glext.h:12687
float translation[3]
Definition: rs_sensor.h:99
static void rs2_fov(const struct rs2_intrinsics *intrin, float to_fov[2])
Definition: rsutil.h:176
playback load_device(const std::string &file)
Definition: rs_context.hpp:184
const char * rs2_distortion_to_string(rs2_distortion distortion)
Definition: rs.cpp:1264
device_list query_devices() const
Definition: rs_context.hpp:112
uint32_t size() const
Definition: rs_device.hpp:711
bool safe_get_intrinsics(const video_stream_profile &profile, rs2_intrinsics &intrinsics)
float coeffs[5]
Definition: rs_types.h:67
Definition: cah-model.h:10
d
Definition: rmse.py:171
void enable_device(const std::string &serial)
GLhandleARB obj
Definition: glext.h:4157
e
Definition: rmse.py:177
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
Definition: pose.h:88
float rotation[9]
Definition: rs_sensor.h:98
Definition: parser.hpp:154
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
static const char * log_to_console
Definition: model-views.h:160
not_this_one begin(...)
dictionary streams
Definition: t265_stereo.py:140
std::ostream & cout()
devices
Definition: test-fg.py:9
bool operator<(failed, failed)
GLint GLint GLsizei GLint GLenum format
GLint j
GLint left
Definition: glext.h:1963
void print(const rs2_extrinsics &extrinsics)
bool operator==(const std::shared_ptr< librealsense::video_stream_profile > &a, const std::shared_ptr< librealsense::video_stream_profile > &b)
Definition: src/stream.h:223
bool supports(rs2_camera_info info) const
Definition: rs_sensor.hpp:125
rs2_intrinsics get_intrinsics() const
Definition: rs_frame.hpp:238
dictionary intrinsics
Definition: t265_stereo.py:142
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
T & getValue()
Definition: ValueArg.h:322
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
void next(auto_any_t cur, type2type< T, C > *)
Definition: foreach.hpp:757
int main(int argc, char **argv)
const char * rs2_camera_info_to_string(rs2_camera_info info)
Definition: rs.cpp:1266
rs2_distortion model
Definition: rs_types.h:66
void parse(int argc, const char *const *argv)
Definition: CmdLine.h:433
pipeline_profile resolve(std::shared_ptr< rs2_pipeline > p) const
GLdouble right
static auto it
GLenum GLenum GLenum input
Definition: glext.h:10805
int min(int a, int b)
Definition: lz4s.c:73
GLenum GLfloat param
#define RS2_API_VERSION_STR
Definition: rs.h:43
Video stream intrinsics.
Definition: rs_types.h:58
std::ostream & cerr()
void add(Arg &a)
Definition: CmdLine.h:413
::realsense_legacy_msgs::motion_intrinsics_< std::allocator< void > > motion_intrinsics
GLsizei range
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:103
int i
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
rs2_format format
Definition: Arg.h:57
std::vector< stream_profile > get_stream_profiles() const
Definition: rs_sensor.hpp:219
bool safe_get_motion_intrinsics(const motion_stream_profile &profile, rs2_motion_device_intrinsic &intrinsics)
YYCODETYPE lhs
Definition: sqlite3.c:132469
const std::string & get_failed_function() const
Definition: rs_types.hpp:112


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