diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index d4bf8e9c84..da07fcc552 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -122,6 +122,14 @@ namespace optimizer::optimizer( settings const & s ) : _settings( s ) { + if (_settings.is_manual_trigger) + { + adjust_params_to_manual_mode(); + } + else + { + adjust_params_to_auto_mode(); + } } static std::vector< double > get_direction_deg( @@ -375,18 +383,35 @@ std::vector sum_gradient_depth(std::vector &gradient, std::vecto std::vector< byte > find_valid_depth_edges( std::vector< double > const & grad_in_direction, std::vector< byte > const & is_supressed, std::vector< double > const & values_for_subedges, - int const gradZTh ) + std::vector< double > const & ir_local_edges, + const params & p ) { std::vector< byte > res; res.reserve( grad_in_direction.size() ); //%validEdgePixels = zGradInDirection > params.gradZTh & isSupressed & zValuesForSubEdges > 0; - for (int i = 0; i < grad_in_direction.size(); i++) + if (p.use_enhanced_preprocessing) + { + for (int i = 0; i < grad_in_direction.size(); i++) + { + bool cond1 = (grad_in_direction[i] > p.grad_z_low_th && ir_local_edges[i * 4 + 2] > p.grad_ir_high_th) || + (grad_in_direction[i] > p.grad_z_high_th && ir_local_edges[i * 4 + 2] > p.grad_ir_low_th); + + bool cond2 = is_supressed[i]; + bool cond3 = values_for_subedges[i] > 0; + res.push_back(cond1 && cond2 && cond3); + } + } + else { - bool cond1 = grad_in_direction[i] > gradZTh; - bool cond2 = is_supressed[i]; - bool cond3 = values_for_subedges[i] > 0; - res.push_back( cond1 && cond2 && cond3 ); + for (int i = 0; i < grad_in_direction.size(); i++) + { + bool cond1 = grad_in_direction[i] > p.grad_z_threshold; + bool cond2 = is_supressed[i]; + bool cond3 = values_for_subedges[i] > 0; + res.push_back(cond1 && cond2 && cond3); + } } + return res; } @@ -419,7 +444,7 @@ void optimizer::set_z_data( std::vector< z_t > && depth_data, /*[zEdge,Zx,Zy] = OnlineCalibration.aux.edgeSobelXY(uint16(frame.z),2); % Added the second input - margin to zero out [iEdge,Ix,Iy] = OnlineCalibration.aux.edgeSobelXY(uint16(frame.i),2); % Added the second input - margin to zero out validEdgePixelsByIR = iEdge>params.gradITh; */ - _params.set_depth_resolution(depth_intrinsics.width, depth_intrinsics.height); + _params.set_depth_resolution(depth_intrinsics.width, depth_intrinsics.height, _settings.ambient); _z.width = depth_intrinsics.width; _z.height = depth_intrinsics.height; _z.orig_intrinsics = depth_intrinsics; @@ -442,8 +467,18 @@ void optimizer::set_z_data( std::vector< z_t > && depth_data, _z.edges = calc_intensity(_z.gradient_x, _z.gradient_y); _ir.edges = calc_intensity(_ir.gradient_x, _ir.gradient_y); - for( auto it = _ir.edges.begin(); it < _ir.edges.end(); it++ ) - _ir.valid_edge_pixels_by_ir.push_back( *it > _params.grad_ir_threshold ); + for (auto ir = _ir.edges.begin(), z = _z.edges.begin(); ir < _ir.edges.end() && z < _z.edges.end(); ir++, z++) + { + auto valid_edge = true; + if (_params.use_enhanced_preprocessing) + { + _ir.valid_edge_pixels_by_ir.push_back((*ir > _params.grad_ir_high_th && *z > _params.grad_z_low_th) + || (*ir > _params.grad_ir_low_th && *z > _params.grad_z_high_th)); + } + else + _ir.valid_edge_pixels_by_ir.push_back(*ir > _params.grad_ir_threshold); + } + /*sz = size(frame.i); [gridX,gridY] = meshgrid(1:sz(2),1:sz(1)); % gridX/Y contains the indices of the pixels @@ -616,7 +651,8 @@ void optimizer::set_z_data( std::vector< z_t > && depth_data, _z.supressed_edges = find_valid_depth_edges( _z.grad_in_direction, _ir.is_supressed, _z.values_for_subedges, - _params.grad_z_threshold ); + _ir.local_edges, + _params); std::vector valid_values_for_subedges; @@ -1267,16 +1303,54 @@ svm_model_linear::svm_model_linear() svm_model_gaussian::svm_model_gaussian() { } -void params::set_depth_resolution( size_t width, size_t height ) +void params::set_depth_resolution( size_t width, size_t height, rs2_ambient_light ambient) { AC_LOG( DEBUG, " depth resolution= " << width << "x" << height ); // Some parameters are resolution-dependent bool const XGA = (width == 1024 && height == 768); + bool const VGA = (width == 640 && height == 480); if( XGA ) { AC_LOG( DEBUG, " changing IR threshold: " << grad_ir_threshold << " -> " << 2.5 << " (because of resolution)" ); grad_ir_threshold = 2.5; } + if (use_enhanced_preprocessing) + { + if (ambient == RS2_AMBIENT_LIGHT_NO_AMBIENT) + { + if (VGA) + { + grad_ir_low_th = 1.5; + grad_ir_high_th = 3.5; + grad_z_low_th = 0; + grad_z_high_th = 100; + } + else if (XGA) + { + grad_ir_low_th = 1; + grad_ir_high_th = 2.5; + grad_z_low_th = 0; + grad_z_high_th = 80; + } + } + else + { + if (VGA) + { + grad_ir_low_th = std::numeric_limits::max(); + grad_ir_high_th = 3.5; + grad_z_low_th = 0; + grad_z_high_th = std::numeric_limits::max(); + } + else if (XGA) + { + grad_ir_low_th = std::numeric_limits::max(); + grad_ir_high_th = 2.5; + grad_z_low_th = 0; + grad_z_high_th = std::numeric_limits::max(); + } + } + } } void params::set_rgb_resolution( size_t width, size_t height ) @@ -1525,6 +1599,46 @@ void optimizer::set_cycle_data(const std::vector& vertices, _dsm_params_cand_from_bin = dsm_params_cand; } +void optimizer::adjust_params_to_apd_gain() +{ + if(_settings.ambient == RS2_AMBIENT_LIGHT_NO_AMBIENT) // long preset + _params.saturation_value = 230; + else if(_settings.ambient == RS2_AMBIENT_LIGHT_LOW_AMBIENT) // short preset + _params.saturation_value = 250; + else + throw std::runtime_error( to_string() <<_settings.ambient <<" invalid ambient value"); +} + +void optimizer::adjust_params_to_manual_mode() +{ + _params.max_global_los_scaling_step = 0.005; + _params.pix_per_section_depth_th = 0; + _params.pix_per_section_rgb_th = 0; + _params.min_section_with_enough_edges = 0; + _params.edges_per_direction_ratio_th = 0; + _params.minimal_full_directions = 0; + + const static double newvals[N_BASIC_DIRECTIONS] = { 0, 0, 0, 0 }; + std::copy(std::begin(newvals), std::end(newvals), std::begin(_params.dir_std_th)); + _params.saturation_ratio_th = 1; + _params.saturation_value = 256; +} + +void optimizer::adjust_params_to_auto_mode() +{ + _params.max_global_los_scaling_step = 0.004; + _params.pix_per_section_depth_th = 0.01; + _params.pix_per_section_rgb_th = 0.01; + _params.min_section_with_enough_edges = 2; + _params.edges_per_direction_ratio_th = 0.004; + _params.minimal_full_directions = 2; + + const static double newvals[N_BASIC_DIRECTIONS] = { 0.09,0.09,0.09,0.09 }; + std::copy(std::begin(newvals), std::end(newvals), std::begin(_params.dir_std_th)); + _params.saturation_ratio_th = 0.05; + adjust_params_to_apd_gain(); +} + size_t optimizer::optimize_p ( const optimization_params& params_curr, diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index e73aa6f593..cd91aa637f 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -30,7 +30,7 @@ namespace depth_to_rgb_calibration { { params(); - void set_depth_resolution(size_t width, size_t height); + void set_depth_resolution(size_t width, size_t height, rs2_ambient_light ambient); void set_rgb_resolution(size_t width, size_t height); double gamma = 0.9; @@ -41,6 +41,12 @@ namespace depth_to_rgb_calibration { double grad_z_max = 1000; double edge_thresh4_logic_lum = 0.1; + // enhanced preprocessing params + double grad_ir_low_th = std::numeric_limits::max(); + double grad_ir_high_th = 2.5; + double grad_z_low_th = 0; + double grad_z_high_th = std::numeric_limits::max(); + double max_step_size = 1; double min_step_size = 0.00001; double control_param = 0.5; @@ -57,7 +63,7 @@ namespace depth_to_rgb_calibration { double edge_distribution_min_max_ratio = 0.005; double grad_dir_ratio = 10; double grad_dir_ratio_prep = 1.5; - size_t dilation_size = 3; + size_t dilation_size = 1; double gauss_sigma = 1; size_t gause_kernel_size = 5; double move_thresh_pix_val = 20; @@ -74,18 +80,20 @@ namespace depth_to_rgb_calibration { double const max_K2DSM_iters = 10; // TODO: the following should be 0.2% but was increased to 0.5% to account for // manual trigger activation - double const max_global_los_scaling_step = 0.005; // the difference (.5%) between starting and final scale - - double const edges_per_direction_ratio_th = 0.0041; - double const dir_std_th[N_BASIC_DIRECTIONS] = { 0.126, 0.126, 0.126, 0.126 }; - int const minimal_full_directions = 2; - bool const require_orthogonal_valid_dirs = false; - - int const saturation_value = 230; - double const saturation_ratio_th = 0.05; + double max_global_los_scaling_step = 0.004; // the difference (.5%) between starting and final scale - double const pix_per_section_th = 0.01; - int const min_section_with_enough_edges = 2; + // input validation + double edges_per_direction_ratio_th = 0.004; + double dir_std_th[N_BASIC_DIRECTIONS] = { 0.09, 0.09, 0.09, 0.09 }; + int minimal_full_directions = 2; + bool require_orthogonal_valid_dirs = false; + int saturation_value = 230; + double saturation_ratio_th = 0.05; + double pix_per_section_rgb_th = 0.01; + double pix_per_section_depth_th = 0.01; + int min_section_with_enough_edges = 2; + + bool use_enhanced_preprocessing = true; }; // svm struct decision_params @@ -265,6 +273,7 @@ namespace depth_to_rgb_calibration { class optimizer { public: +#pragma pack(push, 1) struct settings { bool is_manual_trigger = false; @@ -272,6 +281,7 @@ namespace depth_to_rgb_calibration { rs2_ambient_light ambient = RS2_AMBIENT_LIGHT_NO_AMBIENT; int receiver_gain = 0; // aka APD }; +#pragma pack(pop) optimizer( settings const & ); @@ -341,6 +351,10 @@ namespace depth_to_rgb_calibration { const p_matrix& p_mat_opt = p_matrix()); private: + void adjust_params_to_manual_mode(); + void adjust_params_to_auto_mode(); + void adjust_params_to_apd_gain(); + // 1 cycle of optimization size_t optimize_p(const optimization_params& params_curr, const std::vector& new_vertices, diff --git a/src/algo/depth-to-rgb-calibration/valid-scene.cpp b/src/algo/depth-to-rgb-calibration/valid-scene.cpp index 9c4de45598..aa87704bfe 100644 --- a/src/algo/depth-to-rgb-calibration/valid-scene.cpp +++ b/src/algo/depth-to-rgb-calibration/valid-scene.cpp @@ -507,13 +507,19 @@ uint8_t dilation_calc(std::vector const& sub_image, std::vector cons } void optimizer::images_dilation(yuy2_frame_data& yuy) { - auto area = yuy.height * yuy.width; - std::vector dilation_mask = { 1, 1, 1, - 1, 1, 1, - 1, 1, 1 }; + if (_params.dilation_size == 1) + yuy.dilated_image = yuy.prev_logic_edges; + else + { + auto area = yuy.height * yuy.width; + std::vector dilation_mask = { 1, 1, 1, + 1, 1, 1, + 1, 1, 1 }; - yuy.dilated_image = dilation_convolution(yuy.prev_logic_edges, yuy.width, yuy.height, _params.dilation_size, _params.dilation_size, [&](std::vector const& sub_image) + yuy.dilated_image = dilation_convolution(yuy.prev_logic_edges, yuy.width, yuy.height, _params.dilation_size, _params.dilation_size, [&](std::vector const& sub_image) {return dilation_calc(sub_image, dilation_mask); }); + } + } template @@ -681,8 +687,7 @@ bool optimizer::is_scene_valid(input_validity_data* data) //return((!res_movement) && res_edges && res_gradient); auto valid = !res_movement; - if( ! _settings.is_manual_trigger ) - valid = valid && input_validity_checks(data); + valid = valid && input_validity_checks(data); return(valid); } @@ -815,10 +820,10 @@ bool check_saturation(const std::vector< ir_t >& ir_frame, bool check_edges_spatial_spread(const std::vector& section_map, size_t width, size_t height, - const params& p) + double th, + size_t n_sections, + size_t min_section_with_enough_edges) { - const auto n_sections = p.num_of_sections_for_edge_distribution_x*p.num_of_sections_for_edge_distribution_y; - std::vector num_pix_per_sec(n_sections, 0); for (auto&&i : section_map) @@ -831,20 +836,31 @@ bool check_edges_spatial_spread(const std::vector& section_map, for (auto i = 0; i < n_sections; i++) { num_pix_per_sec_over_area[i] = (double)num_pix_per_sec[i] / (width*height)*n_sections; - num_sections_with_enough_edges[i] = num_pix_per_sec_over_area[i] > p.pix_per_section_th; + num_sections_with_enough_edges[i] = num_pix_per_sec_over_area[i] > th; } double sum = std::accumulate(num_sections_with_enough_edges.begin(), num_sections_with_enough_edges.end(), 0.0); - return sum >= p.min_section_with_enough_edges; + return sum >= min_section_with_enough_edges; } bool optimizer::input_validity_checks(input_validity_data* data ) { auto dir_spread = check_edges_dir_spread(_z.directions, _z.subpixels_x, _z.subpixels_y, _z.width, _z.height, _params); auto not_saturated = check_saturation(_ir.ir_frame, _ir.width, _ir.height, _params); - auto depth_spatial_spread = check_edges_spatial_spread(_z.section_map, _z.width, _z.height, _params); - auto rgb_spatial_spread = check_edges_spatial_spread(_yuy.section_map, _yuy.width, _yuy.height, _params); + + auto depth_spatial_spread = check_edges_spatial_spread( + _z.section_map, _z.width, _z.height, + _params.pix_per_section_depth_th, + _params.num_of_sections_for_edge_distribution_x*_params.num_of_sections_for_edge_distribution_y, + _params.min_section_with_enough_edges); + + auto rgb_spatial_spread = check_edges_spatial_spread( + _yuy.section_map, + _yuy.width, _yuy.height, + _params.pix_per_section_rgb_th, + _params.num_of_sections_for_edge_distribution_x*_params.num_of_sections_for_edge_distribution_y, + _params.min_section_with_enough_edges); if (!depth_spatial_spread) AC_LOG(ERROR, "Scene is not valid since there is not enough depth edge spread"); diff --git a/unit-tests/algo/d2rgb/compare-scene.h b/unit-tests/algo/d2rgb/compare-scene.h index d8e8e3d868..7bae50c26f 100644 --- a/unit-tests/algo/d2rgb/compare-scene.h +++ b/unit-tests/algo/d2rgb/compare-scene.h @@ -94,11 +94,7 @@ void compare_scene( std::string const & scene_dir, scene_stats * stats = nullptr scene_metadata md( scene_dir ); algo::optimizer::settings settings; - //read_data_from( bin_dir( scene_dir ) + filename, &settings ); - settings.is_manual_trigger = true; - settings.ambient = RS2_AMBIENT_LIGHT_NO_AMBIENT; - settings.receiver_gain = 9; - settings.hum_temp = 40; + read_data_from( bin_dir( scene_dir ) + "settings", &settings ); algo::optimizer cal( settings ); /*std::vector in = { 1,2,3,4 };