PR IntelRealSense#30 from Avishag: WW29.3 Enhancements to depth prepr…

…ocessing and input validity check

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<double> sum_gradient_depth(std::vector<double> &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<double> 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<double>::max();
+                grad_ir_high_th = 3.5;
+                grad_z_low_th = 0;
+                grad_z_high_th = std::numeric_limits<double>::max();
+            }
+            else if (XGA)
+            {
+                grad_ir_low_th = std::numeric_limits<double>::max();
+                grad_ir_high_th = 2.5;
+                grad_z_low_th = 0;
+                grad_z_high_th = std::numeric_limits<double>::max();
+            }
+        }
+    }
 }
 
 void params::set_rgb_resolution( size_t width, size_t height )
@@ -1525,6 +1599,46 @@ void optimizer::set_cycle_data(const std::vector<double3>& 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,

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<double>::max();
+        double grad_ir_high_th = 2.5;
+        double grad_z_low_th = 0;
+        double grad_z_high_th = std::numeric_limits<double>::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,13 +273,15 @@ namespace depth_to_rgb_calibration {
     class optimizer
     {
     public:
+#pragma pack(push, 1)
         struct settings
         {
             bool is_manual_trigger = false;
             double hum_temp = 0.;
             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<double3>& new_vertices,

src/algo/depth-to-rgb-calibration/valid-scene.cpp

@@ -507,13 +507,19 @@ uint8_t dilation_calc(std::vector<T> const& sub_image, std::vector<uint8_t> cons
 }
 void optimizer::images_dilation(yuy2_frame_data& yuy)
 {
-    auto area = yuy.height * yuy.width;
-    std::vector<uint8_t> 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<uint8_t> dilation_mask = { 1, 1, 1,
+                                                  1,  1,  1,
+                                                  1,  1,  1 };
 
-    yuy.dilated_image = dilation_convolution<uint8_t>(yuy.prev_logic_edges, yuy.width, yuy.height, _params.dilation_size, _params.dilation_size, [&](std::vector<uint8_t> const& sub_image)
+        yuy.dilated_image = dilation_convolution<uint8_t>(yuy.prev_logic_edges, yuy.width, yuy.height, _params.dilation_size, _params.dilation_size, [&](std::vector<uint8_t> const& sub_image)
         {return dilation_calc(sub_image, dilation_mask); });
+    }
+
 
 }
 template<class T>
@@ -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<byte>& 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<int> num_pix_per_sec(n_sections, 0);
 
     for (auto&&i : section_map)
@@ -831,20 +836,31 @@ bool check_edges_spatial_spread(const std::vector<byte>& 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");

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<double> in = { 1,2,3,4 };