You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

738 lines
31 KiB

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2018 Intel Corporation. All Rights Reserved.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This set of tests is valid for any number and combination of RealSense cameras, including R200 and F200 //
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "unit-tests-common.h"
#include "unit-tests-post-processing.h"
#include <librealsense2/rs_advanced_mode.hpp>
#include <librealsense2/hpp/rs_frame.hpp>
#include <cmath>
#include <iostream>
#include <chrono>
#include <ctime>
#include <algorithm>
# define SECTION_FROM_TEST_NAME space_to_underscore(Catch::getCurrentContext().getResultCapture()->getCurrentTestName()).c_str()
class post_processing_filters
{
public:
post_processing_filters(void) : depth_to_disparity(true),disparity_to_depth(false) {};
~post_processing_filters() noexcept {};
void configure(const ppf_test_config& filters_cfg);
rs2::frame process(rs2::frame input_frame);
private:
post_processing_filters(const post_processing_filters& other);
post_processing_filters(post_processing_filters&& other);
// Declare filters
rs2::decimation_filter dec_filter; // Decimation - frame downsampling using median filter
rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing
rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise
rs2::hole_filling_filter hole_filling_filter; // try reconstruct the missing data
// Declare disparity transform from depth to disparity and vice versa
rs2::disparity_transform depth_to_disparity;
rs2::disparity_transform disparity_to_depth;
bool dec_pb = false;
bool spat_pb = false;
bool temp_pb = false;
bool holes_pb = false;
};
void post_processing_filters::configure(const ppf_test_config& filters_cfg)
{
// Reconfigure the post-processing according to the test spec
dec_pb = (filters_cfg.downsample_scale != 1);
dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, (float)filters_cfg.downsample_scale);
if ((spat_pb = filters_cfg.spatial_filter))
{
spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, filters_cfg.spatial_alpha);
spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, filters_cfg.spatial_delta);
spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, (float)filters_cfg.spatial_iterations);
//spat_filter.set_option(RS2_OPTION_HOLES_FILL, filters_cfg.holes_filling_mode); // Currently disabled
}
if ((temp_pb = filters_cfg.temporal_filter))
{
temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, filters_cfg.temporal_alpha);
temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, filters_cfg.temporal_delta);
temp_filter.set_option(RS2_OPTION_HOLES_FILL, filters_cfg.temporal_persistence);
}
if ((holes_pb = filters_cfg.holes_filter))
{
hole_filling_filter.set_option(RS2_OPTION_HOLES_FILL, float(filters_cfg.holes_filling_mode));
}
}
rs2::frame post_processing_filters::process(rs2::frame input)
{
auto processed = input;
// The filters are applied in the order mandated by reference design to enable byte-by-byte results verification
// Decimation -> Depth2Disparity -> Spatial ->Temporal -> Disparity2Depth -> HolesFilling
if (dec_pb)
processed = dec_filter.process(processed);
// Domain transform is mandatory according to the reference design
processed = depth_to_disparity.process(processed);
if (spat_pb)
processed = spat_filter.process(processed);
if (temp_pb)
processed = temp_filter.process(processed);
processed= disparity_to_depth.process(processed);
if (holes_pb)
processed = hole_filling_filter.process(processed);
return processed;
}
bool validate_ppf_results(rs2::frame origin_depth, rs2::frame result_depth, const ppf_test_config& reference_data, size_t frame_idx)
{
std::vector<uint16_t> diff2orig;
std::vector<uint16_t> diff2ref;
// Basic sanity scenario with no filters applied.
// validating domain transform in/out conversion. Requiring input=output
bool domain_transform_only = (reference_data.downsample_scale == 1) &&
(!reference_data.spatial_filter) && (!reference_data.temporal_filter);
auto result_profile = result_depth.get_profile().as<rs2::video_stream_profile>();
REQUIRE(result_profile);
CAPTURE(result_profile.width());
CAPTURE(result_profile.height());
REQUIRE(result_profile.width() == reference_data.output_res_x);
REQUIRE(result_profile.height() == reference_data.output_res_y);
auto pixels = result_profile.width()*result_profile.height();
diff2ref.resize(pixels);
if (domain_transform_only)
diff2orig.resize(pixels);
// Pixel-by-pixel comparison of the resulted filtered depth vs data ercorded with external tool
auto v1 = reinterpret_cast<const uint16_t*>(result_depth.get_data());
auto v2 = reinterpret_cast<const uint16_t*>(reference_data._output_frames[frame_idx].data());
for (auto i = 0; i < pixels; i++)
{
uint16_t diff = std::abs(*v1++ - *v2++);
diff2ref[i] = diff;
}
// validating depth<->disparity domain transformation is lostless.
if (domain_transform_only)
REQUIRE(profile_diffs("./DomainTransform.txt",diff2orig, 0, 0, frame_idx));
// Validate the filters
// The differences between the reference code and librealsense implementation are byte-compared below
return profile_diffs("./Filterstransform.txt", diff2ref, 0.f, 0, frame_idx);
}
void compare_frame_md(rs2::frame origin_depth, rs2::frame result_depth)
{
for (auto i = 0; i < rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT; i++)
{
bool origin_supported = origin_depth.supports_frame_metadata((rs2_frame_metadata_value)i);
bool result_supported = result_depth.supports_frame_metadata((rs2_frame_metadata_value)i);
REQUIRE(origin_supported == result_supported);
if (origin_supported && result_supported)
{
//FRAME_TIMESTAMP and SENSOR_TIMESTAMP metadatas are not included in post proccesing frames,
//TIME_OF_ARRIVAL continues to increase after post proccesing
if (i == RS2_FRAME_METADATA_FRAME_TIMESTAMP ||
i == RS2_FRAME_METADATA_SENSOR_TIMESTAMP ||
i == RS2_FRAME_METADATA_TIME_OF_ARRIVAL) continue;
rs2_metadata_type origin_val = origin_depth.get_frame_metadata((rs2_frame_metadata_value)i);
rs2_metadata_type result_val = result_depth.get_frame_metadata((rs2_frame_metadata_value)i);
REQUIRE(origin_val == result_val);
}
}
}
// Test file name , Filters configuraiton
const std::vector< std::pair<std::string, std::string>> ppf_test_cases = {
// All the tests below include depth-disparity domain transformation
// Downsample scales 2 and 3 are tested only. scales 4-7 are differ in impementation from the reference code:
// In Librealsense for all scales [2-8] the filter is the mean of depth.
// 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
{"1551257764229", "D435_DS(2)"},
{"1551257812956", "D435_DS(3)"},
// Downsample + Hole-Filling modes 0/1/2
{ "1551257880762","D435_DS(2)_HoleFill(0)" },
{ "1551257882796","D435_DS(2)_HoleFill(1)" },
{ "1551257884097","D435_DS(2)_HoleFill(2)" },
// Downsample + Spatial Filter parameters
{ "1551257987255", "D435_DS(2)+Spat(A:0.85/D:32/I:3)" },
{ "1551259481873", "D435_DS(2)+Spat(A:0.5/D:15/I:2)" },
// Downsample + Temporal Filter
{ "1551261946511", "D435_DS(2)+Temp(A:0.25/D:15/P:0)" },
{ "1551262153516", "D435_DS(2)+Temp(A:0.45/D:25/P:1)" },
{ "1551262256875", "D435_DS(2)+Temp(A:0.5/D:30/P:4)" },
{ "1551262841203", "D435_DS(2)+Temp(A:0.5/D:30/P:6)" },
{ "1551262772964", "D435_DS(2)+Temp(A:0.5/D:30/P:8)" },
// Downsample + Spatial + Temporal
{ "1551262971309", "D435_DS(2)_Spat(A:0.7/D:25/I:2)_Temp(A:0.6/D:15/P:6)" },
// Downsample + Spatial + Temporal (+ Hole-Filling)
{ "1551263177558", "D435_DS(2)_Spat(A:0.7/D:25/I:2)_Temp(A:0.6/D:15/P:6))_HoleFill(1)" },
};
// The test is intended to check the results of filters applied on a sequence of frames, specifically the temporal filter
// that preserves an internal state. The test utilizes rosbag recordings
TEST_CASE("Post-Processing Filters sequence validation", "[software-device][post-processing-filters]")
{
rs2::context ctx = make_context( SECTION_FROM_TEST_NAME );
if( ctx )
{
ppf_test_config test_cfg;
for (auto& ppf_test : ppf_test_cases)
{
CAPTURE(ppf_test.first);
CAPTURE(ppf_test.second);
WARN("PPF test " << ppf_test.first << "[" << ppf_test.second << "]");
// Load the data from configuration and raw frame files
if (!load_test_configuration(ppf_test.first, test_cfg))
continue;
post_processing_filters ppf;
// Apply the retrieved configuration onto a local post-processing chain of filters
REQUIRE_NOTHROW(ppf.configure(test_cfg));
rs2::software_device dev; // Create software-only device
auto depth_sensor = dev.add_sensor("Depth");
int width = test_cfg.input_res_x;
int height = test_cfg.input_res_y;
int depth_bpp = 2; //16bit unsigned
int frame_number = 1;
rs2_intrinsics depth_intrinsics = { width, height,
width / 2.f, height / 2.f, // Principal point (N/A in this test)
test_cfg.focal_length ,test_cfg.focal_length, // Focal Length
RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } };
auto depth_stream_profile = depth_sensor.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, width, height, 30, depth_bpp, RS2_FORMAT_Z16, depth_intrinsics });
depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, test_cfg.depth_units);
depth_sensor.add_read_only_option(RS2_OPTION_STEREO_BASELINE, test_cfg.stereo_baseline_mm);
// Establish the required chain of filters
dev.create_matcher(RS2_MATCHER_DLR_C);
rs2::syncer sync;
depth_sensor.open(depth_stream_profile);
depth_sensor.start(sync);
size_t frames = (test_cfg.frames_sequence_size > 1) ? test_cfg.frames_sequence_size : 1;
for (auto i = 0; i < frames; i++)
{
// Inject input frame
depth_sensor.on_video_frame({ test_cfg._input_frames[i].data(), // Frame pixels from capture API
[](void*) {}, // Custom deleter (if required)
(int)test_cfg.input_res_x *depth_bpp, // Stride
depth_bpp, // Bytes-per-pixels
(rs2_time_t)frame_number + i, // Timestamp
RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME, // Clock Domain
frame_number, // Frame# for potential sync services
depth_stream_profile, // Depth stream profile
test_cfg.depth_units }); // Depth units
rs2::frameset fset = sync.wait_for_frames();
REQUIRE(fset);
rs2::frame depth = fset.first_or_default(RS2_STREAM_DEPTH);
REQUIRE(depth);
// ... here the actual filters are being applied
auto filtered_depth = ppf.process(depth);
// Compare the resulted frame versus input
validate_ppf_results(depth, filtered_depth, test_cfg, i);
}
depth_sensor.stop();
depth_sensor.close();
}
}
}
TEST_CASE("Post-Processing Filters metadata validation", "[software-device][post-processing-filters]")
{
rs2::context ctx = make_context( SECTION_FROM_TEST_NAME );
if( ctx )
{
ppf_test_config test_cfg;
for (auto& ppf_test : ppf_test_cases)
{
CAPTURE(ppf_test.first);
CAPTURE(ppf_test.second);
WARN("PPF test " << ppf_test.first << "[" << ppf_test.second << "]");
// Load the data from configuration and raw frame files
if (!load_test_configuration(ppf_test.first, test_cfg))
continue;
post_processing_filters ppf;
// Apply the retrieved configuration onto a local post-processing chain of filters
REQUIRE_NOTHROW(ppf.configure(test_cfg));
rs2::software_device dev; // Create software-only device
auto depth_sensor = dev.add_sensor("Depth");
int width = test_cfg.input_res_x;
int height = test_cfg.input_res_y;
int depth_bpp = 2; //16bit unsigned
int frame_number = 1;
rs2_intrinsics depth_intrinsics = { width, height,
width / 2.f, height / 2.f, // Principal point (N/A in this test)
test_cfg.focal_length ,test_cfg.focal_length, // Focal Length
RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } };
auto depth_stream_profile = depth_sensor.add_video_stream({ RS2_STREAM_DEPTH, 0, 0, width, height, 30, depth_bpp, RS2_FORMAT_Z16, depth_intrinsics });
// Establish the required chain of filters
dev.create_matcher(RS2_MATCHER_DLR_C);
rs2::syncer sync;
depth_sensor.open(depth_stream_profile);
depth_sensor.start(sync);
size_t frames = (test_cfg.frames_sequence_size > 1) ? test_cfg.frames_sequence_size : 1;
for (auto i = 0; i < frames; i++)
{
//set next frames metadata
for (auto i = 0; i < rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT; i++)
depth_sensor.set_metadata((rs2_frame_metadata_value)i, rand());
// Inject input frame
depth_sensor.on_video_frame({ test_cfg._input_frames[i].data(), // Frame pixels from capture API
[](void*) {}, // Custom deleter (if required)
(int)test_cfg.input_res_x *depth_bpp, // Stride
depth_bpp, // Bytes-per-pixels
(rs2_time_t)frame_number + i, // Timestamp
RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME, // Clock Domain
frame_number, // Frame# for potential sync services
depth_stream_profile }); // Depth stream profile
rs2::frameset fset = sync.wait_for_frames();
REQUIRE(fset);
rs2::frame depth = fset.first_or_default(RS2_STREAM_DEPTH);
REQUIRE(depth);
// ... here the actual filters are being applied
auto filtered_depth = ppf.process(depth);
// Compare the resulted frame metadata versus input
compare_frame_md(depth, filtered_depth);
}
depth_sensor.stop();
depth_sensor.close();
}
}
}
bool is_subset(rs2::frameset full, rs2::frameset sub)
{
if (!sub.is<rs2::frameset>())
return false;
if (full.size() == 0 && sub.size() == 0)
return false;
for (auto f : full)
{
if (!sub.first(f.get_profile().stream_type(), f.get_profile().format()))
return false;
}
return true;
}
bool is_equal(rs2::frameset org, rs2::frameset processed)
{
if (!org.is<rs2::frameset>() || !processed.is<rs2::frameset>())
return false;
if (org.size() != processed.size() || org.size() == 0)
return false;
for (auto o : org)
{
auto curr_profile = o.get_profile();
bool found = false;
processed.foreach_rs([&curr_profile, &found](const rs2::frame& f)
{
auto processed_profile = f.get_profile();
if (curr_profile.unique_id() == processed_profile.unique_id())
found = true;
});
if(!found)
return false;
}
return true;
}
TEST_CASE("Post-Processing expected output", "[post-processing-filters]")
{
rs2::context ctx = make_context( SECTION_FROM_TEST_NAME );
if (!ctx)
return;
rs2::temporal_filter temporal;
rs2::hole_filling_filter hole_filling;
rs2::spatial_filter spatial;
rs2::decimation_filter decimation(4);
rs2::align aligner(RS2_STREAM_COLOR);
rs2::colorizer depth_colorizer;
rs2::disparity_transform to_disp;
rs2::disparity_transform from_disp(false);
rs2::config cfg;
cfg.enable_all_streams();
rs2::pipeline pipe(ctx);
auto profile = pipe.start(cfg);
bool supports_disparity = false;
for (auto s : profile.get_device().query_sensors())
{
if (s.supports(RS2_OPTION_STEREO_BASELINE))
{
supports_disparity = true;
break;
}
}
rs2::frameset original = pipe.wait_for_frames();
//set to set
rs2::frameset temp_processed_set = original.apply_filter(temporal);
REQUIRE(is_subset(original, temp_processed_set));
REQUIRE(is_subset(temp_processed_set, original));
rs2::frameset hole_processed_set = original.apply_filter(hole_filling);
REQUIRE(is_subset(original, hole_processed_set));
REQUIRE(is_subset(hole_processed_set, original));
rs2::frameset spatial_processed_set = original.apply_filter(spatial);
REQUIRE(is_subset(original, spatial_processed_set));
REQUIRE(is_subset(spatial_processed_set, original));
rs2::frameset decimation_processed_set = original.apply_filter(decimation);
REQUIRE(is_subset(original, decimation_processed_set));
REQUIRE(is_subset(decimation_processed_set, original));
rs2::frameset align_processed_set = original.apply_filter(aligner);
REQUIRE(is_subset(original, align_processed_set));
REQUIRE(is_subset(align_processed_set, original));
rs2::frameset colorizer_processed_set = original.apply_filter(depth_colorizer);
REQUIRE(is_subset(original, colorizer_processed_set));
REQUIRE_THROWS(is_subset(colorizer_processed_set, original));
rs2::frameset to_disp_processed_set = original.apply_filter(to_disp);
if(supports_disparity)
REQUIRE_THROWS(is_subset(to_disp_processed_set, original));
rs2::frameset from_disp_processed_set = original.apply_filter(from_disp);//should bypass
REQUIRE(is_equal(original, from_disp_processed_set));
//single to single
rs2::video_frame org_depth = original.get_depth_frame();
rs2::video_frame temp_processed_frame = org_depth.apply_filter(temporal);
REQUIRE_FALSE(temp_processed_frame.is<rs2::frameset>());
REQUIRE(temp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(temp_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
REQUIRE(org_depth.get_width() == temp_processed_frame.get_width());
rs2::video_frame hole_processed_frame = org_depth.apply_filter(hole_filling);
REQUIRE_FALSE(hole_processed_frame.is<rs2::frameset>());
REQUIRE(hole_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(hole_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
REQUIRE(org_depth.get_width() == hole_processed_frame.get_width());
rs2::video_frame spatial_processed_frame = org_depth.apply_filter(spatial);
REQUIRE_FALSE(spatial_processed_frame.is<rs2::frameset>());
REQUIRE(spatial_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(spatial_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
REQUIRE(org_depth.get_width() == spatial_processed_frame.get_width());
rs2::video_frame decimation_processed_frame = org_depth.apply_filter(decimation);
REQUIRE_FALSE(decimation_processed_frame.is<rs2::frameset>());
REQUIRE(decimation_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(decimation_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
REQUIRE(org_depth.get_width() > decimation_processed_frame.get_width());
rs2::video_frame colorizer_processed_frame = org_depth.apply_filter(depth_colorizer);
REQUIRE_FALSE(colorizer_processed_frame.is<rs2::frameset>());
REQUIRE(colorizer_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(colorizer_processed_frame.get_profile().format() == RS2_FORMAT_RGB8);
REQUIRE(org_depth.get_width() == colorizer_processed_frame.get_width());
rs2::video_frame to_disp_processed_frame = org_depth.apply_filter(to_disp);
REQUIRE_FALSE(to_disp_processed_frame.is<rs2::frameset>());
REQUIRE(to_disp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
bool is_disp = to_disp_processed_frame.get_profile().format() == RS2_FORMAT_DISPARITY16 ||
to_disp_processed_frame.get_profile().format() == RS2_FORMAT_DISPARITY32;
if (supports_disparity)
{
REQUIRE(is_disp);
REQUIRE(org_depth.get_width() == to_disp_processed_frame.get_width());
}
rs2::video_frame from_disp_processed_frame = org_depth.apply_filter(from_disp);//should bypass
REQUIRE_FALSE(from_disp_processed_frame.is<rs2::frameset>());
REQUIRE(from_disp_processed_frame.get_profile().stream_type() == RS2_STREAM_DEPTH);
REQUIRE(from_disp_processed_frame.get_profile().format() == RS2_FORMAT_Z16);
REQUIRE(org_depth.get_width() == from_disp_processed_frame.get_width());
pipe.stop();
}
TEST_CASE("Post-Processing processing pipe", "[post-processing-filters]")
{
rs2::context ctx = make_context( SECTION_FROM_TEST_NAME );
if( ! ctx )
return;
rs2::temporal_filter temporal;
rs2::hole_filling_filter hole_filling;
rs2::spatial_filter spatial;
rs2::decimation_filter decimation(4);
rs2::align aligner(RS2_STREAM_COLOR);
rs2::colorizer depth_colorizer;
rs2::disparity_transform to_disp;
rs2::disparity_transform from_disp(false);
rs2::pointcloud pc(RS2_STREAM_DEPTH);
rs2::config cfg;
cfg.enable_all_streams();
rs2::pipeline pipe(ctx);
auto profile = pipe.start(cfg);
bool supports_disparity = false;
for (auto s : profile.get_device().query_sensors())
{
if (s.supports(RS2_OPTION_STEREO_BASELINE))
{
supports_disparity = true;
break;
}
}
rs2::frameset original = pipe.wait_for_frames();
rs2::frameset full_pipe;
int run_for = 10;
std::set<int> uids;
size_t uid_count = 0;
while (run_for--)
{
full_pipe = pipe.wait_for_frames();
full_pipe = full_pipe.apply_filter(decimation);
full_pipe = full_pipe.apply_filter(to_disp);
full_pipe = full_pipe.apply_filter(spatial);
full_pipe = full_pipe.apply_filter(temporal);
full_pipe = full_pipe.apply_filter(from_disp);
full_pipe = full_pipe.apply_filter(aligner);
full_pipe = full_pipe.apply_filter(hole_filling);
full_pipe = full_pipe.apply_filter(depth_colorizer);
full_pipe = full_pipe.apply_filter(pc);
//printf("test frame:\n");
full_pipe.foreach_rs([&](const rs2::frame& f) {
uids.insert(f.get_profile().unique_id());
//printf("stream: %s, format: %d, uid: %d\n", f.get_profile().stream_name().c_str(), f.get_profile().format(), f.get_profile().unique_id());
});
if (uid_count == 0)
uid_count = uids.size();
REQUIRE(uid_count == uids.size());
}
REQUIRE(is_subset(original, full_pipe));
REQUIRE_THROWS(is_subset(full_pipe, original));
pipe.stop();
}
TEST_CASE("Align Processing Block", "[live][pipeline][post-processing-filters][!mayfail]") {
rs2::context ctx = make_context( SECTION_FROM_TEST_NAME );
if( ctx )
{
auto list = ctx.query_devices();
REQUIRE(list.size());
rs2::device dev;
rs2::pipeline pipe(ctx);
rs2::config cfg;
rs2::pipeline_profile pipe_profile;
REQUIRE_NOTHROW(cfg.enable_all_streams());
REQUIRE_NOTHROW(pipe_profile = cfg.resolve(pipe));
REQUIRE(pipe_profile);
REQUIRE_NOTHROW(dev = pipe_profile.get_device());
REQUIRE(dev);
disable_sensitive_options_for(dev);
dev_type PID = get_PID(dev);
CAPTURE(PID.first);
CAPTURE(PID.second);
REQUIRE_NOTHROW(pipe_profile = pipe.start(cfg));
REQUIRE(pipe_profile);
std::vector<rs2::stream_profile> active_streams;
REQUIRE_NOTHROW(active_streams = pipe_profile.get_streams());
// Make sure that there is a frame for each stream opened
rs2::frameset fs;
for (auto i = 0; i < 300; i++)
{
if ((pipe.try_wait_for_frames(&fs,500)) && (fs.size() == active_streams.size()))
{
pipe.stop();
break;
}
}
// Sanity check and registration of all possible source and target streams for alignment process
std::set<rs2_stream> streams_under_test;
for (const auto &str_type : { RS2_STREAM_COLOR, RS2_STREAM_INFRARED, RS2_STREAM_FISHEYE, RS2_STREAM_CONFIDENCE })
{
// Currently there is no API to explicitely select target in presense of multiple candidates (IR2)
if (auto fr = fs.first_or_default(str_type))
streams_under_test.insert(str_type);
}
// Sanity check
if (!fs.get_depth_frame() || !streams_under_test.size())
{
WARN("Align block test requires a device with Depth and Video sensor(s): current device "
<< "[" << PID.first << ":" << PID.second << "]. Test skipped");
return;
}
// Check depth->{uvc} alignment.
// Note that the test is for verification purposes and does not indicate quality of the mapping process
WARN("Testing Depth aligned to 2D video stream");
for (auto& tgt_stream : streams_under_test)
{
WARN("Testing Depth aligned to " << rs2_stream_to_string(tgt_stream));
rs2::align align_pb(tgt_stream);
auto aligned_fs = align_pb.process(fs);
auto origin_depth_frame = fs.get_depth_frame();
auto aligned_depth_frame = aligned_fs.get_depth_frame(); // Depth frame is replaced by the aligned depth
auto reference_frame = aligned_fs.first(tgt_stream);
auto orig_dpt_profile = origin_depth_frame.get_profile();
auto aligned_dpt_profile = aligned_depth_frame.get_profile();
auto aligned_dpth_video_pf = aligned_dpt_profile.as<rs2::video_stream_profile>();
auto ref_video_profile = reference_frame.get_profile().as<rs2::video_stream_profile>();
// Test: the depth frame retains the core attributes of depth stream
// Stream type/format/fps, (Depth units currently not available).
// TODO solution for Baseline ???
REQUIRE(orig_dpt_profile == aligned_dpt_profile);
REQUIRE(orig_dpt_profile.unique_id() != aligned_dpt_profile.unique_id());
// Test: the resulted depth frame properties correspond to the target
// Resolution, Intrinsic, Extrinsic
REQUIRE(aligned_dpth_video_pf.width() == ref_video_profile.width());
REQUIRE(aligned_dpth_video_pf.height() == ref_video_profile.height());
const auto ref_intr = ref_video_profile.get_intrinsics();
const auto align_dpt_intr = aligned_dpth_video_pf.get_intrinsics();
for (auto i = 0; i < 5; i++)
{
REQUIRE(ref_intr.coeffs[i] == approx(align_dpt_intr.coeffs[i]));
}
REQUIRE(ref_intr.fx == approx(align_dpt_intr.fx));
REQUIRE(ref_intr.fy == approx(align_dpt_intr.fy));
REQUIRE(ref_intr.ppx == approx(align_dpt_intr.ppx));
REQUIRE(ref_intr.ppy == approx(align_dpt_intr.ppy));
REQUIRE(ref_intr.model == align_dpt_intr.model);
REQUIRE(ref_intr.width == align_dpt_intr.width);
REQUIRE(ref_intr.height == align_dpt_intr.height);
// Extrinsic tests: Aligned_depth_extrinsic == Target frame extrinsic
rs2_extrinsics actual_extrinsics = ref_video_profile.get_extrinsics_to(aligned_dpt_profile);
rs2_extrinsics expected_extrinsics = { {1,0,0, 0,1,0, 0,0,1}, {0,0,0} };
CAPTURE(actual_extrinsics.rotation);
CAPTURE(actual_extrinsics.translation);
for (auto i = 0; i < 9; i++)
{
REQUIRE(actual_extrinsics.rotation[i] == approx(expected_extrinsics.rotation[i]));
}
for (auto i = 0; i < 3; i++)
{
REQUIRE(actual_extrinsics.translation[i] == approx(expected_extrinsics.translation[i]));
}
}
WARN("Testing 2D Video stream aligned to Depth sensor");
// Check {uvc}->depth alignment.
for (auto& tgt_stream : streams_under_test)
{
WARN("Testing " << rs2_stream_to_string(tgt_stream) << " aligned to Depth");
rs2::align align_pb(RS2_STREAM_DEPTH);
auto aligned_fs = align_pb.process(fs);
auto origin_2D_frame = fs.first(tgt_stream);
auto aligned_2D_frame = aligned_fs.first(tgt_stream);
auto depth_frame = aligned_fs.get_depth_frame();
auto orig_2D_profile = origin_2D_frame.get_profile();
auto aligned_2D_profile = aligned_2D_frame.get_profile().as<rs2::video_stream_profile>();
auto ref_video_profile = depth_frame.get_profile().as<rs2::video_stream_profile>();
// Test: the 2D frame retains the core attributes of the original stream
// Stream type/format/fps
REQUIRE(orig_2D_profile == aligned_2D_profile);
REQUIRE(orig_2D_profile.unique_id() != aligned_2D_profile.unique_id());
// Test: the resulted 2D frame properties correspond to the target depth
// Resolution, Intrinsic
REQUIRE(aligned_2D_profile.width() == ref_video_profile.width());
REQUIRE(aligned_2D_profile.height() == ref_video_profile.height());
const auto ref_intr = ref_video_profile.get_intrinsics();
const auto align_2D_intr = aligned_2D_profile.get_intrinsics();
for (auto i = 0; i < 5; i++)
{
REQUIRE(ref_intr.coeffs[i] == approx(align_2D_intr.coeffs[i]));
}
REQUIRE(ref_intr.fx == approx(align_2D_intr.fx));
REQUIRE(ref_intr.fy == approx(align_2D_intr.fy));
REQUIRE(ref_intr.ppx == approx(align_2D_intr.ppx));
REQUIRE(ref_intr.ppy == approx(align_2D_intr.ppy));
REQUIRE(ref_intr.model == align_2D_intr.model);
REQUIRE(ref_intr.width == align_2D_intr.width);
REQUIRE(ref_intr.height == align_2D_intr.height);
// Extrinsic tests: Aligned_depth_extrinsic == Target frame extrinsic
rs2_extrinsics actual_extrinsics = aligned_2D_profile.get_extrinsics_to(ref_video_profile);
rs2_extrinsics expected_extrinsics = { {1,0,0, 0,1,0, 0,0,1}, {0,0,0} };
CAPTURE(actual_extrinsics.rotation);
CAPTURE(actual_extrinsics.translation);
for (auto i = 0; i < 9; i++)
{
REQUIRE(actual_extrinsics.rotation[i] == approx(expected_extrinsics.rotation[i]));
}
for (auto i = 0; i < 3; i++)
{
REQUIRE(actual_extrinsics.translation[i] == approx(expected_extrinsics.translation[i]));
}
}
}
}