debug message massage plus a small fix in valid-scene with movement

src/algo/depth-to-rgb-calibration/optimizer.cpp

@@ -1053,11 +1053,11 @@ std::vector< byte > optimizer::get_luminance_from_yuy2( std::vector< yuy_t > con
     return res;
 }
 
-std::vector<uint8_t> optimizer::get_logic_edges( std::vector<double> edges )
+std::vector< uint8_t > optimizer::get_logic_edges( std::vector< double > const & edges )
 {
     std::vector<uint8_t> logic_edges( edges.size(), 0 );
     auto max = std::max_element( edges.begin(), edges.end() );
-    auto thresh = *max*_params.edge_thresh4_logic_lum;
+    auto thresh = *max * _params.edge_thresh4_logic_lum;
 
     for( auto i = 0; i < edges.size(); i++ )
     {

src/algo/depth-to-rgb-calibration/optimizer.h

@@ -378,7 +378,7 @@ namespace depth_to_rgb_calibration {
         std::vector<double> blur_edges( std::vector<double> const & edges, size_t image_width, size_t image_height );
         std::vector< byte > get_luminance_from_yuy2( std::vector< yuy_t > const & yuy2_imagh );
 
-        std::vector<uint8_t> get_logic_edges( std::vector<double> edges );
+        std::vector<uint8_t> get_logic_edges( std::vector<double> const & edges );
         std::vector <double3> subedges2vertices(z_frame_data& z_data, const rs2_intrinsics_double& intrin, double depth_units);
 
         optimization_params back_tracking_line_search( optimization_params const & opt_params,

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

@@ -505,22 +505,27 @@ uint8_t dilation_calc(std::vector<T> const& sub_image, std::vector<uint8_t> cons
 
     return res;
 }
-std::vector<uint8_t> optimizer::images_dilation(std::vector<uint8_t> const &logic_edges, size_t width, size_t height)
+
+
+std::vector< uint8_t > optimizer::images_dilation( std::vector< uint8_t > const & logic_edges,
+                                                   size_t width,
+                                                   size_t height )
 {
-    if (_params.dilation_size == 1)
+    if( _params.dilation_size == 1 )
        return logic_edges;
-    else
-    {
-        std::vector<uint8_t> dilation_mask = { 1, 1, 1,
-                                                  1,  1,  1,
-                                                  1,  1,  1 };
-
-        return dilation_convolution<uint8_t>(logic_edges, width, height, _params.dilation_size, _params.dilation_size, [&](std::vector<uint8_t> const& sub_image)
-        {return dilation_calc(sub_image, dilation_mask); });
-    }
-
 
+    std::vector< uint8_t > dilation_mask = { 1, 1, 1, 1, 1, 1, 1, 1, 1 };
+    return dilation_convolution< uint8_t >( logic_edges,
+                                            width,
+                                            height,
+                                            _params.dilation_size,
+                                            _params.dilation_size,
+                                            [&]( std::vector< uint8_t > const & sub_image ) {
+                                                return dilation_calc( sub_image, dilation_mask );
+                                            } );
 }
+
+
 template<class T>
 double gaussian_calc(std::vector<T> const& sub_image, std::vector<double> const& mask)
 {
@@ -534,34 +539,43 @@ double gaussian_calc(std::vector<T> const& sub_image, std::vector<double> const&
     return res;
 }
 
-void optimizer::gaussian_filter(std::vector<uint8_t> const& lum_frame,
-    std::vector<uint8_t> const& prev_lum_frame,
-    std::vector<double>& yuy_diff, 
-    std::vector<double>& gaussian_filtered_image, 
-    size_t width, size_t height)
+void optimizer::gaussian_filter( std::vector< uint8_t > const & lum_frame,
+                                 std::vector< uint8_t > const & prev_lum_frame,
+                                 std::vector< double > & yuy_diff,
+                                 std::vector< double > & gaussian_filtered_image,
+                                 size_t width,
+                                 size_t height )
 {
 
     auto area = height *width;
 
     /* diffIm = abs(im1-im2);
 diffIm = imgaussfilt(im1-im2,params.moveGaussSigma);*/
     // use this matlab function to get gauss kernel with sigma=1: disp17(fspecial('gaussian',5,1))
-    std::vector<double>  gaussian_kernel = { 0.0029690167439504968, 0.013306209891013651, 0.021938231279714643, 0.013306209891013651, 0.0029690167439504968,
-        0.013306209891013651, 0.059634295436180138, 0.098320331348845769, 0.059634295436180138, 0.013306209891013651,
-        0.021938231279714643, 0.098320331348845769, 0.16210282163712664, 0.098320331348845769, 0.021938231279714643,
-        0.013306209891013651, 0.059634295436180138, 0.098320331348845769, 0.059634295436180138, 0.013306209891013651,
-        0.0029690167439504968, 0.013306209891013651, 0.021938231279714643, 0.013306209891013651, 0.0029690167439504968
-    };
+    std::vector< double > gaussian_kernel
+        = { 0.0029690167439504968, 0.013306209891013651, 0.021938231279714643, 0.013306209891013651,
+            0.0029690167439504968, 0.013306209891013651, 0.059634295436180138, 0.098320331348845769,
+            0.059634295436180138,  0.013306209891013651, 0.021938231279714643, 0.098320331348845769,
+            0.16210282163712664,   0.098320331348845769, 0.021938231279714643, 0.013306209891013651,
+            0.059634295436180138,  0.098320331348845769, 0.059634295436180138, 0.013306209891013651,
+            0.0029690167439504968, 0.013306209891013651, 0.021938231279714643, 0.013306209891013651,
+            0.0029690167439504968 };
 
     auto yuy_iter = lum_frame.begin();
     auto yuy_prev_iter = prev_lum_frame.begin();
     for (auto i = 0; i < area; i++, yuy_iter++, yuy_prev_iter++)
     {
         yuy_diff.push_back((double)(*yuy_prev_iter) - (double)(*yuy_iter)); // used for testing only
     }
-    gaussian_filtered_image = gauss_convolution<double>(yuy_diff, width,height, _params.gause_kernel_size, _params.gause_kernel_size, [&](std::vector<double> const& sub_image)
-        {return gaussian_calc(sub_image, gaussian_kernel); });
-    return;
+    gaussian_filtered_image
+        = gauss_convolution< double >( yuy_diff,
+                                       width,
+                                       height,
+                                       _params.gause_kernel_size,
+                                       _params.gause_kernel_size,
+                                       [&]( std::vector< double > const & sub_image ) {
+                                           return gaussian_calc( sub_image, gaussian_kernel );
+                                       } );
 }
 void abs_values(std::vector< double >& vec_in)
 {
@@ -586,25 +600,32 @@ void gaussian_dilation_mask(std::vector< double >& gauss_diff, std::vector< uint
         }
     }
 }
-void move_suspected_mask(std::vector< uint8_t >& move_suspect, std::vector< double >& gauss_diff_masked, double movement_threshold)
+
+static size_t move_suspected_mask( std::vector< uint8_t > & move_suspect,
+                                   std::vector< double > & gauss_diff_masked,
+                                   double const movement_threshold )
 {
-    for (auto it = gauss_diff_masked.begin(); it != gauss_diff_masked.end(); ++it)
+    size_t n_movements = 0;
+    for( auto it = gauss_diff_masked.begin(); it != gauss_diff_masked.end(); ++it )
     {
-        if (*it > movement_threshold)
+        if( *it > movement_threshold )
         {
             move_suspect.push_back(1);
+            ++n_movements;
         }
         else
         {
             move_suspect.push_back(0);
         }
     }
+    return n_movements;
 }
-bool optimizer::is_movement_in_images(
-    movement_inputs_for_frame const& prev,
-    movement_inputs_for_frame const& curr,
-    movement_result_data& result_data,
-    size_t width, size_t height)
+
+bool optimizer::is_movement_in_images( movement_inputs_for_frame const & prev,
+                                       movement_inputs_for_frame const & curr,
+                                       movement_result_data & result_data,
+                                       size_t width,
+                                       size_t height )
 {
     /*function [isMovement,movingPixels] = isMovementInImages(im1,im2, params)
 isMovement = false;
@@ -633,13 +654,10 @@ end*/
     result_data.gaussian_diff_masked = result_data.gaussian_filtered_image;
     abs_values(result_data.gaussian_diff_masked);
     gaussian_dilation_mask(result_data.gaussian_diff_masked, result_data.dilated_image);
-    move_suspected_mask(result_data.move_suspect, result_data.gaussian_diff_masked, _params.move_thresh_pix_val);
-    auto sum_move_suspect = 0;
-    for (auto it = result_data.move_suspect.begin(); it != result_data.move_suspect.end(); ++it)
-    {
-        sum_move_suspect += *it;
-    }
-    if (sum_move_suspect > _params.move_threshold_pix_num)
+    auto sum_move_suspect = move_suspected_mask( result_data.move_suspect,
+                                                 result_data.gaussian_diff_masked,
+                                                 _params.move_thresh_pix_val );
+    if( sum_move_suspect > _params.move_threshold_pix_num )
     {
         AC_LOG( DEBUG,
                 "    found movement: " << sum_move_suspect << " pixels above threshold; allowed: "
@@ -785,7 +803,9 @@ bool check_edges_dir_spread(const std::vector<double>& directions,
 
     if (!edges_dir_spread)
     {
-        AC_LOG( ERROR, "Scene is not valid: not enough edge direction spread" );
+        AC_LOG( ERROR,
+                "Scene is not valid: not enough edge direction spread (have "
+                    << valid_directions_sum << "; need " << p.minimal_full_directions << ")" );
         return edges_dir_spread;
     }
 
@@ -892,7 +912,7 @@ bool optimizer::input_validity_checks(input_validity_data* data )
 
     auto is_movement_from_last_success = true;
 
-    if (!_settings.is_manual_trigger && !_yuy.last_successful_frame.empty())
+    if( ! _settings.is_manual_trigger )
     {
         is_movement_from_last_success
             = is_movement_in_images( { _yuy.last_successful_edges, _yuy.last_successful_lum_frame },

src/depth-to-rgb-calibration.cpp

@@ -123,7 +123,7 @@ rs2_calibration_status depth_to_rgb_calibration::optimize(
         AC_LOG( DEBUG, "Checking scene validity" );
         if( !_algo.is_scene_valid() )
         {
-            AC_LOG( ERROR, "Calibration scene was found invalid!" );
+            //AC_LOG( ERROR, "Calibration scene was found invalid!" );
             call_back( RS2_CALIBRATION_SCENE_INVALID );
             if( !getenv( DISABLE_RS2_CALIBRATION_CHECKS ) )
             {

src/l500/l500-color.cpp

@@ -420,16 +420,16 @@ namespace librealsense
 
     void l500_color_sensor::start_stream_for_calibration(const stream_profiles& requests)
     {
-        std::lock_guard<std::mutex> lock(_state_mutex);
+        std::lock_guard< std::mutex > lock( _state_mutex );
 
         // Allow calibration process to open the color stream only if it is not started by the user.
-        if (_state == sensor_state::CLOSED)
+        if( _state == sensor_state::CLOSED )
         {
             synthetic_sensor::open(requests);
             set_sensor_state(sensor_state::OWNED_BY_AUTO_CAL);
-            AC_LOG(INFO, "Start color sensor stream for calibration");
-            delayed_start(make_frame_callback([&](frame_holder fref) {}));
-            AC_LOG(INFO, "Color sensor stream started");
+            AC_LOG( INFO, "Start color sensor stream for calibration" );
+            delayed_start( make_frame_callback( [&]( frame_holder fref ) {} ) );
+            AC_LOG( INFO, "Color sensor stream started" );
         }
         else
         {
@@ -448,30 +448,30 @@ namespace librealsense
 
     void l500_color_sensor::stop_stream_for_calibration()
     {
-        std::lock_guard<std::mutex> lock(_state_mutex);
+        std::lock_guard< std::mutex > lock( _state_mutex );
 
-        if (_state == sensor_state::OWNED_BY_AUTO_CAL)
+        if( _state == sensor_state::OWNED_BY_AUTO_CAL )
         {
             if( is_streaming() )
             {
-                AC_LOG(INFO, "Stopping color sensor stream from calibration");
+                AC_LOG( INFO, "Stopping color sensor stream from calibration" );
                 delayed_stop();
-                AC_LOG(INFO, "Color sensor stream stopped");
+                AC_LOG( INFO, "Color sensor stream stopped" );
 
             }
             if (is_opened())
             {
-                LOG_DEBUG("Closing color sensor...");
+                LOG_DEBUG( "Closing color sensor..." );
                 synthetic_sensor::close();
-                LOG_DEBUG("Color sensor closed");
+                LOG_DEBUG( "Color sensor closed" );
             }
 
             // If we got here with no exception it means the start has succeeded.
-            set_sensor_state(sensor_state::CLOSED);
+            set_sensor_state( sensor_state::CLOSED );
         }
         else
         {
-            AC_LOG(DEBUG, "Color sensor was opened before calibration stated, no need to close");
+            AC_LOG( DEBUG, "Color sensor was not opened by us; no need to close" );
         }
     }