12 #include "../../common/tiny-profiler.h" 13 #include "./../src/environment.h" 32 double line_fitting(
const std::vector<double>& y_vec, std::vector<double>y_fit = std::vector<double>())
34 double ysum = std::accumulate(y_vec.begin(), y_vec.end(), 0.0);
38 auto y_vec_it = y_vec.begin();
39 size_t n = y_vec.size();
40 for (
auto i = 0;
i <
n;
i++)
43 x2sum = x2sum + pow(i, 2);
44 xysum = xysum + i * *(y_vec_it +
i);
46 double a = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum);
47 double b = (x2sum * ysum - xsum * xysum) / (x2sum * n - xsum * xsum);
50 auto it = y_fit.begin();
51 for (
auto i = 0;
i <
n;
i++)
53 *(
it +
i) = a *
i + b;
60 void data_filter(
const std::vector<double>& stream_vec, std::vector<double>& filtered_stream_vec)
62 std::vector<double> y_fit(stream_vec.size());
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++)
70 double diff = abs(*(y_fit_it +
i) - *(stream_vec_it +
i));
71 diff_y_fit.push_back(diff);
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());
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) {
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++)
92 filtered_stream_vec.push_back(*(stream_vec_it +
i));
95 TEST_CASE(
"Extrinsic graph management",
"[live][multicam]")
100 std::cout <<
"Extrinsic graph management started" << std::endl;
104 std::map<std::string, size_t> extrinsic_graph_at_sensor;
107 std::cout <<
" Initial Extrinsic Graph size is " << init_size << std::endl;
109 for (
int i = 0;
i < 10;
i++)
111 std::cout <<
"Iteration " <<
i <<
" : Extrinsic graph map size is " <<
b._streams.size() << std::endl;
113 for (
auto&&
dev : list)
116 for (
auto&& snr :
dev.query_sensors())
118 std::vector<rs2::stream_profile> profs;
124 if (extrinsic_graph_at_sensor.count(snr_id))
127 CAPTURE(extrinsic_graph_at_sensor.at(snr_id));
129 REQUIRE(
b._streams.size() == extrinsic_graph_at_sensor.at(snr_id));
132 extrinsic_graph_at_sensor[snr_id] =
b._streams.size();
136 auto prof = profs[0];
137 extrin = prof.get_extrinsics_to(prof);
151 auto end_size =
b._streams.size();
152 std::cout <<
" Final Extrinsic Graph size is " << end_size << std::endl;
154 WARN(
"TODO: Graph size shall be preserved: init " << init_size <<
" != final " << end_size);
166 std::cout <<
"Extrinsic memory leak detection started" << std::endl;
167 bool is_pipe_test[2] = {
true,
false };
169 for (
auto is_pipe : is_pipe_test)
173 auto dev = list.front();
174 auto sensors = dev.query_sensors();
180 bool usb3_device =
is_usb3(dev);
181 int fps = usb3_device ? 30 : 15;
182 int req_fps = usb3_device ? 60 : 30;
187 if (device_type ==
"L500")
193 for (
auto&
s :
res.first)
208 std::vector<size_t> extrinsics_table_size;
209 std::map<std::string, std::vector<double>> streams_delay;
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;
217 std::cout <<
"==============================================" << std::endl;
218 std::cout <<
"Pipe Test is running .." << std::endl;
221 std::cout <<
"==============================================" << std::endl;
222 std::cout <<
"Sensor Test is running .." << std::endl;
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;
240 for (
auto&
s :
res.first)
249 auto w = vid.width();
252 if (sp.stream_type() ==
RS2_STREAM_COLOR) sensor_stream_profiles[1].push_back(sp);
257 for (
auto it = new_frame.begin();
it != new_frame.end();
it++)
262 auto start_time_milli = std::chrono::duration_cast<std::chrono::milliseconds>(start_time).
count();
268 std::lock_guard<std::mutex>
lock(mutex_2);
269 auto stream_type =
f.get_profile().stream_name();
273 if (!new_frame[stream_type])
275 streams_delay[stream_type].push_back(
time_of_arrival - start_time_milli);
276 new_frame[stream_type] =
true;
278 new_frame[stream_type] += 1;
282 std::lock_guard<std::mutex>
lock(mutex);
304 for (
auto&
s :
res.first)
306 if (sensor_stream_profiles.find(i) == sensor_stream_profiles.end())
continue;
307 s.open(sensor_stream_profiles[i]);
311 for (
auto&
s :
res.first)
313 if (sensor_stream_profiles.find(i) == sensor_stream_profiles.end())
continue;
323 if (new_frame.size() == cfg_size)
326 for (
auto it = new_frame.begin();
it != new_frame.end();
it++)
339 std::cout <<
"Iteration failed " << std::endl;
349 for (
auto&
s :
res.first)
353 for (
auto&
s :
res.first)
359 extrinsics_table_size.push_back(
b._extrinsics.size());
362 std::cout <<
"Analyzing info .. " << std::endl;
368 if (extrinsics_table_size.size())
370 CAPTURE(extrinsics_table_size);
372 CHECK(std::adjacent_find(extrinsics_table_size.begin(), extrinsics_table_size.end(), std::not_equal_to<size_t>()) == extrinsics_table_size.end());
376 for (
auto&
stream : streams_delay)
393 std::map<std::string, double> delay_thresholds;
395 delay_thresholds[
"Accel"] = 1200;
396 delay_thresholds[
"Color"] = 1200;
397 delay_thresholds[
"Depth"] = 1200;
398 delay_thresholds[
"Gyro"] = 1200;
399 delay_thresholds[
"Infrared 1"] = 1200;
400 delay_thresholds[
"Infrared 2"] = 1200;
403 if (device_type ==
"L500")
405 delay_thresholds[
"Accel"] = 2200;
406 delay_thresholds[
"Color"] = 2000;
407 delay_thresholds[
"Depth"] = 2000;
408 delay_thresholds[
"Gyro"] = 2200;
409 delay_thresholds[
"Confidence"] = 2000;
410 delay_thresholds[
"Infrared"] = 1700;
414 if (device_type ==
"SR300")
416 delay_thresholds[
"Accel"] = 1200;
417 delay_thresholds[
"Color"] = 1200;
418 delay_thresholds[
"Depth"] = 1200;
419 delay_thresholds[
"Gyro"] = 1200;
420 delay_thresholds[
"Infrared 1"] = 1200;
421 delay_thresholds[
"Infrared 2"] = 1200;
424 for (
const auto& stream_ : streams_delay)
426 std::vector<double> filtered_stream_vec;
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();
434 for (
auto it = stream_.second.begin();
it != stream_.second.end(); ++
it) {
449 streams_cfg(std::pair<std::vector<rs2::sensor>, std::vector<profile>>
res) :_res(
res)
451 for (
auto&
p :
res.second)
454 if (_filtered_streams[
p.stream])
456 _filtered_streams[
p.stream] =
idx;
460 void test_configuration(
int i,
int j,
bool enable_all_streams)
462 filter_streams(i, j, enable_all_streams);
470 std::lock_guard<std::mutex>
lock(mutex);
475 _counters[
f.get_profile().unique_id()]++;
480 _counters[
frame.get_profile().unique_id()]++;
489 _stream_names_types[
p.unique_id()] = {
p.stream_name(),
p.stream_type() };
491 std::this_thread::sleep_for(std::chrono::seconds(1));
493 for (
auto p : _counters)
495 CAPTURE(_stream_names_types[
p.first].first);
496 CHECK(_filtered_streams.count(_stream_names_types[
p.first].second) > 0);
502 void filter_streams(
int i,
int j,
bool enable_all_streams)
504 if (enable_all_streams)
505 _cfg.enable_all_streams();
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)
513 if (disabled_streams.size() > 0 &&
std::find(disabled_streams.begin(), disabled_streams.end(),
p) != disabled_streams.end())
continue;
516 enabled_streams =
tmp;
518 if (!enabled_streams.empty() && !enable_all_streams)
520 _filtered_streams.clear();
521 for (
auto& en_p : enabled_streams)
523 auto idx = en_p.index;
524 if (_filtered_streams[en_p.stream])
idx = 0;
525 _filtered_streams[en_p.stream] =
idx;
529 for (
auto&
p : disabled_streams)
531 if (_filtered_streams[
p.stream] && _filtered_streams[
p.stream] ==
p.index)
532 _filtered_streams.erase(
p.stream);
536 std::vector<profile> select_profiles(
rs2::config&
cfg,
size_t n,
bool enable_streams =
true)
538 std::vector<profile> filtered_profiles;
539 std::vector<profile>::iterator
it = _res.second.begin();
542 filtered_profiles.push_back(*(it +
idx));
548 return filtered_profiles;
551 std::pair<std::vector<rs2::sensor>, std::vector<profile>> _res;
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;
563 auto dev = list.front();
564 auto sensors = dev.query_sensors();
569 bool usb3_device =
is_usb3(dev);
570 int fps = usb3_device ? 30 : 15;
571 int req_fps = usb3_device ? 60 : 30;
576 if (device_type ==
"L500")
588 std::map<int, std::pair < std::string, rs2_stream>> stream_names_types;
591 std::lock_guard<std::mutex>
lock(mutex);
596 counters[
f.get_profile().unique_id()]++;
601 counters[
frame.get_profile().unique_id()]++;
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)
615 CAPTURE(stream_names_types[
p.first].first);
624 bool streams_state[2] = {
true,
false };
625 for (
auto& enable_all_streams : streams_state)
628 for (
auto i = 0;
i <
res.second.size();
i++)
630 for (
auto j = 0;
j <
res.second.size() - 1;
j++)
633 streams_cfg st_cfg(
res);
634 st_cfg.test_configuration(
i,
j, enable_all_streams);
648 for (
auto&&
device : list)
652 auto sensors =
device.query_sensors();
655 for (
auto& control : controls)
657 for (
auto&
s : sensors)
660 if (!
s.supports(control))
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)
672 s.set_option(control, val);
673 limit =
s.get_option(control);
static const textual_icon lock
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_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
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
device_list query_devices() const
TEST_CASE("Extrinsic graph management","[live][multicam]")
constexpr int DELAY_INCREMENT_THRESHOLD_IMU
std::map< int, std::weak_ptr< const stream_interface > > _streams
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
GLfloat GLfloat GLfloat GLfloat h
void require_zero_vector(const float(&vector)[3])
GLboolean GLboolean GLboolean GLboolean a
GLint GLsizei GLsizei height
#define REQUIRE_THROWS(...)
#define SECTION_FROM_TEST_NAME
void disable_stream(rs2_stream stream, int index=-1)
void enable_all_streams()
std::vector< IpDeviceControlData > controls
void log_to_file(rs2_log_severity min_severity, const char *file_path=nullptr)
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
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
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...
constexpr int ITERATIONS_PER_CONFIG
constexpr int SPIKE_THRESHOLD
REQUIRE_NOTHROW(rs2_log(RS2_LOG_SEVERITY_INFO,"Log message using rs2_log()", nullptr))
GLuint GLenum GLenum transform
bool is_usb3(const rs2::device &dev)
constexpr int DELAY_INCREMENT_THRESHOLD