unit-tests-post-processing.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2018 Intel Corporation. All Rights Reserved.
3 
5 // This set of tests is valid for any number and combination of RealSense cameras, including R200 and F200 //
7 #include "unit-tests-common.h"
9 #include "../include/librealsense2/rs_advanced_mode.hpp"
11 #include <cmath>
12 #include <iostream>
13 #include <chrono>
14 #include <ctime>
15 #include <algorithm>
16 
17 
18 # define SECTION_FROM_TEST_NAME space_to_underscore(Catch::getCurrentContext().getResultCapture()->getCurrentTestName()).c_str()
19 
21 {
22 public:
24  ~post_processing_filters() noexcept {};
25 
26  void configure(const ppf_test_config& filters_cfg);
27  rs2::frame process(rs2::frame input_frame);
28 
29 private:
32 
33  // Declare filters
34  rs2::decimation_filter dec_filter; // Decimation - frame downsampling using median filter
35  rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing
36  rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise
37  rs2::hole_filling_filter hole_filling_filter; // try reconstruct the missing data
38 
39  // Declare disparity transform from depth to disparity and vice versa
42 
43  bool dec_pb = false;
44  bool spat_pb = false;
45  bool temp_pb = false;
46  bool holes_pb = false;
47 
48 };
49 
51 {
52  // Reconfigure the post-processing according to the test spec
53  dec_pb = (filters_cfg.downsample_scale != 1);
55 
56  if ((spat_pb = filters_cfg.spatial_filter))
57  {
61  //spat_filter.set_option(RS2_OPTION_HOLES_FILL, filters_cfg.holes_filling_mode); // Currently disabled
62  }
63 
64  if ((temp_pb = filters_cfg.temporal_filter))
65  {
69  }
70 
71  if ((holes_pb = filters_cfg.holes_filter))
72  {
74  }
75 }
76 
78 {
79  auto processed = input;
80 
81  // The filters are applied in the order mandated by reference design to enable byte-by-byte results verification
82  // Decimation -> Depth2Disparity -> Spatial ->Temporal -> Disparity2Depth -> HolesFilling
83 
84  if (dec_pb)
85  processed = dec_filter.process(processed);
86 
87  // Domain transform is mandatory according to the reference design
88  processed = depth_to_disparity.process(processed);
89 
90  if (spat_pb)
91  processed = spat_filter.process(processed);
92 
93  if (temp_pb)
94  processed = temp_filter.process(processed);
95 
96  processed= disparity_to_depth.process(processed);
97 
98  if (holes_pb)
99  processed = hole_filling_filter.process(processed);
100 
101  return processed;
102 }
103 
104 bool validate_ppf_results(rs2::frame origin_depth, rs2::frame result_depth, const ppf_test_config& reference_data, size_t frame_idx)
105 {
106  std::vector<uint16_t> diff2orig;
107  std::vector<uint16_t> diff2ref;
108 
109  // Basic sanity scenario with no filters applied.
110  // validating domain transform in/out conversion. Requiring input=output
111  bool domain_transform_only = (reference_data.downsample_scale == 1) &&
112  (!reference_data.spatial_filter) && (!reference_data.temporal_filter);
113 
114  auto result_profile = result_depth.get_profile().as<rs2::video_stream_profile>();
115  REQUIRE(result_profile);
116  CAPTURE(result_profile.width());
117  CAPTURE(result_profile.height());
118 
119  REQUIRE(result_profile.width() == reference_data.output_res_x);
120  REQUIRE(result_profile.height() == reference_data.output_res_y);
121 
122  auto pixels = result_profile.width()*result_profile.height();
123  diff2ref.resize(pixels);
124  if (domain_transform_only)
125  diff2orig.resize(pixels);
126 
127  // Pixel-by-pixel comparison of the resulted filtered depth vs data ercorded with external tool
128  auto v1 = reinterpret_cast<const uint16_t*>(result_depth.get_data());
129  auto v2 = reinterpret_cast<const uint16_t*>(reference_data._output_frames[frame_idx].data());
130 
131  for (auto i = 0; i < pixels; i++)
132  {
133  uint16_t diff = std::abs(*v1++ - *v2++);
134  diff2ref[i] = diff;
135  }
136 
137  // validating depth<->disparity domain transformation is lostless.
138  if (domain_transform_only)
139  REQUIRE(profile_diffs("./DomainTransform.txt",diff2orig, 0, 0, frame_idx));
140 
141  // Validate the filters
142  // The differences between the reference code and librealsense implementation are byte-compared below
143  return profile_diffs("./Filterstransform.txt", diff2ref, 0.f, 0, frame_idx);
144 }
145 
146 void compare_frame_md(rs2::frame origin_depth, rs2::frame result_depth)
147 {
149  {
150  bool origin_supported = origin_depth.supports_frame_metadata((rs2_frame_metadata_value)i);
151  bool result_supported = result_depth.supports_frame_metadata((rs2_frame_metadata_value)i);
152  REQUIRE(origin_supported == result_supported);
153  if (origin_supported && result_supported)
154  {
155  //FRAME_TIMESTAMP and SENSOR_TIMESTAMP metadatas are not included in post proccesing frames,
156  //TIME_OF_ARRIVAL continues to increase after post proccesing
159  i == RS2_FRAME_METADATA_TIME_OF_ARRIVAL) continue;
160  rs2_metadata_type origin_val = origin_depth.get_frame_metadata((rs2_frame_metadata_value)i);
161  rs2_metadata_type result_val = result_depth.get_frame_metadata((rs2_frame_metadata_value)i);
162  REQUIRE(origin_val == result_val);
163  }
164  }
165 }
166 
167 // Test file name , Filters configuraiton
168 const std::vector< std::pair<std::string, std::string>> ppf_test_cases = {
169  // All the tests below include depth-disparity domain transformation
170  // Downsample scales 2 and 3 are tested only. scales 4-7 are differ in impementation from the reference code:
171  // In Librealsense for all scales [2-8] the filter is the mean of depth.
172  // I the reference code for [2-3] the filter uses mean of depth, and for 4-7 is switches to man of disparities doe to implementation constrains
173  {"1551257764229", "D435_DS(2)"},
174  {"1551257812956", "D435_DS(3)"},
175  // Downsample + Hole-Filling modes 0/1/2
176  { "1551257880762","D435_DS(2)_HoleFill(0)" },
177  { "1551257882796","D435_DS(2)_HoleFill(1)" },
178  { "1551257884097","D435_DS(2)_HoleFill(2)" },
179  // Downsample + Spatial Filter parameters
180  { "1551257987255", "D435_DS(2)+Spat(A:0.85/D:32/I:3)" },
181  { "1551259481873", "D435_DS(2)+Spat(A:0.5/D:15/I:2)" },
182 // Downsample + Temporal Filter
183 { "1551261946511", "D435_DS(2)+Temp(A:0.25/D:15/P:0)" },
184 { "1551262153516", "D435_DS(2)+Temp(A:0.45/D:25/P:1)" },
185 { "1551262256875", "D435_DS(2)+Temp(A:0.5/D:30/P:4)" },
186 { "1551262841203", "D435_DS(2)+Temp(A:0.5/D:30/P:6)" },
187 { "1551262772964", "D435_DS(2)+Temp(A:0.5/D:30/P:8)" },
188 // Downsample + Spatial + Temporal
189 { "1551262971309", "D435_DS(2)_Spat(A:0.7/D:25/I:2)_Temp(A:0.6/D:15/P:6)" },
190 // Downsample + Spatial + Temporal (+ Hole-Filling)
191 { "1551263177558", "D435_DS(2)_Spat(A:0.7/D:25/I:2)_Temp(A:0.6/D:15/P:6))_HoleFill(1)" },
192 };
193 
194 // The test is intended to check the results of filters applied on a sequence of frames, specifically the temporal filter
195 // that preserves an internal state. The test utilizes rosbag recordings
196 TEST_CASE("Post-Processing Filters sequence validation", "[software-device][post-processing-filters]")
197 {
199 
201  {
202  ppf_test_config test_cfg;
203 
204  for (auto& ppf_test : ppf_test_cases)
205  {
206  CAPTURE(ppf_test.first);
207  CAPTURE(ppf_test.second);
208 
209  WARN("PPF test " << ppf_test.first << "[" << ppf_test.second << "]");
210 
211  // Load the data from configuration and raw frame files
212  if (!load_test_configuration(ppf_test.first, test_cfg))
213  continue;
214 
216 
217  // Apply the retrieved configuration onto a local post-processing chain of filters
218  REQUIRE_NOTHROW(ppf.configure(test_cfg));
219 
220  rs2::software_device dev; // Create software-only device
221  auto depth_sensor = dev.add_sensor("Depth");
222 
223  int width = test_cfg.input_res_x;
224  int height = test_cfg.input_res_y;
225  int depth_bpp = 2; //16bit unsigned
226  int frame_number = 1;
228  width / 2.f, height / 2.f, // Principal point (N/A in this test)
229  test_cfg.focal_length ,test_cfg.focal_length, // Focal Length
230  RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } };
231 
232  auto depth_stream_profile = depth_sensor.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, width, height, 30, depth_bpp, RS2_FORMAT_Z16, depth_intrinsics });
233  depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, test_cfg.depth_units);
234  depth_sensor.add_read_only_option(RS2_OPTION_STEREO_BASELINE, test_cfg.stereo_baseline_mm);
235 
236  // Establish the required chain of filters
237  dev.create_matcher(RS2_MATCHER_DLR_C);
239 
240  depth_sensor.open(depth_stream_profile);
241  depth_sensor.start(sync);
242 
243  size_t frames = (test_cfg.frames_sequence_size > 1) ? test_cfg.frames_sequence_size : 1;
244  for (auto i = 0; i < frames; i++)
245  {
246  // Inject input frame
247  depth_sensor.on_video_frame({ test_cfg._input_frames[i].data(), // Frame pixels from capture API
248  [](void*) {}, // Custom deleter (if required)
249  (int)test_cfg.input_res_x *depth_bpp, // Stride
250  depth_bpp, // Bytes-per-pixels
251  (rs2_time_t)frame_number + i, // Timestamp
252  RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME, // Clock Domain
253  frame_number, // Frame# for potential sync services
254  depth_stream_profile }); // Depth stream profile
255 
256  rs2::frameset fset = sync.wait_for_frames();
257  REQUIRE(fset);
259  REQUIRE(depth);
260 
261  // ... here the actual filters are being applied
262  auto filtered_depth = ppf.process(depth);
263 
264  // Compare the resulted frame versus input
265  validate_ppf_results(depth, filtered_depth, test_cfg, i);
266  }
267  }
268  }
269 }
270 
271 TEST_CASE("Post-Processing Filters metadata validation", "[software-device][post-processing-filters]")
272 {
274 
276  {
277  ppf_test_config test_cfg;
278  for (auto& ppf_test : ppf_test_cases)
279  {
280  CAPTURE(ppf_test.first);
281  CAPTURE(ppf_test.second);
282 
283  WARN("PPF test " << ppf_test.first << "[" << ppf_test.second << "]");
284 
285  // Load the data from configuration and raw frame files
286  if (!load_test_configuration(ppf_test.first, test_cfg))
287  continue;
288 
290 
291  // Apply the retrieved configuration onto a local post-processing chain of filters
292  REQUIRE_NOTHROW(ppf.configure(test_cfg));
293 
294  rs2::software_device dev; // Create software-only device
295  auto depth_sensor = dev.add_sensor("Depth");
296 
297  int width = test_cfg.input_res_x;
298  int height = test_cfg.input_res_y;
299  int depth_bpp = 2; //16bit unsigned
300  int frame_number = 1;
302  width / 2.f, height / 2.f, // Principal point (N/A in this test)
303  test_cfg.focal_length ,test_cfg.focal_length, // Focal Length
304  RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } };
305 
306  auto depth_stream_profile = depth_sensor.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, width, height, 30, depth_bpp, RS2_FORMAT_Z16, depth_intrinsics });
307 
308  // Establish the required chain of filters
309  dev.create_matcher(RS2_MATCHER_DLR_C);
311 
312  depth_sensor.open(depth_stream_profile);
313  depth_sensor.start(sync);
314 
315  size_t frames = (test_cfg.frames_sequence_size > 1) ? test_cfg.frames_sequence_size : 1;
316  for (auto i = 0; i < frames; i++)
317  {
318  //set next frames metadata
320  depth_sensor.set_metadata((rs2_frame_metadata_value)i, rand());
321 
322  // Inject input frame
323  depth_sensor.on_video_frame({ test_cfg._input_frames[i].data(), // Frame pixels from capture API
324  [](void*) {}, // Custom deleter (if required)
325  (int)test_cfg.input_res_x *depth_bpp, // Stride
326  depth_bpp, // Bytes-per-pixels
327  (rs2_time_t)frame_number + i, // Timestamp
328  RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME, // Clock Domain
329  frame_number, // Frame# for potential sync services
330  depth_stream_profile }); // Depth stream profile
331 
332  rs2::frameset fset = sync.wait_for_frames();
333  REQUIRE(fset);
335  REQUIRE(depth);
336 
337  // ... here the actual filters are being applied
338  auto filtered_depth = ppf.process(depth);
339 
340  // Compare the resulted frame metadata versus input
341  compare_frame_md(depth, filtered_depth);
342  }
343  }
344  }
345 }
346 
348 {
349  if (!sub.is<rs2::frameset>())
350  return false;
351  if (full.size() == 0 && sub.size() == 0)
352  return false;
353  for (auto f : full)
354  {
355  if (!sub.first(f.get_profile().stream_type(), f.get_profile().format()))
356  return false;
357  }
358  return true;
359 }
360 
362 {
363  if (!org.is<rs2::frameset>() || !processed.is<rs2::frameset>())
364  return false;
365  if (org.size() != processed.size() || org.size() == 0)
366  return false;
367  for (auto o : org)
368  {
369  auto curr_profile = o.get_profile();
370  bool found = false;
371  processed.foreach_rs([&curr_profile, &found](const rs2::frame& f)
372  {
373  auto processed_profile = f.get_profile();
374  if (curr_profile.unique_id() == processed_profile.unique_id())
375  found = true;
376  });
377  if(!found)
378  return false;
379  }
380  return true;
381 }
382 
383 TEST_CASE("Post-Processing expected output", "[post-processing-filters]")
384 {
386 
388  return;
389 
390  rs2::temporal_filter temporal;
391  rs2::hole_filling_filter hole_filling;
392  rs2::spatial_filter spatial;
393  rs2::decimation_filter decimation(4);
394  rs2::align aligner(RS2_STREAM_COLOR);
395  rs2::colorizer depth_colorizer;
396  rs2::disparity_transform to_disp;
397  rs2::disparity_transform from_disp(false);
398 
400  cfg.enable_all_streams();
401 
402  rs2::pipeline pipe(ctx);
403  auto profile = pipe.start(cfg);
404 
405  bool supports_disparity = false;
406  for (auto s : profile.get_device().query_sensors())
407  {
408  if (s.supports(RS2_OPTION_STEREO_BASELINE))
409  {
410  supports_disparity = true;
411  break;
412  }
413  }
414 
415  rs2::frameset original = pipe.wait_for_frames();
416 
417  //set to set
418  rs2::frameset temp_processed_set = original.apply_filter(temporal);
419  REQUIRE(is_subset(original, temp_processed_set));
420  REQUIRE(is_subset(temp_processed_set, original));
421 
422  rs2::frameset hole_processed_set = original.apply_filter(hole_filling);
423  REQUIRE(is_subset(original, hole_processed_set));
424  REQUIRE(is_subset(hole_processed_set, original));
425 
426  rs2::frameset spatial_processed_set = original.apply_filter(spatial);
427  REQUIRE(is_subset(original, spatial_processed_set));
428  REQUIRE(is_subset(spatial_processed_set, original));
429 
430  rs2::frameset decimation_processed_set = original.apply_filter(decimation);
431  REQUIRE(is_subset(original, decimation_processed_set));
432  REQUIRE(is_subset(decimation_processed_set, original));
433 
434  rs2::frameset align_processed_set = original.apply_filter(aligner);
435  REQUIRE(is_subset(original, align_processed_set));
436  REQUIRE(is_subset(align_processed_set, original));
437 
438  rs2::frameset colorizer_processed_set = original.apply_filter(depth_colorizer);
439  REQUIRE(is_subset(original, colorizer_processed_set));
440  REQUIRE_THROWS(is_subset(colorizer_processed_set, original));
441 
442  rs2::frameset to_disp_processed_set = original.apply_filter(to_disp);
443  if(supports_disparity)
444  REQUIRE_THROWS(is_subset(to_disp_processed_set, original));
445 
446  rs2::frameset from_disp_processed_set = original.apply_filter(from_disp);//should bypass
447  REQUIRE(is_equal(original, from_disp_processed_set));
448 
449  //single to single
450  rs2::video_frame org_depth = original.get_depth_frame();
451 
452  rs2::video_frame temp_processed_frame = org_depth.apply_filter(temporal);
453  REQUIRE_FALSE(temp_processed_frame.is<rs2::frameset>());
454  REQUIRE(temp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
455  REQUIRE(temp_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
456  REQUIRE(org_depth.get_width() == temp_processed_frame.get_width());
457 
458  rs2::video_frame hole_processed_frame = org_depth.apply_filter(hole_filling);
459  REQUIRE_FALSE(hole_processed_frame.is<rs2::frameset>());
460  REQUIRE(hole_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
461  REQUIRE(hole_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
462  REQUIRE(org_depth.get_width() == hole_processed_frame.get_width());
463 
464  rs2::video_frame spatial_processed_frame = org_depth.apply_filter(spatial);
465  REQUIRE_FALSE(spatial_processed_frame.is<rs2::frameset>());
466  REQUIRE(spatial_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
467  REQUIRE(spatial_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
468  REQUIRE(org_depth.get_width() == spatial_processed_frame.get_width());
469 
470  rs2::video_frame decimation_processed_frame = org_depth.apply_filter(decimation);
471  REQUIRE_FALSE(decimation_processed_frame.is<rs2::frameset>());
472  REQUIRE(decimation_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
473  REQUIRE(decimation_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
474  REQUIRE(org_depth.get_width() > decimation_processed_frame.get_width());
475 
476  rs2::video_frame colorizer_processed_frame = org_depth.apply_filter(depth_colorizer);
477  REQUIRE_FALSE(colorizer_processed_frame.is<rs2::frameset>());
478  REQUIRE(colorizer_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
479  REQUIRE(colorizer_processed_frame.get_profile().format() == RS2_FORMAT_RGB8);
480  REQUIRE(org_depth.get_width() == colorizer_processed_frame.get_width());
481 
482  rs2::video_frame to_disp_processed_frame = org_depth.apply_filter(to_disp);
483  REQUIRE_FALSE(to_disp_processed_frame.is<rs2::frameset>());
484  REQUIRE(to_disp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
485  bool is_disp = to_disp_processed_frame.get_profile().format() == RS2_FORMAT_DISPARITY16 ||
486  to_disp_processed_frame.get_profile().format() == RS2_FORMAT_DISPARITY32;
487  if (supports_disparity)
488  {
489  REQUIRE(is_disp);
490  REQUIRE(org_depth.get_width() == to_disp_processed_frame.get_width());
491  }
492 
493  rs2::video_frame from_disp_processed_frame = org_depth.apply_filter(from_disp);//should bypass
494  REQUIRE_FALSE(from_disp_processed_frame.is<rs2::frameset>());
495  REQUIRE(from_disp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
496  REQUIRE(from_disp_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
497  REQUIRE(org_depth.get_width() == from_disp_processed_frame.get_width());
498 
499  pipe.stop();
500 }
501 
502 TEST_CASE("Post-Processing processing pipe", "[post-processing-filters]")
503 {
505 
507  return;
508 
509  rs2::temporal_filter temporal;
510  rs2::hole_filling_filter hole_filling;
511  rs2::spatial_filter spatial;
512  rs2::decimation_filter decimation(4);
513  rs2::align aligner(RS2_STREAM_COLOR);
514  rs2::colorizer depth_colorizer;
515  rs2::disparity_transform to_disp;
516  rs2::disparity_transform from_disp(false);
518 
520  cfg.enable_all_streams();
521 
522  rs2::pipeline pipe(ctx);
523  auto profile = pipe.start(cfg);
524 
525  bool supports_disparity = false;
526  for (auto s : profile.get_device().query_sensors())
527  {
528  if (s.supports(RS2_OPTION_STEREO_BASELINE))
529  {
530  supports_disparity = true;
531  break;
532  }
533  }
534 
535  rs2::frameset original = pipe.wait_for_frames();
536 
537  rs2::frameset full_pipe;
538  int run_for = 10;
539  std::set<int> uids;
540  size_t uid_count = 0;
541  while (run_for--)
542  {
543  full_pipe = pipe.wait_for_frames();
544  full_pipe = full_pipe.apply_filter(decimation);
545  full_pipe = full_pipe.apply_filter(to_disp);
546  full_pipe = full_pipe.apply_filter(spatial);
547  full_pipe = full_pipe.apply_filter(temporal);
548  full_pipe = full_pipe.apply_filter(from_disp);
549  full_pipe = full_pipe.apply_filter(aligner);
550  full_pipe = full_pipe.apply_filter(hole_filling);
551  full_pipe = full_pipe.apply_filter(depth_colorizer);
552  full_pipe = full_pipe.apply_filter(pc);
553 
554  //printf("test frame:\n");
555  full_pipe.foreach_rs([&](const rs2::frame& f) {
556  uids.insert(f.get_profile().unique_id());
557  //printf("stream: %s, format: %d, uid: %d\n", f.get_profile().stream_name().c_str(), f.get_profile().format(), f.get_profile().unique_id());
558  });
559  if (uid_count == 0)
560  uid_count = uids.size();
561  REQUIRE(uid_count == uids.size());
562  }
563 
564  REQUIRE(is_subset(original, full_pipe));
565  REQUIRE_THROWS(is_subset(full_pipe, original));
566  pipe.stop();
567 }
568 
569 TEST_CASE("Align Processing Block", "[live][pipeline][post-processing-filters][!mayfail]") {
571 
572  if (make_context(SECTION_FROM_TEST_NAME, &ctx, "2.20.0"))
573  {
574  auto list = ctx.query_devices();
575  REQUIRE(list.size());
576 
578  rs2::pipeline pipe(ctx);
580  rs2::pipeline_profile pipe_profile;
581 
583  REQUIRE_NOTHROW(pipe_profile = cfg.resolve(pipe));
584  REQUIRE(pipe_profile);
585  REQUIRE_NOTHROW(dev = pipe_profile.get_device());
586  REQUIRE(dev);
588  dev_type PID = get_PID(dev);
589  CAPTURE(PID.first);
590  CAPTURE(PID.second);
591 
592  REQUIRE_NOTHROW(pipe_profile = pipe.start(cfg));
593  REQUIRE(pipe_profile);
594 
595  std::vector<rs2::stream_profile> active_streams;
596  REQUIRE_NOTHROW(active_streams = pipe_profile.get_streams());
597 
598  // Make sure that there is a frame for each stream opened
599  rs2::frameset fs;
600  for (auto i = 0; i < 300; i++)
601  {
602  if ((pipe.try_wait_for_frames(&fs,500)) && (fs.size() == active_streams.size()))
603  {
604  pipe.stop();
605  break;
606  }
607  }
608 
609  // Sanity check and registration of all possible source and target streams for alignment process
610  std::set<rs2_stream> streams_under_test;
612  {
613  // Currently there is no API to explicitely select target in presense of multiple candidates (IR2)
614  if (auto fr = fs.first_or_default(str_type))
615  streams_under_test.insert(str_type);
616  }
617 
618  // Sanity check
619  if (!fs.get_depth_frame() || !streams_under_test.size())
620  {
621  WARN("Align block test requires a device with Depth and Video sensor(s): current device "
622  << "[" << PID.first << ":" << PID.second << "]. Test skipped");
623  return;
624  }
625 
626  // Check depth->{uvc} alignment.
627  // Note that the test is for verification purposes and does not indicate quality of the mapping process
628  WARN("Testing Depth aligned to 2D video stream");
629  for (auto& tgt_stream : streams_under_test)
630  {
631  WARN("Testing Depth aligned to " << rs2_stream_to_string(tgt_stream));
632  rs2::align align_pb(tgt_stream);
633  auto aligned_fs = align_pb.process(fs);
634  auto origin_depth_frame = fs.get_depth_frame();
635  auto aligned_depth_frame = aligned_fs.get_depth_frame(); // Depth frame is replaced by the aligned depth
636  auto reference_frame = aligned_fs.first(tgt_stream);
637 
638  auto orig_dpt_profile = origin_depth_frame.get_profile();
639  auto aligned_dpt_profile = aligned_depth_frame.get_profile();
640  auto aligned_dpth_video_pf = aligned_dpt_profile.as<rs2::video_stream_profile>();
641  auto ref_video_profile = reference_frame.get_profile().as<rs2::video_stream_profile>();
642 
643  // Test: the depth frame retains the core attributes of depth stream
644  // Stream type/format/fps, (Depth units currently not available).
645  // TODO solution for Baseline ???
646  REQUIRE(orig_dpt_profile == aligned_dpt_profile);
647  REQUIRE(orig_dpt_profile.unique_id() != aligned_dpt_profile.unique_id());
648 
649  // Test: the resulted depth frame properties correspond to the target
650  // Resolution, Intrinsic, Extrinsic
651  REQUIRE(aligned_dpth_video_pf.width() == ref_video_profile.width());
652  REQUIRE(aligned_dpth_video_pf.height() == ref_video_profile.height());
653  const auto ref_intr = ref_video_profile.get_intrinsics();
654  const auto align_dpt_intr = aligned_dpth_video_pf.get_intrinsics();
655  for (auto i = 0; i < 5; i++)
656  {
657  REQUIRE(ref_intr.coeffs[i] == approx(align_dpt_intr.coeffs[i]));
658  }
659  REQUIRE(ref_intr.fx == approx(align_dpt_intr.fx));
660  REQUIRE(ref_intr.fy == approx(align_dpt_intr.fy));
661  REQUIRE(ref_intr.ppx == approx(align_dpt_intr.ppx));
662  REQUIRE(ref_intr.ppy == approx(align_dpt_intr.ppy));
663  REQUIRE(ref_intr.model == align_dpt_intr.model);
664  REQUIRE(ref_intr.width == align_dpt_intr.width);
665  REQUIRE(ref_intr.height == align_dpt_intr.height);
666 
667  // Extrinsic tests: Aligned_depth_extrinsic == Target frame extrinsic
668  rs2_extrinsics actual_extrinsics = ref_video_profile.get_extrinsics_to(aligned_dpt_profile);
669  rs2_extrinsics expected_extrinsics = { {1,0,0, 0,1,0, 0,0,1}, {0,0,0} };
670  CAPTURE(actual_extrinsics.rotation);
671  CAPTURE(actual_extrinsics.translation);
672  for (auto i = 0; i < 9; i++)
673  {
674  REQUIRE(actual_extrinsics.rotation[i] == approx(expected_extrinsics.rotation[i]));
675  }
676  for (auto i = 0; i < 3; i++)
677  {
678  REQUIRE(actual_extrinsics.translation[i] == approx(expected_extrinsics.translation[i]));
679  }
680  }
681 
682  WARN("Testing 2D Video stream aligned to Depth sensor");
683  // Check {uvc}->depth alignment.
684  for (auto& tgt_stream : streams_under_test)
685  {
686  WARN("Testing " << rs2_stream_to_string(tgt_stream) << " aligned to Depth");
687  rs2::align align_pb(RS2_STREAM_DEPTH);
688  auto aligned_fs = align_pb.process(fs);
689  auto origin_2D_frame = fs.first(tgt_stream);
690  auto aligned_2D_frame = aligned_fs.first(tgt_stream);
691  auto depth_frame = aligned_fs.get_depth_frame();
692 
693  auto orig_2D_profile = origin_2D_frame.get_profile();
694  auto aligned_2D_profile = aligned_2D_frame.get_profile().as<rs2::video_stream_profile>();
695  auto ref_video_profile = depth_frame.get_profile().as<rs2::video_stream_profile>();
696 
697  // Test: the 2D frame retains the core attributes of the original stream
698  // Stream type/format/fps
699  REQUIRE(orig_2D_profile == aligned_2D_profile);
700  REQUIRE(orig_2D_profile.unique_id() != aligned_2D_profile.unique_id());
701 
702  // Test: the resulted 2D frame properties correspond to the target depth
703  // Resolution, Intrinsic
704  REQUIRE(aligned_2D_profile.width() == ref_video_profile.width());
705  REQUIRE(aligned_2D_profile.height() == ref_video_profile.height());
706  const auto ref_intr = ref_video_profile.get_intrinsics();
707  const auto align_2D_intr = aligned_2D_profile.get_intrinsics();
708  for (auto i = 0; i < 5; i++)
709  {
710  REQUIRE(ref_intr.coeffs[i] == approx(align_2D_intr.coeffs[i]));
711  }
712  REQUIRE(ref_intr.fx == approx(align_2D_intr.fx));
713  REQUIRE(ref_intr.fy == approx(align_2D_intr.fy));
714  REQUIRE(ref_intr.ppx == approx(align_2D_intr.ppx));
715  REQUIRE(ref_intr.ppy == approx(align_2D_intr.ppy));
716  REQUIRE(ref_intr.model == align_2D_intr.model);
717  REQUIRE(ref_intr.width == align_2D_intr.width);
718  REQUIRE(ref_intr.height == align_2D_intr.height);
719 
720  // Extrinsic tests: Aligned_depth_extrinsic == Target frame extrinsic
721  rs2_extrinsics actual_extrinsics = aligned_2D_profile.get_extrinsics_to(ref_video_profile);
722  rs2_extrinsics expected_extrinsics = { {1,0,0, 0,1,0, 0,0,1}, {0,0,0} };
723  CAPTURE(actual_extrinsics.rotation);
724  CAPTURE(actual_extrinsics.translation);
725  for (auto i = 0; i < 9; i++)
726  {
727  REQUIRE(actual_extrinsics.rotation[i] == approx(expected_extrinsics.rotation[i]));
728  }
729  for (auto i = 0; i < 3; i++)
730  {
731  REQUIRE(actual_extrinsics.translation[i] == approx(expected_extrinsics.translation[i]));
732  }
733  }
734  }
735 }
frame apply_filter(filter_interface &filter)
Definition: rs_frame.hpp:593
frameset wait_for_frames(unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
Approx approx(F f)
Definition: approx.h:106
GLdouble s
rs2::disparity_transform disparity_to_depth
Definition: lz4.c:398
float translation[3]
Definition: rs_sensor.h:99
rs2_metadata_type get_frame_metadata(rs2_frame_metadata_value frame_metadata) const
Definition: rs_frame.hpp:497
TEST_CASE("Post-Processing Filters sequence validation","[software-device][post-processing-filters]")
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
Definition: rs_frame.hpp:148
stream_profile get_profile() const
Definition: rs_frame.hpp:557
device_list query_devices() const
Definition: rs_context.hpp:112
bool load_test_configuration(const std::string test_name, ppf_test_config &test_config)
bool try_wait_for_frames(frameset *f, unsigned int timeout_ms=RS2_DEFAULT_TIMEOUT) const
GLint GLint GLsizei GLsizei GLsizei depth
const void * get_data() const
Definition: rs_frame.hpp:545
bool supports_frame_metadata(rs2_frame_metadata_value frame_metadata) const
Definition: rs_frame.hpp:509
bool is_equal(rs2::frameset org, rs2::frameset processed)
unsigned short uint16_t
Definition: stdint.h:79
rs2::frame process(rs2::frame input_frame)
std::vector< std::vector< uint8_t > > _output_frames
bool validate_ppf_results(rs2::frame origin_depth, rs2::frame result_depth, const ppf_test_config &reference_data, size_t frame_idx)
float rotation[9]
Definition: rs_sensor.h:98
frameset wait_for_frames(unsigned int timeout_ms=5000) const
depth_frame get_depth_frame() const
Definition: rs_frame.hpp:1006
frame first_or_default(rs2_stream s, rs2_format f=RS2_FORMAT_ANY) const
Definition: rs_frame.hpp:978
GLdouble f
rs2::frame process(rs2::frame frame) const override
bool is() const
Definition: rs_frame.hpp:570
frame first(rs2_stream s, rs2_format f=RS2_FORMAT_ANY) const
Definition: rs_frame.hpp:995
frameset process(frameset frames)
#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
rs2::disparity_transform depth_to_disparity
std::pair< std::string, bool > dev_type
rs2::hole_filling_filter hole_filling_filter
void enable_all_streams()
GLint GLint GLsizei GLint GLenum GLenum const void * pixels
rs2_intrinsics get_intrinsics() const
Definition: rs_frame.hpp:238
device get_device() const
Definition: rs_pipeline.hpp:83
const std::vector< std::pair< std::string, std::string > > ppf_test_cases
bool make_context(const char *id, rs2::context *ctx, std::string min_api_version="0.0.0")
void foreach_rs(T action) const
Definition: rs_frame.hpp:1107
#define SECTION_FROM_TEST_NAME
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
GLdouble GLdouble GLint GLint GLdouble v1
rs2_format format() const
Definition: rs_frame.hpp:44
pipeline_profile resolve(std::shared_ptr< rs2_pipeline > p) const
void configure(const ppf_test_config &filters_cfg)
std::vector< stream_profile > get_streams() const
Definition: rs_pipeline.hpp:29
dev_type get_PID(rs2::device &dev)
const char * rs2_stream_to_string(rs2_stream stream)
Definition: rs.cpp:1262
GLenum GLenum GLenum input
Definition: glext.h:10805
long long rs2_metadata_type
Definition: rs_types.h:301
bool profile_diffs(const std::string &plot_name, std::vector< T > &distances, const float max_allowed_std, const float outlier, size_t frame_idx)
Video stream intrinsics.
Definition: rs_types.h:58
#define REQUIRE_FALSE(...)
Definition: catch.hpp:17399
REQUIRE_NOTHROW(rs2_log(RS2_LOG_SEVERITY_INFO,"Log message using rs2_log()", nullptr))
int i
void disable_sensitive_options_for(rs2::sensor &sen)
const GLdouble * v2
int unique_id() const
Definition: rs_frame.hpp:54
bool is_subset(rs2::frameset full, rs2::frameset sub)
void compare_frame_md(rs2::frame origin_depth, rs2::frame result_depth)
pipeline_profile start()
double rs2_time_t
Definition: rs_types.h:300
size_t size() const
Definition: rs_frame.hpp:1097
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
void set_option(rs2_option option, float value) const
Definition: rs_options.hpp:99
int get_width() const
Definition: rs_frame.hpp:659
rs2_frame_metadata_value
Per-Frame-Metadata is the set of read-only properties that might be exposed for each individual frame...
Definition: rs_frame.h:29
GLint GLsizei width
std::vector< std::vector< uint8_t > > _input_frames


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