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
738 lines
31 KiB
2 months ago
|
// 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]));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|