internal-tests-extrinsic.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 "approx.h"
5 #include <cmath>
6 #include <iostream>
7 #include <chrono>
8 #include <ctime>
9 #include <algorithm>
10 #include <librealsense2/rs.hpp>
12 #include "../../common/tiny-profiler.h"
13 #include "./../src/environment.h"
14 
15 #include <unit-tests-common.h>
16 #include <numeric>
17 #include <stdlib.h>
18 
19 using namespace librealsense;
20 using namespace librealsense::platform;
21 
22 constexpr int ITERATIONS_PER_CONFIG = 100;
23 constexpr int INNER_ITERATIONS_PER_CONFIG = 10;
24 constexpr int DELAY_INCREMENT_THRESHOLD = 3; //[%]
25 constexpr int DELAY_INCREMENT_THRESHOLD_IMU = 8; //[%]
26 constexpr int SPIKE_THRESHOLD = 2; //[stdev]
27 
28 // Input: vector that represent samples of delay to first frame of one stream
29 // Output: - vector of line fitting data according to least squares algorithm
30 // - slope of the fitted line
31 // reference: https://en.wikipedia.org/wiki/Least_squares
32 double line_fitting(const std::vector<double>& y_vec, std::vector<double>y_fit = std::vector<double>())
33 {
34  double ysum = std::accumulate(y_vec.begin(), y_vec.end(), 0.0); //calculate sigma(yi)
35  double xsum = 0;
36  double x2sum = 0;
37  double xysum = 0;
38  auto y_vec_it = y_vec.begin();
39  size_t n = y_vec.size();
40  for (auto i = 0; i < n; i++)
41  {
42  xsum = xsum + i; //sigma(xi)
43  x2sum = x2sum + pow(i, 2); //sigma(x^2i)
44  xysum = xysum + i * *(y_vec_it + i);
45  }
46  double a = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); //slope
47  double b = (x2sum * ysum - xsum * xysum) / (x2sum * n - xsum * xsum); //intercept
48  if (y_fit.size() > 0)
49  {
50  auto it = y_fit.begin();
51  for (auto i = 0; i < n; i++)
52  {
53  *(it + i) = a * i + b;
54  }
55  }
56  return a; // slope is used when checking delay increment
57 }
58 // Input: vector that represent samples of time delay to first frame of one stream
59 // Output: vector of filtered-out spikes
60 void data_filter(const std::vector<double>& stream_vec, std::vector<double>& filtered_stream_vec)
61 {
62  std::vector<double> y_fit(stream_vec.size());
63  double slope = line_fitting(stream_vec, y_fit);
64 
65  auto y_fit_it = y_fit.begin();
66  auto stream_vec_it = stream_vec.begin();
67  std::vector<double> diff_y_fit;
68  for (auto i = 0; i < stream_vec.size(); i++)
69  {
70  double diff = abs(*(y_fit_it + i) - *(stream_vec_it + i));
71  diff_y_fit.push_back(diff);
72  }
73  // calc stdev from fitted linear line
74  double sq_sum = std::inner_product(diff_y_fit.begin(), diff_y_fit.end(), diff_y_fit.begin(), 0.0);
75  double stdev = std::sqrt(sq_sum / diff_y_fit.size());
76 
77  // calc % of each sample from stdev
78  std::vector<double> samples_stdev(diff_y_fit.size());
79  auto v_size = diff_y_fit.size();
80  std::transform(diff_y_fit.begin(), diff_y_fit.end(), samples_stdev.begin(), [stdev](double d) {
81  auto val = d / stdev;
82  return val;
83  }
84  );
85 
86  // filter spikes
87  auto samples_stdev_it = samples_stdev.begin();
88  stream_vec_it = stream_vec.begin();
89  for (auto i = 0; i < samples_stdev.size(); i++)
90  {
91  if (*(samples_stdev_it + i) > SPIKE_THRESHOLD) continue;
92  filtered_stream_vec.push_back(*(stream_vec_it + i));
93  }
94 }
95 TEST_CASE("Extrinsic graph management", "[live][multicam]")
96 {
97  // Require at least one device to be plugged in
99  {
100  std::cout << "Extrinsic graph management started" << std::endl;
101  auto list = ctx.query_devices();
102  REQUIRE(list.size());
103 
104  std::map<std::string, size_t> extrinsic_graph_at_sensor;
106  auto init_size = b._streams.size();
107  std::cout << " Initial Extrinsic Graph size is " << init_size << std::endl;
108 
109  for (int i = 0; i < 10; i++)
110  {
111  std::cout << "Iteration " << i << " : Extrinsic graph map size is " << b._streams.size() << std::endl;
112  // For each device
113  for (auto&& dev : list)
114  {
115  //std::cout << "Dev " << dev.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
116  for (auto&& snr : dev.query_sensors())
117  {
118  std::vector<rs2::stream_profile> profs;
119  REQUIRE_NOTHROW(profs = snr.get_stream_profiles());
120  REQUIRE(profs.size() > 0);
121 
122  std::string snr_id = snr.get_info(RS2_CAMERA_INFO_NAME);
123  snr_id += snr.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
124  if (extrinsic_graph_at_sensor.count(snr_id))
125  {
126  CAPTURE(snr_id);
127  CAPTURE(extrinsic_graph_at_sensor.at(snr_id));
128  CAPTURE(b._streams.size());
129  REQUIRE(b._streams.size() == extrinsic_graph_at_sensor.at(snr_id));
130  }
131  else
132  extrinsic_graph_at_sensor[snr_id] = b._streams.size();
133 
134  rs2_extrinsics extrin{};
135  try {
136  auto prof = profs[0];
137  extrin = prof.get_extrinsics_to(prof);
138  }
139  catch (const rs2::error& e) {
140  // if device isn't calibrated, get_extrinsics must error out (according to old comment. Might not be true under new API)
141  WARN(e.what());
142  continue;
143  }
144 
145  require_identity_matrix(extrin.rotation);
146  require_zero_vector(extrin.translation);
147  }
148  }
149  }
150 
151  auto end_size = b._streams.size();
152  std::cout << " Final Extrinsic Graph size is " << end_size << std::endl;
153  //REQUIRE(end_size == init_size); TODO doesn't pass yet
154  WARN("TODO: Graph size shall be preserved: init " << init_size << " != final " << end_size);
155  }
156 }
157 TEST_CASE("Extrinsic memory leak detection", "[live]")
158 {
159  // Require at least one device to be plugged in
160 
163  {
165 
166  std::cout << "Extrinsic memory leak detection started" << std::endl;
167  bool is_pipe_test[2] = { true, false };
168 
169  for (auto is_pipe : is_pipe_test)
170  {
171  auto list = ctx.query_devices();
172  REQUIRE(list.size());
173  auto dev = list.front();
174  auto sensors = dev.query_sensors();
175 
176  std::string device_type = "L500";
177  if (dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE) && std::string(dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE)) == "D400") device_type = "D400";
178  if (dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE) && std::string(dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE)) == "SR300") device_type = "SR300";
179 
180  bool usb3_device = is_usb3(dev);
181  int fps = usb3_device ? 30 : 15; // In USB2 Mode the devices will switch to lower FPS rates
182  int req_fps = usb3_device ? 60 : 30; // USB2 Mode has only a single resolution for 60 fps which is not sufficient to run the test
183 
184  int width = 848;
185  int height = 480;
186 
187  if (device_type == "L500")
188  {
189  req_fps = 30;
190  width = 640;
191  }
192  auto res = configure_all_supported_streams(dev, width, height, fps);
193  for (auto& s : res.first)
194  {
195  s.close();
196  }
197 
198  // collect a log that contains info about 20 iterations for each stream
199  // the info should include:
200  // 1. extrinsics table size
201  // 2. delay to first frame
202  // 3. delay threshold for each stream (set fps=6 delay as worst case)
203  // the test will succeed only if all 3 conditions are met:
204  // 1. extrinsics table size is preserved over iterations for each stream
205  // 2. no delay increment over iterations
206  // 3. "most" iterations have time to first frame delay below a defined threshold
207 
208  std::vector<size_t> extrinsics_table_size;
209  std::map<std::string, std::vector<double>> streams_delay; // map to vector to collect all data
210  std::map<std::string, std::vector<std::map<unsigned long long, size_t >>> unique_streams_delay;
211  std::map<std::string, size_t> new_frame;
212  std::map<std::string, size_t> extrinsic_graph_at_sensor;
213 
215  if (is_pipe)
216  {
217  std::cout << "==============================================" << std::endl;
218  std::cout << "Pipe Test is running .." << std::endl;
219  }
220  else {
221  std::cout << "==============================================" << std::endl;
222  std::cout << "Sensor Test is running .." << std::endl;
223  }
224 
225  for (auto i = 0; i < ITERATIONS_PER_CONFIG; i++)
226  {
229  size_t cfg_size = 0;
230  std::vector<rs2::stream_profile> valid_stream_profiles;
231  std::map<int, std::vector<rs2::stream_profile>> sensor_stream_profiles;
232  std::map<int, std::vector<rs2_stream>> ds5_sensor_stream_map;
233  for (auto& profile : res.second)
234  {
235  auto fps = profile.fps;
236  if (device_type == "D400" && profile.stream == RS2_STREAM_ACCEL) fps = 250;
237  cfg.enable_stream(profile.stream, profile.index, profile.width, profile.height, profile.format, fps); // all streams in cfg
238  cfg_size += 1;
239  // create stream profiles data structure to open streams per sensor when testing in sensor mode
240  for (auto& s : res.first)
241  {
242  auto stream_profiles = s.get_stream_profiles();
243  for (auto& sp : stream_profiles)
244  {
245  if (!(sp.stream_type() == profile.stream && sp.fps() == fps && sp.stream_index() == profile.index && sp.format() == profile.format)) continue;
246  if (sp.stream_type() == RS2_STREAM_ACCEL || sp.stream_type() == RS2_STREAM_GYRO) sensor_stream_profiles[2].push_back(sp);
247  auto vid = sp.as<rs2::video_stream_profile>();
248  auto h = vid.height();
249  auto w = vid.width();
250  if (!(w == profile.width && h == profile.height)) continue;
251  if (sp.stream_type() == RS2_STREAM_DEPTH || sp.stream_type() == RS2_STREAM_INFRARED || sp.stream_type() == RS2_STREAM_CONFIDENCE) sensor_stream_profiles[0].push_back(sp);
252  if (sp.stream_type() == RS2_STREAM_COLOR) sensor_stream_profiles[1].push_back(sp);
253  }
254  }
255  }
256 
257  for (auto it = new_frame.begin(); it != new_frame.end(); it++)
258  {
259  it->second = 0;
260  }
261  auto start_time = std::chrono::system_clock::now().time_since_epoch();
262  auto start_time_milli = std::chrono::duration_cast<std::chrono::milliseconds>(start_time).count();
263  bool condition = false;
264  std::mutex mutex;
265  std::mutex mutex_2;
266  auto process_frame = [&](const rs2::frame& f)
267  {
268  std::lock_guard<std::mutex> lock(mutex_2);
269  auto stream_type = f.get_profile().stream_name();
270  auto frame_num = f.get_frame_number();
271  auto time_of_arrival = f.get_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL);
272 
273  if (!new_frame[stream_type])
274  {
275  streams_delay[stream_type].push_back(time_of_arrival - start_time_milli);
276  new_frame[stream_type] = true;
277  }
278  new_frame[stream_type] += 1;
279  };
280  auto frame_callback = [&](const rs2::frame& f)
281  {
282  std::lock_guard<std::mutex> lock(mutex);
283  if (rs2::frameset fs = f.as<rs2::frameset>())
284  {
285  // With callbacks, all synchronized stream will arrive in a single frameset
286  for (const rs2::frame& ff : fs)
287  {
288  process_frame(ff);
289  }
290  }
291  else
292  {
293  // Stream that bypass synchronization (such as IMU) will produce single frames
294  process_frame(f);
295  }
296  };
297  if (is_pipe)
298  {
300  }
301  else {
302 
303  int i = 0;
304  for (auto& s : res.first)
305  {
306  if (sensor_stream_profiles.find(i) == sensor_stream_profiles.end()) continue;
307  s.open(sensor_stream_profiles[i]);
308  i += 1;
309  }
310  i = 0;
311  for (auto& s : res.first)
312  {
313  if (sensor_stream_profiles.find(i) == sensor_stream_profiles.end()) continue;
314  s.start(frame_callback);
315  i += 1;
316  }
317  }
318  // to prevent FW issue, at least 20 frames per stream should arrive
319  while (!condition) // the condition is set to true when at least 20 frames are received per stream
320  {
321  try
322  {
323  if (new_frame.size() == cfg_size)
324  {
325  condition = true;
326  for (auto it = new_frame.begin(); it != new_frame.end(); it++)
327  {
328  if (it->second < INNER_ITERATIONS_PER_CONFIG)
329  {
330  condition = false;
331  break;
332  }
333  }
334  // all streams received more than 10 frames
335  }
336  }
337  catch (...)
338  {
339  std::cout << "Iteration failed " << std::endl;
340  }
341  }
342  if (is_pipe)
343  {
344  pipe.stop();
345  }
346  else
347  {
348  // Stop & flush all active sensors. The separation is intended to semi-confirm the FPS
349  for (auto& s : res.first)
350  {
351  s.stop();
352  }
353  for (auto& s : res.first)
354  {
355  s.close();
356  }
357 
358  }
359  extrinsics_table_size.push_back(b._extrinsics.size());
360 
361  }
362  std::cout << "Analyzing info .. " << std::endl;
363 
364  // the test will succeed only if all 3 conditions are met:
365  // 1. extrinsics table size is preserved over iterations for each stream
366  // 2. no delay increment over iterations
367  // 3. "most" iterations have time to first frame delay below a defined threshold
368  if (extrinsics_table_size.size())
369  {
370  CAPTURE(extrinsics_table_size);
371  // 1. extrinsics table preserve its size over iterations
372  CHECK(std::adjacent_find(extrinsics_table_size.begin(), extrinsics_table_size.end(), std::not_equal_to<size_t>()) == extrinsics_table_size.end());
373  }
374  // 2. no delay increment over iterations
375  // filter spikes : calc stdev for each half and filter out samples that are not close
376  for (auto& stream : streams_delay)
377  {
378  // make sure we have enough data for each stream
379  REQUIRE(stream.second.size() > 10);
380 
381  // remove first 5 iterations from each stream
382  stream.second.erase(stream.second.begin(), stream.second.begin() + 5);
383  double slope = line_fitting(stream.second);
384  // check slope value against threshold
386  // IMU streams have different threshold
387  if (stream.first == "Accel" || stream.first == "Gyro") threshold = DELAY_INCREMENT_THRESHOLD_IMU;
388  CAPTURE(stream.first, slope, threshold);
389  CHECK(slope < threshold);
390 
391  }
392  // 3. "most" iterations have time to first frame delay below a defined threshold
393  std::map<std::string, double> delay_thresholds;
394  // D400
395  delay_thresholds["Accel"] = 1200; // ms
396  delay_thresholds["Color"] = 1200; // ms
397  delay_thresholds["Depth"] = 1200; // ms
398  delay_thresholds["Gyro"] = 1200; // ms
399  delay_thresholds["Infrared 1"] = 1200; // ms
400  delay_thresholds["Infrared 2"] = 1200; // ms
401 
402  // L500
403  if (device_type == "L500")
404  {
405  delay_thresholds["Accel"] = 2200; // ms
406  delay_thresholds["Color"] = 2000; // ms
407  delay_thresholds["Depth"] = 2000; // ms
408  delay_thresholds["Gyro"] = 2200; // ms
409  delay_thresholds["Confidence"] = 2000; // ms
410  delay_thresholds["Infrared"] = 1700; // ms
411  }
412 
413  // SR300
414  if (device_type == "SR300")
415  {
416  delay_thresholds["Accel"] = 1200; // ms
417  delay_thresholds["Color"] = 1200; // ms
418  delay_thresholds["Depth"] = 1200; // ms
419  delay_thresholds["Gyro"] = 1200; // ms
420  delay_thresholds["Infrared 1"] = 1200; // ms
421  delay_thresholds["Infrared 2"] = 1200; // ms
422  }
423 
424  for (const auto& stream_ : streams_delay)
425  {
426  std::vector<double> filtered_stream_vec;
427  data_filter(stream_.second, filtered_stream_vec);
428 
429  auto stream = stream_.first;
430  double sum = std::accumulate(filtered_stream_vec.begin(), filtered_stream_vec.end(), 0.0);
431  double avg = sum / filtered_stream_vec.size();
432  std::cout << "Delay of " << stream << " = " << avg * 1.5 << std::endl;
433  CAPTURE(stream);
434  for (auto it = stream_.second.begin(); it != stream_.second.end(); ++it) {
435  CHECK(*it <= delay_thresholds[stream]);
436  }
437  }
438 
439  }
440  }
441 }
442 TEST_CASE("Enable disable all streams", "[live]")
443 {
444  // Require at least one device to be plugged in
446  class streams_cfg
447  {
448  public:
449  streams_cfg(std::pair<std::vector<rs2::sensor>, std::vector<profile>> res) :_res(res)
450  {
451  for (auto& p : res.second)
452  {
453  auto idx = p.index;
454  if (_filtered_streams[p.stream])
455  idx = 0;
456  _filtered_streams[p.stream] = idx;
457  }
458  }
459 
460  void test_configuration(int i, int j, bool enable_all_streams)
461  {
462  filter_streams(i, j, enable_all_streams);
463 
464  // Define frame callback
465  // The callback is executed on a sensor thread and can be called simultaneously from multiple sensors
466  // Therefore any modification to common memory should be done under lock
467  std::mutex mutex;
468  auto callback = [&](const rs2::frame& frame)
469  {
470  std::lock_guard<std::mutex> lock(mutex);
471  if (rs2::frameset fs = frame.as<rs2::frameset>())
472  {
473  // With callbacks, all synchronized stream will arrive in a single frameset
474  for (const rs2::frame& f : fs)
475  _counters[f.get_profile().unique_id()]++;
476  }
477  else
478  {
479  // Stream that bypass synchronization (such as IMU) will produce single frames
480  _counters[frame.get_profile().unique_id()]++;
481  }
482  };
483 
484  // Start streaming through the callback with default recommended configuration
485  // The default video configuration contains Depth and Color streams
486  // If a device is capable to stream IMU data, both Gyro and Accelerometer are enabled by default
487  auto profiles = _pipe.start(_cfg, callback);
488  for (auto p : profiles.get_streams())
489  _stream_names_types[p.unique_id()] = { p.stream_name(), p.stream_type() };
490 
491  std::this_thread::sleep_for(std::chrono::seconds(1));
492  // Collect the enabled streams names
493  for (auto p : _counters)
494  {
495  CAPTURE(_stream_names_types[p.first].first);
496  CHECK(_filtered_streams.count(_stream_names_types[p.first].second) > 0);
497  }
498  _pipe.stop();
499  }
500 
501  private:
502  void filter_streams(int i, int j, bool enable_all_streams)
503  {
504  if (enable_all_streams)
505  _cfg.enable_all_streams();
506  else
507  _cfg.disable_all_streams();
508  auto enabled_streams = select_profiles(_cfg, i);
509  auto disabled_streams = select_profiles(_cfg, j, false);
510  std::vector<profile> tmp;
511  for (auto& p : enabled_streams)
512  {
513  if (disabled_streams.size() > 0 && std::find(disabled_streams.begin(), disabled_streams.end(), p) != disabled_streams.end()) continue; // skip disabled streams
514  tmp.push_back(p);
515  }
516  enabled_streams = tmp;
517  // Update filtered streams according to requested streams and current state (enable all streams or disable all streams)
518  if (!enabled_streams.empty() && !enable_all_streams)
519  {
520  _filtered_streams.clear();
521  for (auto& en_p : enabled_streams)
522  {
523  auto idx = en_p.index;
524  if (_filtered_streams[en_p.stream]) idx = 0; // if IR-1 and IR-2 are enabled, change index to 0
525  _filtered_streams[en_p.stream] = idx;
526  }
527  }
528  // Filter out disabled streams
529  for (auto& p : disabled_streams)
530  {
531  if (_filtered_streams[p.stream] && _filtered_streams[p.stream] == p.index)
532  _filtered_streams.erase(p.stream);
533  }
534  }
535 
536  std::vector<profile> select_profiles(rs2::config& cfg, size_t n, bool enable_streams = true)
537  {
538  std::vector<profile> filtered_profiles;
539  std::vector<profile>::iterator it = _res.second.begin();
540  for (auto idx = 0; idx < n; idx++)
541  {
542  filtered_profiles.push_back(*(it + idx));
543  if (enable_streams)
544  cfg.enable_stream((it + idx)->stream, (it + idx)->index);
545  else
546  cfg.disable_stream((it + idx)->stream, (it + idx)->index);
547  }
548  return filtered_profiles;
549  }
550 
551  std::pair<std::vector<rs2::sensor>, std::vector<profile>> _res;
552  rs2::config _cfg;
553  std::map<rs2_stream, int> _filtered_streams;
554  std::map<int, int> _counters;
555  std::map<int, std::pair < std::string, rs2_stream>> _stream_names_types;
556  rs2::pipeline _pipe;
557  };
558 
560  {
561  auto list = ctx.query_devices();
562  REQUIRE(list.size());
563  auto dev = list.front();
564  auto sensors = dev.query_sensors();
565 
566  REQUIRE(dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE));
568 
569  bool usb3_device = is_usb3(dev);
570  int fps = usb3_device ? 30 : 15; // In USB2 Mode the devices will switch to lower FPS rates
571  int req_fps = usb3_device ? 60 : 30; // USB2 Mode has only a single resolution for 60 fps which is not sufficient to run the test
572 
573  int width = 848;
574  int height = 480;
575 
576  if (device_type == "L500")
577  {
578  req_fps = 30;
579  width = 640;
580  }
581  auto res = configure_all_supported_streams(dev, width, height, fps);
582 
583  // github test : https://github.com/IntelRealSense/librealsense/issues/3919
585  std::map<int, int> counters;
587  std::mutex mutex;
588  std::map<int, std::pair < std::string, rs2_stream>> stream_names_types;
589  auto callback = [&](const rs2::frame& frame)
590  {
591  std::lock_guard<std::mutex> lock(mutex);
592  if (rs2::frameset fs = frame.as<rs2::frameset>())
593  {
594  // With callbacks, all synchronized stream will arrive in a single frameset
595  for (const rs2::frame& f : fs)
596  counters[f.get_profile().unique_id()]++;
597  }
598  else
599  {
600  // Stream that bypass synchronization (such as IMU) will produce single frames
601  counters[frame.get_profile().unique_id()]++;
602  }
603  };
604 
605  CAPTURE("Github test");
606  cfg.enable_all_streams();
608 
609  auto profiles = pipe.start(cfg, callback);
610  for (auto p : profiles.get_streams())
611  stream_names_types[p.unique_id()] = { p.stream_name(), p.stream_type() };
612  std::this_thread::sleep_for(std::chrono::seconds(1));
613  for (auto p : counters)
614  {
615  CAPTURE(stream_names_types[p.first].first);
616  CHECK(stream_names_types[p.first].second != RS2_STREAM_COLOR);
617  }
618  pipe.stop();
619 
620  // Generic tests, in each configuration:
621  // 1. enable or disable all streams, then :
622  // 2. enable/ disable different number of streams. At least 1 stream should be enabled
623 
624  bool streams_state[2] = { true, false };
625  for (auto& enable_all_streams : streams_state)
626  {
627  CAPTURE(enable_all_streams);
628  for (auto i = 0; i < res.second.size(); i++)
629  {
630  for (auto j = 0; j < res.second.size() - 1; j++) // -1 needed to keep at least 1 enabled stream
631  {
632  CAPTURE(i, j);
633  streams_cfg st_cfg(res);
634  st_cfg.test_configuration(i, j, enable_all_streams);
635  }
636  }
637  }
638  }
639 }
640 TEST_CASE("Controls limits validation", "[live]")
641 {
644  {
645  auto list = ctx.query_devices();
646  REQUIRE(list.size());
647 
648  for (auto&& device : list)
649  {
651  continue;
652  auto sensors = device.query_sensors();
653  float limit;
655  for (auto& control : controls)
656  {
657  for (auto& s : sensors)
658  {
660  if (!s.supports(control))
661  break;
662  auto range = s.get_option_range(control);
663  float set_value[3] = { range.min - 10, range.max + 10, std::floor((range.max + range.min) / 2) };
664  for (auto& val : set_value)
665  {
666  CAPTURE(val);
667  CAPTURE(range);
669  REQUIRE_THROWS(s.set_option(control, val));
670  else
671  {
672  s.set_option(control, val);
673  limit = s.get_option(control);
674  REQUIRE(limit == val);
675  }
676  }
677  }
678  }
679  }
680  }
681 }
static const textual_icon lock
Definition: model-views.h:218
GLboolean GLboolean GLboolean b
void data_filter(const std::vector< double > &stream_vec, std::vector< double > &filtered_stream_vec)
double line_fitting(const std::vector< double > &y_vec, std::vector< double >y_fit=std::vector< double >())
rs2_stream stream
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
GLdouble s
GLfloat GLfloat p
Definition: glext.h:12687
constexpr int INNER_ITERATIONS_PER_CONFIG
std::vector< profile > configure_all_supported_streams(rs2::sensor &sensor, int width=640, int height=480, int fps=60)
GLint GLint GLsizei GLuint * counters
Definition: glext.h:5682
device_list query_devices() const
Definition: rs_context.hpp:112
TEST_CASE("Extrinsic graph management","[live][multicam]")
constexpr int DELAY_INCREMENT_THRESHOLD_IMU
std::map< int, std::weak_ptr< const stream_interface > > _streams
Definition: environment.h:62
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
d
Definition: rmse.py:171
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
GLdouble n
Definition: glext.h:1966
GLint limit
Definition: glext.h:9964
e
Definition: rmse.py:177
void require_zero_vector(const float(&vector)[3])
GLuint index
GLboolean GLboolean GLboolean GLboolean a
GLuint GLfloat * val
GLdouble f
GLenum condition
Definition: glext.h:9825
std::ostream & cout()
#define WARN(msg)
Definition: catch.hpp:17431
#define CAPTURE(...)
Definition: catch.hpp:17432
REQUIRE(n_callbacks==1)
GLint GLsizei GLsizei height
#define REQUIRE_THROWS(...)
Definition: catch.hpp:17401
#define SECTION_FROM_TEST_NAME
def find(dir, mask)
Definition: file.py:25
def callback(frame)
Definition: t265_stereo.py:91
void disable_stream(rs2_stream stream, int index=-1)
GLint j
void enable_all_streams()
std::vector< IpDeviceControlData > controls
void log_to_file(rs2_log_severity min_severity, const char *file_path=nullptr)
Definition: rs.hpp:26
void enable_stream(rs2_stream stream_type, int stream_index, int width, int height, rs2_format format=RS2_FORMAT_ANY, int framerate=0)
const std::string & get_info(rs2_camera_info info) const override
Definition: sensor.cpp:621
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:165
static environment & get_instance()
extrinsics_graph & get_extrinsics_graph()
bool make_context(const char *id, rs2::context *ctx, std::string min_api_version="0.0.0")
void require_identity_matrix(const float(&matrix)[9])
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
constexpr int ITERATIONS_PER_CONFIG
static auto it
GLint GLsizei count
constexpr int SPIKE_THRESHOLD
GLsizei range
REQUIRE_NOTHROW(rs2_log(RS2_LOG_SEVERITY_INFO,"Log message using rs2_log()", nullptr))
int i
GLuint res
Definition: glext.h:8856
GLuint GLenum GLenum transform
Definition: glext.h:11553
rs2_format format
pipeline_profile start()
bool is_usb3(const rs2::device &dev)
constexpr int DELAY_INCREMENT_THRESHOLD
#define CHECK(condition)
GLint GLsizei width
T as() const
Definition: rs_frame.hpp:580


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