From a5237879beee90b732b312bd2ca05657eefd59c6 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 21 Jul 2020 10:57:49 +0300 Subject: [PATCH] debug message massage plus a small fix in valid-scene with movement --- .../depth-to-rgb-calibration/optimizer.cpp | 4 +- src/algo/depth-to-rgb-calibration/optimizer.h | 2 +- .../depth-to-rgb-calibration/valid-scene.cpp | 106 +++++++++++------- src/depth-to-rgb-calibration.cpp | 2 +- src/l500/l500-color.cpp | 26 ++--- 5 files changed, 80 insertions(+), 60 deletions(-) diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index 6115173e33..a23d4bc8a5 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/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 optimizer::get_logic_edges( std::vector edges ) +std::vector< uint8_t > optimizer::get_logic_edges( std::vector< double > const & edges ) { std::vector 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++ ) { diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index a2738f4708..060cb947dc 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -378,7 +378,7 @@ namespace depth_to_rgb_calibration { std::vector blur_edges( std::vector 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 get_logic_edges( std::vector edges ); + std::vector get_logic_edges( std::vector const & edges ); std::vector 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, diff --git a/src/algo/depth-to-rgb-calibration/valid-scene.cpp b/src/algo/depth-to-rgb-calibration/valid-scene.cpp index 9e9c20a3ad..06e199582b 100644 --- a/src/algo/depth-to-rgb-calibration/valid-scene.cpp +++ b/src/algo/depth-to-rgb-calibration/valid-scene.cpp @@ -505,22 +505,27 @@ uint8_t dilation_calc(std::vector const& sub_image, std::vector cons return res; } -std::vector optimizer::images_dilation(std::vector 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 dilation_mask = { 1, 1, 1, - 1, 1, 1, - 1, 1, 1 }; - - return dilation_convolution(logic_edges, width, height, _params.dilation_size, _params.dilation_size, [&](std::vector 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 double gaussian_calc(std::vector const& sub_image, std::vector const& mask) { @@ -534,11 +539,12 @@ double gaussian_calc(std::vector const& sub_image, std::vector const& return res; } -void optimizer::gaussian_filter(std::vector const& lum_frame, - std::vector const& prev_lum_frame, - std::vector& yuy_diff, - std::vector& 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; @@ -546,12 +552,14 @@ void optimizer::gaussian_filter(std::vector const& lum_frame, /* 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 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(); @@ -559,9 +567,15 @@ diffIm = imgaussfilt(im1-im2,params.moveGaussSigma);*/ { yuy_diff.push_back((double)(*yuy_prev_iter) - (double)(*yuy_iter)); // used for testing only } - gaussian_filtered_image = gauss_convolution(yuy_diff, width,height, _params.gause_kernel_size, _params.gause_kernel_size, [&](std::vector 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& 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 }, diff --git a/src/depth-to-rgb-calibration.cpp b/src/depth-to-rgb-calibration.cpp index 294971fe3e..e7f3bc8028 100644 --- a/src/depth-to-rgb-calibration.cpp +++ b/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 ) ) { diff --git a/src/l500/l500-color.cpp b/src/l500/l500-color.cpp index 24372b12b0..15d5205792 100644 --- a/src/l500/l500-color.cpp +++ b/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 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 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" ); } }