From 56592d3ea46a98945394cfa04851a2a406df8561 Mon Sep 17 00:00:00 2001 From: Eran Date: Sun, 31 May 2020 22:19:29 +0300 Subject: [PATCH 1/7] remove stdout message in pointcloud --- src/proc/pointcloud.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/proc/pointcloud.cpp b/src/proc/pointcloud.cpp index f57a65f1cf..bb6ea927dc 100644 --- a/src/proc/pointcloud.cpp +++ b/src/proc/pointcloud.cpp @@ -129,11 +129,8 @@ namespace librealsense assert( d2r ); try { - _registered_auto_calib_cb = std::shared_ptr( this, - []( pointcloud * p ) - { - std::cout << "~_registered_auto_calib_cb" << std::endl; - } ); + _registered_auto_calib_cb + = std::shared_ptr< pointcloud >( this, []( pointcloud * p ) {} ); std::weak_ptr< pointcloud > wr { _registered_auto_calib_cb }; auto fn = [=]( rs2_calibration_status status ) From ab483aa0a80d9045d613a827a27a7ab29723b899 Mon Sep 17 00:00:00 2001 From: Eran Date: Sun, 31 May 2020 22:20:02 +0300 Subject: [PATCH 2/7] fix memory leak in occlusion-filter --- src/proc/occlusion-filter.cpp | 49 +++++++++++++++++------------------ 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/src/proc/occlusion-filter.cpp b/src/proc/occlusion-filter.cpp index 05c5140287..265f84b97c 100644 --- a/src/proc/occlusion-filter.cpp +++ b/src/proc/occlusion-filter.cpp @@ -85,38 +85,37 @@ namespace librealsense if (_occlusion_scanning == horizontal) { + for( size_t y = 0; y < points_height; ++y ) { - for (size_t y = 0; y < points_height; ++y) - { - maxInLine = -1; - maxZ = 0; - int occDilationLeft = 0; + maxInLine = -1; + maxZ = 0; + int occDilationLeft = 0; - for (size_t x = 0; x < points_width; ++x) + for( size_t x = 0; x < points_width; ++x ) + { + if( points_ptr->z ) { - if (points_ptr->z) + // Occlusion detection + if( pixels_ptr->x < maxInLine + || ( pixels_ptr->x == maxInLine && ( points_ptr->z - maxZ ) > occZTh ) ) + { + *points_ptr = { 0, 0, 0 }; + occDilationLeft = occDilationSz; + } + else { - //Occlusion detection - if (pixels_ptr->x < maxInLine || (pixels_ptr->x == maxInLine && (points_ptr->z - maxZ) > occZTh)) + maxInLine = pixels_ptr->x; + maxZ = points_ptr->z; + if( occDilationLeft > 0 ) { *points_ptr = { 0, 0, 0 }; - occDilationLeft = occDilationSz; - } - else - { - maxInLine = pixels_ptr->x; - maxZ = points_ptr->z; - if (occDilationLeft > 0) - { - *points_ptr = { 0, 0, 0 }; - occDilationLeft--; - } + occDilationLeft--; } } - ++points_ptr; - ++uv_map_ptr; - ++pixels_ptr; } + ++points_ptr; + ++uv_map_ptr; + ++pixels_ptr; } } } @@ -125,9 +124,9 @@ namespace librealsense auto rotated_depth_width = _depth_intrinsics->height; auto rotated_depth_height = _depth_intrinsics->width; auto depth_ptr = (byte*)(depth.get_data()); - std::allocator alloc; + std::vector< byte > alloc( depth.get_bytes_per_pixel() * points_width * points_height ); byte* depth_planes[1]; - depth_planes[0] = (byte*)alloc.allocate(depth.get_bytes_per_pixel() * points_width * points_height); + depth_planes[0] = alloc.data(); rotate_image_optimized<2>(depth_planes, (const byte*)(depth.get_data()), points_width, points_height); From 0cdadbc0ec3b09884e9fcda1cb825f9291a91ad6 Mon Sep 17 00:00:00 2001 From: Eran Date: Sun, 31 May 2020 22:58:11 +0300 Subject: [PATCH 3/7] fix ac_logger so it's static and won't cause duplicate debug AC log messages --- src/l500/l500-private.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/l500/l500-private.cpp b/src/l500/l500-private.cpp index a395d8622b..65e5d1a6cb 100644 --- a/src/l500/l500-private.cpp +++ b/src/l500/l500-private.cpp @@ -281,6 +281,9 @@ namespace librealsense _f.open( filename ); if( _f ) std::cout << "-D- AC log is being written to: " << filename << std::endl; + + librealsense::log_to_callback( RS2_LOG_SEVERITY_ALL, + { this, []( rs2_log_callback * p ) {} } ); } void on_log( rs2_log_severity severity, rs2_log_message const & msg ) noexcept override @@ -300,9 +303,7 @@ namespace librealsense void release() override { delete this; } }; - librealsense::log_to_callback( RS2_LOG_SEVERITY_ALL, - { new ac_logger, []( rs2_log_callback * p ) { p->release(); } } - ); + static ac_logger one_logger; #endif } From 5d8a715e85bba1ec4c72381b75f4628c0febff44 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 1 Jun 2020 06:47:01 +0300 Subject: [PATCH 4/7] fix cmath include in svd_3x4.cpp --- src/algo/depth-to-rgb-calibration/svd_3x4.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/algo/depth-to-rgb-calibration/svd_3x4.cpp b/src/algo/depth-to-rgb-calibration/svd_3x4.cpp index 6f2c434a02..3babfd9a06 100644 --- a/src/algo/depth-to-rgb-calibration/svd_3x4.cpp +++ b/src/algo/depth-to-rgb-calibration/svd_3x4.cpp @@ -3,8 +3,9 @@ // Code generated by Matlab Coder -#include #include "utils.h" +#include +#include namespace From 79cd3496f41c3257bbbe0f2c8c57121c6381ef71 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 1 Jun 2020 06:47:52 +0300 Subject: [PATCH 5/7] add *.TMP to .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 097c8806a4..e857e226bf 100644 --- a/.gitignore +++ b/.gitignore @@ -30,6 +30,7 @@ DerivedData librealsense.xc/build *~ +*.TMP *.a *.o *.so From 2001fb9835ec924528159e1c5de8e1e657c25783 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 1 Jun 2020 11:12:52 +0300 Subject: [PATCH 6/7] fix more includes in svd_3x4.cpp and pinv_3x3.cpp --- src/algo/depth-to-rgb-calibration/pinv_3x3.cpp | 4 +++- src/algo/depth-to-rgb-calibration/svd_3x4.cpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/algo/depth-to-rgb-calibration/pinv_3x3.cpp b/src/algo/depth-to-rgb-calibration/pinv_3x3.cpp index 0798b6fb1a..810beff8a1 100644 --- a/src/algo/depth-to-rgb-calibration/pinv_3x3.cpp +++ b/src/algo/depth-to-rgb-calibration/pinv_3x3.cpp @@ -3,8 +3,10 @@ // Code generated by Matlab Coder -#include #include "utils.h" +#include +#include +#include namespace diff --git a/src/algo/depth-to-rgb-calibration/svd_3x4.cpp b/src/algo/depth-to-rgb-calibration/svd_3x4.cpp index 3babfd9a06..fca9423943 100644 --- a/src/algo/depth-to-rgb-calibration/svd_3x4.cpp +++ b/src/algo/depth-to-rgb-calibration/svd_3x4.cpp @@ -6,6 +6,7 @@ #include "utils.h" #include #include +#include namespace From 93463b1b2bdfd64458e1fe6b341a48cd037d1a9f Mon Sep 17 00:00:00 2001 From: aangerma Date: Sun, 31 May 2020 03:24:27 +0300 Subject: [PATCH 7/7] cycles of optimize ->k2dsm. --- .../depth-to-rgb-calibration/calibration.cpp | 6 +- .../depth-to-rgb-calibration/frame-data.h | 1 + .../depth-to-rgb-calibration/k-to-dsm.cpp | 173 ++++--- src/algo/depth-to-rgb-calibration/k-to-dsm.h | 92 +++- .../depth-to-rgb-calibration/optimizer.cpp | 178 +++++-- src/algo/depth-to-rgb-calibration/optimizer.h | 42 +- .../valid-results.cpp | 22 +- .../depth-to-rgb-calibration/compare-scene.h | 435 ++++++++++-------- .../depth-to-rgb-calibration/scene-data.h | 6 +- 9 files changed, 612 insertions(+), 343 deletions(-) diff --git a/src/algo/depth-to-rgb-calibration/calibration.cpp b/src/algo/depth-to-rgb-calibration/calibration.cpp index 44476f5a5f..f9f04d1acf 100644 --- a/src/algo/depth-to-rgb-calibration/calibration.cpp +++ b/src/algo/depth-to-rgb-calibration/calibration.cpp @@ -321,9 +321,9 @@ krt p_matrix::decompose() const std::vector inv_k_square_vac( 9, 0 ); inv( k_square.to_vector().data(), inv_k_square_vac.data() ); - double3x3 inv_k_square = { inv_k_square_vac[0], inv_k_square_vac[1], inv_k_square_vac[2], - inv_k_square_vac[3], inv_k_square_vac[4], inv_k_square_vac[5], - inv_k_square_vac[6], inv_k_square_vac[7], inv_k_square_vac[8] }; + double3x3 inv_k_square = { inv_k_square_vac[0], inv_k_square_vac[3], inv_k_square_vac[6], + inv_k_square_vac[1], inv_k_square_vac[4], inv_k_square_vac[7], + inv_k_square_vac[2], inv_k_square_vac[5], inv_k_square_vac[8] }; //%KInv = cholesky3x3(KSquareInv)';% Cholsky decomposition 3 by 3. returns a lower triangular matrix 3x3. Equal to inv(Krgb') auto k_inv = cholesky3x3( inv_k_square ).transpose(); diff --git a/src/algo/depth-to-rgb-calibration/frame-data.h b/src/algo/depth-to-rgb-calibration/frame-data.h index 21f9c96e80..309527e3ae 100644 --- a/src/algo/depth-to-rgb-calibration/frame-data.h +++ b/src/algo/depth-to-rgb-calibration/frame-data.h @@ -105,6 +105,7 @@ namespace depth_to_rgb_calibration { std::vector valid_edge_sub_pixel_y; std::vector sub_points; std::vector vertices; + std::vector orig_vertices; std::vector vertices3; std::vector uvmap; std::vector< byte > is_inside; diff --git a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp index 47478bca93..7f64ef51dc 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -2,6 +2,7 @@ //// Copyright(c) 2020 Intel Corporation. All Rights Reserved. #include "k-to-dsm.h" +#include "optimizer.h" #include "debug.h" #include "utils.h" #include @@ -18,7 +19,7 @@ rs2_intrinsics_double rotate_k_mat(const rs2_intrinsics_double& k_mat) return res; } -k_to_DSM::k_to_DSM( const rs2_dsm_params & orig_dsm_params, +k_to_DSM::k_to_DSM( const rs2_dsm_params_double & orig_dsm_params, algo_calibration_info const & cal_info, algo_calibration_registers const & cal__regs, const double & max_scaling_step ) @@ -30,7 +31,7 @@ k_to_DSM::k_to_DSM( const rs2_dsm_params & orig_dsm_params, } algo_calibration_registers -k_to_DSM::apply_ac_res_on_dsm_model( const rs2_dsm_params & ac_data, +k_to_DSM::apply_ac_res_on_dsm_model( const rs2_dsm_params_double & ac_data, const algo_calibration_registers & dsm_regs, const ac_to_dsm_dir & type ) { @@ -44,24 +45,24 @@ k_to_DSM::apply_ac_res_on_dsm_model( const rs2_dsm_params & ac_data, res = dsm_regs; break; case AOT: // AOT model - res.EXTLdsmXscale = float( (double)dsm_regs.EXTLdsmXscale * ac_data.h_scale ); - res.EXTLdsmYscale = float( (double)dsm_regs.EXTLdsmYscale * ac_data.v_scale ); + res.EXTLdsmXscale = (double)dsm_regs.EXTLdsmXscale * ac_data.h_scale ; + res.EXTLdsmYscale = (double)dsm_regs.EXTLdsmYscale * ac_data.v_scale ; res.EXTLdsmXoffset - = float( ( (double)dsm_regs.EXTLdsmXoffset + (double)ac_data.h_offset ) - / (double)ac_data.h_scale ); + = ( (double)dsm_regs.EXTLdsmXoffset + (double)ac_data.h_offset ) + / (double)ac_data.h_scale ; res.EXTLdsmYoffset - = float( ( (double)dsm_regs.EXTLdsmYoffset + (double)ac_data.v_offset ) - / (double)ac_data.v_scale ); + = ( (double)dsm_regs.EXTLdsmYoffset + (double)ac_data.v_offset ) + / (double)ac_data.v_scale ; break; case dsm_model::TOA: // TOA model - res.EXTLdsmXscale = float( (double)dsm_regs.EXTLdsmXscale * ac_data.h_scale ); - res.EXTLdsmYscale = float( (double)dsm_regs.EXTLdsmYscale * ac_data.v_scale ); + res.EXTLdsmXscale = (double)dsm_regs.EXTLdsmXscale * ac_data.h_scale ; + res.EXTLdsmYscale = (double)dsm_regs.EXTLdsmYscale * ac_data.v_scale ; res.EXTLdsmXoffset - = float( ( (double)dsm_regs.EXTLdsmXoffset + (double)ac_data.h_offset ) - / (double)dsm_regs.EXTLdsmXscale ); + = ( (double)dsm_regs.EXTLdsmXoffset + (double)ac_data.h_offset ) + / (double)dsm_regs.EXTLdsmXscale ; res.EXTLdsmYoffset - = float( ( (double)dsm_regs.EXTLdsmYoffset + (double)ac_data.v_offset ) - / (double)dsm_regs.EXTLdsmYscale ); + = ( (double)dsm_regs.EXTLdsmYoffset + (double)ac_data.v_offset ) + / (double)dsm_regs.EXTLdsmYscale ; break; default: throw std::runtime_error(_ac_data.flags[0] + " is not a valid model"); @@ -75,16 +76,16 @@ k_to_DSM::apply_ac_res_on_dsm_model( const rs2_dsm_params & ac_data, res = dsm_regs; break; case dsm_model::AOT: // AOT model - res.EXTLdsmXscale = dsm_regs.EXTLdsmXscale / ac_data.h_scale; - res.EXTLdsmYscale = dsm_regs.EXTLdsmYscale / ac_data.v_scale; - res.EXTLdsmXoffset = dsm_regs.EXTLdsmXoffset* ac_data.h_scale - ac_data.h_offset; - res.EXTLdsmYoffset = dsm_regs.EXTLdsmYoffset* ac_data.v_scale - ac_data.v_offset; + res.EXTLdsmXscale = (double)dsm_regs.EXTLdsmXscale / (double)ac_data.h_scale; + res.EXTLdsmYscale = (double)dsm_regs.EXTLdsmYscale / (double)ac_data.v_scale; + res.EXTLdsmXoffset = (double)dsm_regs.EXTLdsmXoffset* (double)ac_data.h_scale - (double)ac_data.h_offset; + res.EXTLdsmYoffset = (double)dsm_regs.EXTLdsmYoffset* (double)ac_data.v_scale - (double)ac_data.v_offset; break; case dsm_model::TOA: // TOA model - res.EXTLdsmXscale = dsm_regs.EXTLdsmXscale / ac_data.h_scale; - res.EXTLdsmYscale = dsm_regs.EXTLdsmYscale / ac_data.v_scale; - res.EXTLdsmXoffset = dsm_regs.EXTLdsmXoffset - ac_data.h_offset / res.EXTLdsmXscale; - res.EXTLdsmYoffset = dsm_regs.EXTLdsmYoffset - ac_data.v_offset / res.EXTLdsmYscale; + res.EXTLdsmXscale = (double)dsm_regs.EXTLdsmXscale / (double)ac_data.h_scale; + res.EXTLdsmYscale = (double)dsm_regs.EXTLdsmYscale / (double)ac_data.v_scale; + res.EXTLdsmXoffset = (double)dsm_regs.EXTLdsmXoffset - (double)ac_data.h_offset / (double)res.EXTLdsmXscale; + res.EXTLdsmYoffset = (double)dsm_regs.EXTLdsmYoffset - (double)ac_data.v_offset / (double)res.EXTLdsmYscale; break; default: throw std::runtime_error(ac_data.flags[0] + "is not valid model"); @@ -96,10 +97,10 @@ k_to_DSM::apply_ac_res_on_dsm_model( const rs2_dsm_params & ac_data, los_shift_scaling k_to_DSM::convert_ac_data_to_los_error( const algo_calibration_registers & dsm_regs, - const rs2_dsm_params & ac_data ) + const rs2_dsm_params_double & ac_data ) { los_shift_scaling res; - switch (_ac_data.flags[0]) + switch (ac_data.model) { case dsm_model::none: // none res.los_scaling_x = 1; @@ -108,19 +109,19 @@ k_to_DSM::convert_ac_data_to_los_error( const algo_calibration_registers & dsm_r res.los_shift_y = 0; break; case dsm_model::AOT: - res.los_scaling_x = 1/ _ac_data.h_scale; - res.los_scaling_y = 1/_ac_data.v_scale; - res.los_shift_x = -_ac_data.h_offset*res.los_scaling_x; - res.los_shift_y = -_ac_data.v_offset*res.los_scaling_y; + res.los_scaling_x = 1/ (double)ac_data.h_scale; + res.los_scaling_y = 1/ (double)ac_data.v_scale; + res.los_shift_x = -(double)ac_data.h_offset*res.los_scaling_x; + res.los_shift_y = -(double)ac_data.v_offset*res.los_scaling_y; break; case dsm_model::TOA: - res.los_scaling_x = 1 / _ac_data.h_scale; - res.los_scaling_y = 1 / _ac_data.v_scale; + res.los_scaling_x = 1 / (double)ac_data.h_scale; + res.los_scaling_y = 1 / (double)ac_data.v_scale; auto dsm_orig = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); - res.los_shift_x = -_ac_data.h_offset / dsm_orig.EXTLdsmXscale - dsm_orig.EXTLdsmXoffset*(1 - res.los_scaling_x); - res.los_shift_y = -_ac_data.v_offset / dsm_orig.EXTLdsmYscale - dsm_orig.EXTLdsmYoffset*(1 - res.los_scaling_y); + res.los_shift_x = -(double)ac_data.h_offset / (double)dsm_orig.EXTLdsmXscale - (double)dsm_orig.EXTLdsmXoffset*(1 - res.los_scaling_x); + res.los_shift_y = -(double)ac_data.v_offset / (double)dsm_orig.EXTLdsmYscale - (double)dsm_orig.EXTLdsmYoffset*(1 - res.los_scaling_y); break; } return res; @@ -129,7 +130,7 @@ k_to_DSM::convert_ac_data_to_los_error( const algo_calibration_registers & dsm_r pre_process_data k_to_DSM::pre_processing ( const algo_calibration_info& regs, - const rs2_dsm_params& ac_data, + const rs2_dsm_params_double& ac_data, const algo_calibration_registers& algo_calibration_registers, const rs2_intrinsics_double& orig_k_raw, const std::vector& relevant_pixels_image @@ -146,11 +147,13 @@ pre_process_data k_to_DSM::pre_processing return res; } -rs2_dsm_params k_to_DSM::convert_new_k_to_DSM +rs2_dsm_params_double k_to_DSM::convert_new_k_to_DSM ( const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const z_frame_data& z + const z_frame_data& z, // + std::vector& new_vertices, + iteration_data_collect* data ) { auto w = old_k.width; @@ -163,6 +166,7 @@ rs2_dsm_params k_to_DSM::convert_new_k_to_DSM std::vector relevant_pixels_image_rot(z.relevant_pixels_image.size(), 0); rotate_180(z.relevant_pixels_image.data(), relevant_pixels_image_rot.data(), w, h); + _pre_process_data = pre_processing(_regs, _ac_data, _dsm_regs, old_k_raw, relevant_pixels_image_rot); _new_los_scaling = convert_k_to_los_error(_regs, _dsm_regs, new_k_raw); @@ -170,7 +174,7 @@ rs2_dsm_params k_to_DSM::convert_new_k_to_DSM auto ac_data_cand = convert_los_error_to_ac_data(_ac_data,_dsm_regs, los_shift, _new_los_scaling); auto dsm_regs_cand = apply_ac_res_on_dsm_model(ac_data_cand, dsm_orig, direct); - auto sc_vertices = z.vertices; + auto sc_vertices = z.orig_vertices; for (auto i = 0; i < sc_vertices.size(); i++) { @@ -178,7 +182,15 @@ rs2_dsm_params k_to_DSM::convert_new_k_to_DSM sc_vertices[i].y *= -1; } auto los = convert_norm_vertices_to_los(_regs, _dsm_regs, sc_vertices); - auto new_vertices = convert_los_to_norm_vertices(_regs, dsm_regs_cand, los); + new_vertices = convert_los_to_norm_vertices(_regs, dsm_regs_cand, los, data); + + if (data) + { + data->cycle_data_p.dsm_regs_orig = dsm_orig; + data->cycle_data_p.relevant_pixels_image_rot = relevant_pixels_image_rot; + data->cycle_data_p.dsm_regs_cand = dsm_regs_cand; + data->cycle_data_p.los = los; + } for (auto i = 0; i < new_vertices.size(); i++) { @@ -269,16 +281,16 @@ double2 k_to_DSM::convert_k_to_los_error return los_scaling; } -rs2_dsm_params k_to_DSM::convert_los_error_to_ac_data +rs2_dsm_params_double k_to_DSM::convert_los_error_to_ac_data ( - const rs2_dsm_params& ac_data, + const rs2_dsm_params_double& ac_data, const algo_calibration_registers& dsm_regs, double2 los_shift, double2 los_scaling ) { - rs2_dsm_params ac_data_out = _ac_data; - ac_data_out.model = _ac_data.model; + rs2_dsm_params_double ac_data_out = ac_data; + ac_data_out.model = ac_data.model; switch (ac_data_out.model) { case dsm_model::none: @@ -288,21 +300,21 @@ rs2_dsm_params k_to_DSM::convert_los_error_to_ac_data ac_data_out.v_offset = 0; break; case dsm_model::AOT: - ac_data_out.h_scale = float( 1 / los_scaling.x ); - ac_data_out.v_scale = float( 1 / los_scaling.y ); - ac_data_out.h_offset = float( -los_shift.x / los_scaling.x ); - ac_data_out.v_offset = float( -los_shift.y / los_scaling.y ); + ac_data_out.h_scale = 1 / los_scaling.x ; + ac_data_out.v_scale = 1 / los_scaling.y ; + ac_data_out.h_offset = -los_shift.x / los_scaling.x; + ac_data_out.v_offset = -los_shift.y / los_scaling.y; break; case dsm_model::TOA: - ac_data_out.h_scale = float( 1 / los_scaling.x ); - ac_data_out.v_scale = float( 1 / los_scaling.y ); + ac_data_out.h_scale = 1 / los_scaling.x; + ac_data_out.v_scale = 1 / los_scaling.y; _dsm_regs = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); // the one used for assessing LOS error ac_data_out.h_offset - = float( -( _dsm_regs.EXTLdsmXoffset * ( 1 - los_scaling.x ) + los_shift.x ) - * _dsm_regs.EXTLdsmXscale ); + = -( _dsm_regs.EXTLdsmXoffset * ( 1 - los_scaling.x ) + los_shift.x ) + * _dsm_regs.EXTLdsmXscale ; ac_data_out.v_offset - = float( -( _dsm_regs.EXTLdsmYoffset * ( 1 - los_scaling.y ) + los_shift.y ) - * _dsm_regs.EXTLdsmYscale ); + = -( _dsm_regs.EXTLdsmYoffset * ( 1 - los_scaling.y ) + los_shift.y ) + * _dsm_regs.EXTLdsmYscale ; break; } @@ -503,41 +515,50 @@ std::vector k_to_DSM::convert_los_to_norm_vertices ( algo_calibration_info const & regs, algo_calibration_registers const &dsm_regs, - std::vector los + std::vector los, + iteration_data_collect* data ) { std::vector fove_x_indicent_direction(los.size()); std::vector fove_y_indicent_direction(los.size()); - auto laser_incident = laser_incident_direction({ regs.FRMWlaserangleH , regs.FRMWlaserangleV + 180 }); + auto laser_incident = laser_incident_direction({ (double)regs.FRMWlaserangleH , (double)regs.FRMWlaserangleV + 180 }); + + std::vector dsm(los.size()); for (auto i = 0; i < los.size(); i++) { - auto dsm_x = (los[i].x + dsm_regs.EXTLdsmXoffset)*dsm_regs.EXTLdsmXscale - 2047; - auto dsm_y = (los[i].y + dsm_regs.EXTLdsmYoffset)*dsm_regs.EXTLdsmYscale - 2047; + dsm[i].x = (los[i].x + (double)dsm_regs.EXTLdsmXoffset)*(double)dsm_regs.EXTLdsmXscale - (double)2047; + dsm[i].y = (los[i].y + (double)dsm_regs.EXTLdsmYoffset)*(double)dsm_regs.EXTLdsmYscale - (double)2047; + + auto dsm_x = dsm[i].x; + auto dsm_y = dsm[i].y; - auto dsm_x_corr_coarse = dsm_x + (dsm_x / 2047)*regs.FRMWpolyVars[0] + - std::pow(dsm_x / 2047, 2)*_regs.FRMWpolyVars[1] + - std::pow(dsm_x / 2047, 3)*_regs.FRMWpolyVars[2]; + auto dsm_x_corr_coarse = dsm_x + (dsm_x / 2047)*(double)regs.FRMWpolyVars[0] + + std::pow(dsm_x / 2047, 2)*(double)regs.FRMWpolyVars[1] + + std::pow(dsm_x / 2047, 3)*(double)regs.FRMWpolyVars[2]; - auto dsm_y_corr_coarse = dsm_y + (dsm_x / 2047)*regs.FRMWpitchFixFactor; + auto dsm_y_corr_coarse = dsm_y + (dsm_x / 2047)*(double)regs.FRMWpitchFixFactor; - auto dsm_x_corr = dsm_x_corr_coarse + (dsm_x_corr_coarse / 2047)*regs.FRMWundistAngHorz[0] + - std::pow(dsm_x_corr_coarse / 2047, 2)*regs.FRMWundistAngHorz[1] + - std::pow(dsm_x_corr_coarse / 2047, 3)*regs.FRMWundistAngHorz[2] + - std::pow(dsm_x_corr_coarse / 2047, 4)*regs.FRMWundistAngHorz[3]; + auto dsm_x_corr = dsm_x_corr_coarse + (dsm_x_corr_coarse / 2047)*(double)regs.FRMWundistAngHorz[0] + + std::pow(dsm_x_corr_coarse / 2047, 2)*(double)regs.FRMWundistAngHorz[1] + + std::pow(dsm_x_corr_coarse / 2047, 3)*(double)regs.FRMWundistAngHorz[2] + + std::pow(dsm_x_corr_coarse / 2047, 4)*(double)regs.FRMWundistAngHorz[3]; auto dsm_y_corr = dsm_y_corr_coarse; auto mode = 1; - auto ang_x = dsm_x_corr * (regs.FRMWxfov[mode] * 0.25 / 2047); - auto ang_y = dsm_y_corr * (regs.FRMWyfov[mode] * 0.25 / 2047); + auto ang_x = dsm_x_corr * ((double)regs.FRMWxfov[mode] * 0.25 / 2047); + auto ang_y = dsm_y_corr * ((double)regs.FRMWyfov[mode] * 0.25 / 2047); auto mirror_normal_direction = laser_incident_direction({ ang_x ,ang_y }); fove_x_indicent_direction[i] = laser_incident - mirror_normal_direction*(2 * (mirror_normal_direction*laser_incident)); } + if (data) + data->cycle_data_p.dsm = dsm; + for (auto i = 0; i < fove_x_indicent_direction.size(); i++) { fove_x_indicent_direction[i].x /= sqrt(fove_x_indicent_direction[i].get_norm()); @@ -546,16 +567,16 @@ std::vector k_to_DSM::convert_los_to_norm_vertices } auto outbound_direction = fove_x_indicent_direction; - if (_regs.FRMWfovexExistenceFlag) + if (regs.FRMWfovexExistenceFlag) { std::fill(outbound_direction.begin(), outbound_direction.end(), double3{ 0,0,0 }); for (auto i = 0; i < fove_x_indicent_direction.size(); i++) { auto ang_pre_exp = rad_to_deg(std::acos(fove_x_indicent_direction[i].z)); - auto ang_post_exp = ang_pre_exp + ang_pre_exp * _regs.FRMWfovexNominal[0] + - std::pow(ang_pre_exp, 2) * _regs.FRMWfovexNominal[1] + - std::pow(ang_pre_exp, 3) * _regs.FRMWfovexNominal[2] + - std::pow(ang_pre_exp, 4) * _regs.FRMWfovexNominal[3]; + auto ang_post_exp = ang_pre_exp + ang_pre_exp * (double)regs.FRMWfovexNominal[0] + + std::pow(ang_pre_exp, 2) * (double)regs.FRMWfovexNominal[1] + + std::pow(ang_pre_exp, 3) * (double)regs.FRMWfovexNominal[2] + + std::pow(ang_pre_exp, 4) * (double)regs.FRMWfovexNominal[3]; outbound_direction[i].z = std::cos(deg_to_rad(ang_post_exp)); auto xy_norm = fove_x_indicent_direction[i].x*fove_x_indicent_direction[i].x + @@ -642,10 +663,10 @@ std::vector k_to_DSM::convert_norm_vertices_to_los for (auto i = 0; i < angle; i++) { ang_grid[i] = i; - auto fovex_nominal = std::pow(i,1)* (double)_regs.FRMWfovexNominal[0] + - std::pow(i, 2)* (double)_regs.FRMWfovexNominal[1] + - std::pow(i, 3)* (double)_regs.FRMWfovexNominal[2] + - std::pow(i, 4)* (double)_regs.FRMWfovexNominal[3]; + auto fovex_nominal = std::pow(i,1)* (double)regs.FRMWfovexNominal[0] + + std::pow(i, 2)* (double)regs.FRMWfovexNominal[1] + + std::pow(i, 3)* (double)regs.FRMWfovexNominal[2] + + std::pow(i, 4)* (double)regs.FRMWfovexNominal[3]; ang_out_on_grid[i] = i + fovex_nominal; } @@ -685,8 +706,8 @@ std::vector k_to_DSM::convert_norm_vertices_to_los int mirror_mode = 1/*_regs.frmw.mirrorMovmentMode*/; - dsm_x_corr[i] = ang_x / (double)(_regs.FRMWxfov[mirror_mode - 1] * 0.25 / 2047); - dsm_y_corr[i] = ang_y / (double)(_regs.FRMWyfov[mirror_mode - 1] * 0.25 / 2047); + dsm_x_corr[i] = ang_x / (double)(regs.FRMWxfov[mirror_mode - 1] * 0.25 / 2047); + dsm_y_corr[i] = ang_y / (double)(regs.FRMWyfov[mirror_mode - 1] * 0.25 / 2047); } diff --git a/src/algo/depth-to-rgb-calibration/k-to-dsm.h b/src/algo/depth-to-rgb-calibration/k-to-dsm.h index ad81c16a74..ffc522eb91 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -29,12 +29,59 @@ namespace depth_to_rgb_calibration { struct los_shift_scaling { - float los_scaling_x; - float los_scaling_y; - float los_shift_x; - float los_shift_y; + double los_scaling_x; + double los_scaling_y; + double los_shift_x; + double los_shift_y; }; + struct rs2_dsm_params_double + { + rs2_dsm_params_double(rs2_dsm_params obj) + :timestamp(obj.timestamp), + version(obj.version), + model(obj.model), + h_scale(obj.h_scale), + v_scale(obj.v_scale), + h_offset(obj.h_offset), + v_offset(obj.v_offset), + rtd_offset(obj.rtd_offset), + temp_x2(obj.temp_x2){} //todo: flags + + rs2_dsm_params_double() = default; + + operator rs2_dsm_params() + { + rs2_dsm_params res; + + res.timestamp = timestamp; + res.version = version; + res.model = model; + for (auto i = 0; i < 5; i++) + res.flags[i] = flags[i]; + + res.h_scale = h_scale; + res.v_scale = v_scale; + res.h_offset = h_offset; + res.v_offset = v_offset; + res.rtd_offset = rtd_offset; + res.temp_x2 = temp_x2; + return res; + } + + unsigned long long timestamp; /**< system_clock::time_point::time_since_epoch().count() */ + unsigned short version; /**< MAJOR<<12 | MINOR<<4 | PATCH */ + unsigned char model; /**< rs2_dsm_correction_model */ + unsigned char flags[5]; /**< TBD, now 0s */ + double h_scale; /**< the scale factor to horizontal DSM scale thermal results */ + double v_scale; /**< the scale factor to vertical DSM scale thermal results */ + double h_offset; /**< the offset to horizontal DSM offset thermal results */ + double v_offset; /**< the offset to vertical DSM offset thermal results */ + double rtd_offset; /**< the offset to the Round-Trip-Distance delay thermal results */ + unsigned char temp_x2; /**< the temperature recorded times 2 (ldd for depth; hum for rgb) */ + unsigned char reserved[11]; + }; + #pragma pack(push, 1) // This table is read from FW and is used in the optimizer struct algo_calibration_info @@ -118,12 +165,13 @@ namespace depth_to_rgb_calibration { float FRMWhumidApdTempDiff; uint8_t reserved[94]; }; + struct algo_calibration_registers { - float EXTLdsmXscale; - float EXTLdsmYscale; - float EXTLdsmXoffset; - float EXTLdsmYoffset; + double EXTLdsmXscale; + double EXTLdsmYscale; + double EXTLdsmXoffset; + double EXTLdsmYoffset; }; #pragma pack(pop) @@ -136,29 +184,36 @@ namespace depth_to_rgb_calibration { std::vector los_orig; }; + struct iteration_data_collect; class k_to_DSM { public: - k_to_DSM(const rs2_dsm_params& orig_dsm_params, + k_to_DSM(const rs2_dsm_params_double& orig_dsm_params, algo::depth_to_rgb_calibration::algo_calibration_info const & cal_info, algo::depth_to_rgb_calibration::algo_calibration_registers const & cal_regs, const double& max_scaling_step); - algo_calibration_registers apply_ac_res_on_dsm_model(const rs2_dsm_params& ac_data, const algo_calibration_registers& regs, const ac_to_dsm_dir& type); + algo_calibration_registers apply_ac_res_on_dsm_model(const rs2_dsm_params_double& ac_data, + const algo_calibration_registers& regs, + const ac_to_dsm_dir& type); - los_shift_scaling convert_ac_data_to_los_error(const algo_calibration_registers& algo_calibration_registers, const rs2_dsm_params& ac_data); + los_shift_scaling convert_ac_data_to_los_error(const algo_calibration_registers& algo_calibration_registers, + const rs2_dsm_params_double& ac_data); pre_process_data pre_processing(const algo_calibration_info& regs, - const rs2_dsm_params& ac_data, + const rs2_dsm_params_double& ac_data, const algo_calibration_registers& algo_calibration_registers, const rs2_intrinsics_double& k_raw, const std::vector& relevant_pixels_image); - rs2_dsm_params convert_new_k_to_DSM( + //return rs2_dsm_params and new vertices + rs2_dsm_params_double convert_new_k_to_DSM( const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const z_frame_data& z); + const z_frame_data& z, + std::vector& new_vertices, + iteration_data_collect* data = nullptr); const pre_process_data& get_pre_process_data() const; @@ -168,8 +223,8 @@ namespace depth_to_rgb_calibration { algo_calibration_registers const &dsm_regs, rs2_intrinsics_double const & k_raw); - rs2_dsm_params convert_los_error_to_ac_data( - const rs2_dsm_params& ac_data, + rs2_dsm_params_double convert_los_error_to_ac_data( + const rs2_dsm_params_double& ac_data, const algo_calibration_registers& dsm_regs, double2 los_shift, double2 los_scaling); @@ -189,7 +244,8 @@ namespace depth_to_rgb_calibration { std::vector convert_los_to_norm_vertices( algo::depth_to_rgb_calibration::algo_calibration_info const & regs, algo_calibration_registers const &dsm_regs, - std::vector los); + std::vector los, + iteration_data_collect* data = nullptr); std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); @@ -207,7 +263,7 @@ namespace depth_to_rgb_calibration { double2 _new_los_scaling; //input camera params - rs2_dsm_params _ac_data; + rs2_dsm_params_double _ac_data; algo::depth_to_rgb_calibration::algo_calibration_info _regs; algo::depth_to_rgb_calibration::algo_calibration_registers _dsm_regs; diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index a4e77dd7ac..6e696c858a 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -791,17 +791,21 @@ void optimizer::set_ir_data( _ir.edges = calc_edges( _ir.ir_frame, width, height ); } -void optimizer::decompose_p_mat() +calib optimizer::decompose_p_mat(p_matrix p) { - _final_calibration = decompose( _params_curr.curr_p_mat, _original_calibration ); - - _z.new_intrinsics = _z.orig_intrinsics; - _z.new_intrinsics.fx = _z.new_intrinsics.fx / _final_calibration.k_mat.fx*_original_calibration.k_mat.fx; - _z.new_intrinsics.fy = _z.new_intrinsics.fy / _final_calibration.k_mat.fy*_original_calibration.k_mat.fy; + auto calib = decompose(p, _original_calibration); + return calib; +} - // TODO AVISHAG - //_final_calibration.k_mat.fx = _original_calibration.k_mat.fx; - //_final_calibration.k_mat.fy = _original_calibration.k_mat.fy; + +rs2_intrinsics_double optimizer::get_new_z_intrinsics_from_new_calib(const rs2_intrinsics_double& orig, const calib & new_c, const calib & orig_c) +{ + rs2_intrinsics_double res; + res = orig; + res.fx = res.fx / new_c.k_mat.fx*orig_c.k_mat.fx; + res.fy = res.fy / new_c.k_mat.fy*orig_c.k_mat.fy; + + return res; } void optimizer::zero_invalid_edges( z_frame_data & z_data, ir_frame_data const & ir_data ) @@ -1281,6 +1285,11 @@ calib const & optimizer::get_calibration() const return _final_calibration; } +rs2_dsm_params const & optimizer::get_dsm_params() const +{ + return _final_dsm_params; +} + double optimizer::get_cost() const { return _params_curr.cost; @@ -1482,73 +1491,148 @@ optimization_params optimizer::back_tracking_line_search( optimization_params co return new_params; } -size_t optimizer::optimize( std::function< void( iteration_data_collect const & data ) > cb ) +size_t optimizer::optimize_p +( + const optimization_params& params_curr, + optimization_params& params_new, + calib& new_rgb_calib, + rs2_intrinsics_double& new_z_k, + std::function cb, + iteration_data_collect* data +) { - optimization_params params_orig; - params_orig.curr_p_mat = _original_calibration.calc_p_mat(); - - auto res = calc_cost_and_grad( _z, _yuy, _original_calibration, params_orig.curr_p_mat ); - params_orig.cost = res.first; - params_orig.calib_gradients = res.second; - - _params_curr = params_orig; - size_t n_iterations = 0; - while( 1 ) + auto curr = params_curr; + while (1) { - iteration_data_collect data; - data.cycle = 1; - data.iteration = n_iterations; - calib curr_calib = decompose( _params_curr.curr_p_mat, _original_calibration ); - auto res = calc_cost_and_grad( _z, _yuy, curr_calib, _params_curr.curr_p_mat, &data ); - _params_curr.cost = res.first; - _params_curr.calib_gradients = res.second; - AC_LOG( DEBUG, n_iterations << ": Cost = " << AC_D_PREC << _params_curr.cost ); + auto res = calc_cost_and_grad(_z, _yuy, new_rgb_calib, curr.curr_p_mat, data); + curr.cost = res.first; + curr.calib_gradients = res.second; + AC_LOG( DEBUG, " ------> " << n_iterations << ": cost= " << AC_D_PREC << curr.cost ); - data.params = _params_curr; + if (data) + { + data->type = iteration_data; + data->params = curr; + data->c = new_rgb_calib; + data->iteration = n_iterations; + } - auto prev_params = _params_curr; - _params_curr = back_tracking_line_search( _params_curr, &data); - data.next_params = _params_curr; + params_new = back_tracking_line_search(curr, data); + + if (data) + data->next_params = params_new; if (cb) - cb(data); + cb(*data); - auto norm = (_params_curr.curr_p_mat - prev_params.curr_p_mat).get_norma(); - if( norm < _params.min_rgb_mat_delta ) + auto norm = (params_new.curr_p_mat - curr.curr_p_mat).get_norma(); + if (norm < _params.min_rgb_mat_delta) { - AC_LOG( DEBUG, "... {normal(new-curr)} " << norm << " < " << _params.min_rgb_mat_delta << " {min_rgb_mat_delta} --> stopping" ); + AC_LOG(DEBUG, "... {normal(new-curr)} " << norm << " < " << _params.min_rgb_mat_delta << " {min_rgb_mat_delta} --> stopping"); break; } - auto delta = _params_curr.cost - prev_params.cost; + auto delta = params_new.cost - curr.cost; AC_LOG( DEBUG, " delta= " << AC_D_PREC << delta ); - delta = abs( delta ); - if( delta < _params.min_cost_delta ) + delta = abs(delta); + if (delta < _params.min_cost_delta) { - AC_LOG( DEBUG, "... delta < " << _params.min_cost_delta << " --> stopping" ); + AC_LOG(DEBUG, "... delta < " << _params.min_cost_delta << " --> stopping"); break; } - if( ++n_iterations >= _params.max_optimization_iters ) + if (++n_iterations >= _params.max_optimization_iters) { - AC_LOG( DEBUG, "... exceeding max iterations --> stopping" ); + AC_LOG(DEBUG, "... exceeding max iterations --> stopping"); break; } + + curr = params_new; + new_rgb_calib = decompose_p_mat(params_new.curr_p_mat); } - if( !n_iterations ) + + if (!n_iterations) { - AC_LOG( INFO, "Calibration not necessary; nothing done" ); + AC_LOG(INFO, "Calibration not necessary; nothing done"); } else { - AC_LOG( INFO, "Calibration finished after " << n_iterations << " iterations; original cost= " << params_orig.cost << " optimized cost= " << _params_curr.cost ); + AC_LOG(INFO, "Calibration finished after " << n_iterations << " iterations; original cost= " << params_curr.cost << " optimized cost= " << params_new.cost); } + new_rgb_calib = decompose_p_mat(params_new.curr_p_mat); + auto orig_rgb_calib = decompose_p_mat(params_curr.curr_p_mat); + new_z_k = get_new_z_intrinsics_from_new_calib(_z.orig_intrinsics, new_rgb_calib, orig_rgb_calib); + new_rgb_calib.k_mat.fx = _original_calibration.k_mat.fx; + new_rgb_calib.k_mat.fy = _original_calibration.k_mat.fy; + + params_new.curr_p_mat = new_rgb_calib.calc_p_mat(); + return n_iterations; +} - decompose_p_mat(); +size_t optimizer::optimize( std::function< void( iteration_data_collect const & data ) > cb ) +{ + optimization_params params_orig; + params_orig.curr_p_mat = _original_calibration.calc_p_mat(); + _original_calibration = decompose(params_orig.curr_p_mat, _original_calibration); + _params_curr = params_orig; + + iteration_data_collect data; + + auto cycle = 1; + data.cycle = cycle; + + auto res = calc_cost_and_grad(_z, _yuy, decompose(_params_curr.curr_p_mat, _original_calibration), _params_curr.curr_p_mat, &data); + _params_curr.cost = res.first; + _params_curr.calib_gradients = res.second; + + optimization_params new_params; + calib new_calib = _original_calibration; + rs2_intrinsics_double new_k_depth; + double last_cost = _params_curr.cost; + + auto n_iterations = optimize_p(_params_curr, new_params, new_calib, new_k_depth, cb, &data); + AC_LOG(DEBUG, n_iterations << ": Cost = " << AC_D_PREC << new_params.cost); + + _z.orig_vertices = _z.vertices; + rs2_dsm_params_double new_dsm_params = _z.orig_dsm_params; + while (cycle < _params.max_K2DSM_iters) + { + data.cycle = ++cycle; + + std::vector new_vertices; + auto dsm_candidate = _k_to_DSM->convert_new_k_to_DSM(_z.orig_intrinsics, new_k_depth, _z, new_vertices, &data); + data.type = cycle_data; + + data.cycle_data_p.dsm_params_cand = dsm_candidate; + data.cycle_data_p.vertices = new_vertices; + data.cycle_data_p.dsm_pre_process_data = _k_to_DSM->get_pre_process_data(); + + if (cb) + cb(data); + + _z.vertices = new_vertices; + + optimization_params params_candidate; + calib calib_candidate = new_calib; + rs2_intrinsics_double k_depth_candidate; + optimize_p(new_params, params_candidate, calib_candidate, k_depth_candidate, cb, &data); + + if (params_candidate.cost < last_cost) + break; + + new_params = params_candidate; + new_calib = calib_candidate; + new_k_depth = k_depth_candidate; + new_dsm_params = dsm_candidate; + last_cost = new_params.cost; + } + + AC_LOG(INFO, "Calibration converged; cost= " << new_params.cost); - _k_to_DSM->convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z); + _final_dsm_params = clip_ac_scaling(_z.orig_dsm_params, new_dsm_params); + _final_calibration = new_calib; return n_iterations; } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index 5f0d6c032f..d243b19fc6 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -72,6 +72,8 @@ namespace depth_to_rgb_calibration { double const max_xy_movement_per_calibration[3] = { 10, 2, 2 }; double const max_xy_movement_from_origin = 20; double const max_scaling_step = 0.020000000000000; + double const max_K2DSM_iters = 10; + double const max_global_los_scaling_step = 0.005; }; // svm struct decision_params @@ -138,11 +140,33 @@ namespace depth_to_rgb_calibration { }; }; // Data that's passed to a callback at each optimization iteration + enum data_type + { + cycle_data, + iteration_data + }; + + struct cycle_data_params + { + algo_calibration_registers dsm_regs_orig; + std::vector < uint8_t> relevant_pixels_image_rot; + pre_process_data dsm_pre_process_data; + rs2_dsm_params_double dsm_params_cand; + algo_calibration_registers dsm_regs_cand; + std::vector los; + std::vector dsm; + std::vector < double3> vertices; + }; + struct iteration_data_collect { + data_type type; size_t cycle; size_t iteration; + cycle_data_params cycle_data_p; + optimization_params params; + calib c; std::vector< double2 > uvmap; std::vector< double > d_vals; std::vector< double > d_vals_x; @@ -160,6 +184,7 @@ namespace depth_to_rgb_calibration { int back_tracking_line_search_iters; double t; optimization_params next_params; + calib next_c; }; @@ -194,6 +219,8 @@ namespace depth_to_rgb_calibration { bool is_valid_results(); calib const & get_calibration() const; + rs2_dsm_params const & get_dsm_params() const; + double get_cost() const; double calc_correction_in_pixels( calib const & from_calibration ) const; double calc_correction_in_pixels() const { return calc_correction_in_pixels( _original_calibration ); } @@ -207,7 +234,18 @@ namespace depth_to_rgb_calibration { params const & get_params() const { return _params; } private: - void decompose_p_mat(); + + // 1 cycle of optimization + size_t optimize_p(const optimization_params& params_curr, + optimization_params& params_new, + calib& new_rgb_calib, + rs2_intrinsics_double& new_z_k, + std::function< void(iteration_data_collect const & data) > + iteration_callback = nullptr, + iteration_data_collect* data = nullptr); + + calib decompose_p_mat(p_matrix p); + rs2_intrinsics_double get_new_z_intrinsics_from_new_calib(const rs2_intrinsics_double& orig, const calib & new_c, const calib & orig_c); void zero_invalid_edges( z_frame_data& z_data, ir_frame_data const & ir_data ); std::vector get_direction( std::vector gradient_x, std::vector gradient_y ); std::vector get_direction2(std::vector gradient_x, std::vector gradient_y); @@ -237,6 +275,7 @@ namespace depth_to_rgb_calibration { // output validation void clip_pixel_movement( size_t iteration_number = 0 ); std::vector< double > cost_per_section_diff( calib const & old_calib, calib const & new_calib ); + rs2_dsm_params clip_ac_scaling(rs2_dsm_params_double ac_data_orig, rs2_dsm_params_double ac_data_new); private: params _params; @@ -250,6 +289,7 @@ namespace depth_to_rgb_calibration { z_frame_data _z; calib _original_calibration; // starting state of auto-calibration calib _final_calibration; // starting state of auto-calibration + rs2_dsm_params _final_dsm_params; calib _factory_calibration; // factory default calibration of the camera optimization_params _params_curr; // last-known setting std::shared_ptr _k_to_DSM; diff --git a/src/algo/depth-to-rgb-calibration/valid-results.cpp b/src/algo/depth-to-rgb-calibration/valid-results.cpp index 7735cc7fab..6c1be94255 100644 --- a/src/algo/depth-to-rgb-calibration/valid-results.cpp +++ b/src/algo/depth-to-rgb-calibration/valid-results.cpp @@ -81,7 +81,7 @@ std::vector< double > optimizer::cost_per_section_diff(calib const & old_calib, if (_z.section_map.size() != _z.weights.size()) throw std::runtime_error("section_map has not been initialized"); - auto uvmap_old = get_texture_map(_z.vertices, old_calib, old_calib.calc_p_mat()); + auto uvmap_old = get_texture_map(_z.orig_vertices, old_calib, old_calib.calc_p_mat()); auto uvmap_new = get_texture_map(_z.vertices, new_calib, new_calib.calc_p_mat()); size_t const n_sections_x = _params.num_of_sections_for_edge_distribution_x; @@ -125,6 +125,26 @@ std::vector< double > optimizer::cost_per_section_diff(calib const & old_calib, return cost_per_section_diff; } + + +rs2_dsm_params optimizer::clip_ac_scaling(rs2_dsm_params_double ac_data_in, rs2_dsm_params_double ac_data_new) +{ + if (abs(ac_data_in.h_scale - ac_data_new.h_scale) > _params.max_global_los_scaling_step) + { + ac_data_new.h_scale = ac_data_in.h_scale + (ac_data_new.h_scale - ac_data_in.h_scale) / + abs(ac_data_new.h_scale - ac_data_in.h_scale)*_params.max_global_los_scaling_step; + + } + if (abs(ac_data_in.v_scale - ac_data_new.v_scale) > _params.max_global_los_scaling_step) + { + ac_data_new.v_scale = ac_data_in.v_scale + (ac_data_new.v_scale - ac_data_in.v_scale) / + abs(ac_data_new.v_scale - ac_data_in.v_scale)*_params.max_global_los_scaling_step; + + } + + return ac_data_new; +} + std::vector< double > extract_features(decision_params& decision_params) { svm_features features; diff --git a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h index f87f6868f7..78469400ac 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -145,223 +145,266 @@ void compare_scene( std::string const & scene_dir ) CHECK( compare_to_bin_file< double >( yuy_data.gaussian_diff_masked, scene_dir, "IDiffMasked", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< uint8_t >( yuy_data.move_suspect, scene_dir, "ixMoveSuspect", rgb_w, rgb_h, "uint8_00", compare_same_vectors ) ); - //-- TRACE( "\nOptimizing:" ); auto cb = [&]( algo::iteration_data_collect const & data ) { // data.iteration is 0-based! //REQUIRE( data.iteration < md.n_iterations ); + REQUIRE( data.cycle <= md.n_cycles ); - CHECK( compare_to_bin_file< double >( - std::vector< double >( std::begin( data.params.curr_p_mat.vals ), - std::end( data.params.curr_p_mat.vals ) ), - scene_dir, - bin_file( "p_matrix_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, - 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( 1, data.params.cost ), - scene_dir, - bin_file( "cost_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin" ), - 1, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< algo::double2 >( - data.uvmap, - scene_dir, - bin_file( "uvmap_iteration", data.cycle, data.iteration + 1, 2, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - data.d_vals, - scene_dir, - bin_file( "DVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - data.d_vals_x, - scene_dir, - bin_file( "DxVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - data.d_vals_y, - scene_dir, - bin_file( "DyVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< algo::double2 >( - data.xy, - scene_dir, - bin_file( "xy_iteration", data.cycle, data.iteration + 1, 2, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - data.rc, - scene_dir, - bin_file( "rc_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< algo::p_matrix >( data.coeffs_p.x_coeffs, - scene_dir, - bin_file( "xCoeff_P_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, md.n_edges, - "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< algo::p_matrix >( data.coeffs_p.y_coeffs, - scene_dir, - bin_file( "yCoeff_P_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, md.n_edges, - "double_00.bin" ), - md.n_edges, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( std::begin( data.params.calib_gradients.vals ), - std::end( data.params.calib_gradients.vals ) ), - scene_dir, - bin_file( "grad_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( 1, data.grads_norma ), - scene_dir, - bin_file( "grad_norma_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin" ), - 1, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( std::begin( data.grads_norm.vals ), - std::end( data.grads_norm.vals ) ), - scene_dir, - bin_file( "grads_norm_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - - CHECK( compare_to_bin_file< double >( - std::vector< double >( std::begin( data.normalized_grads.vals ), - std::end( data.normalized_grads.vals ) ), - scene_dir, - bin_file( "normalized_grads_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - - CHECK( - compare_to_bin_file< double >( std::vector< double >( std::begin( data.unit_grad.vals ), - std::end( data.unit_grad.vals ) ), - scene_dir, - bin_file( "unit_grad_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( 1, data.t ), - scene_dir, - bin_file( "t_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin" ), - 1, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( 1, data.back_tracking_line_search_iters ), - scene_dir, - bin_file( "back_tracking_line_iter_count_iteration", - data.cycle, - data.iteration + 1, - 1, 1, - "double_00.bin" ), - 1, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( std::begin( data.next_params.curr_p_mat.vals ), - std::end( data.next_params.curr_p_mat.vals ) ), - scene_dir, - bin_file( "next_p_matrix_iteration", - data.cycle, - data.iteration + 1, - num_of_p_matrix_elements, 1, - "double_00.bin" ), - num_of_p_matrix_elements, 1, - compare_same_vectors ) ); - - CHECK( compare_to_bin_file< double >( - std::vector< double >( 1, data.next_params.cost ), - scene_dir, - bin_file( "next_cost_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin" ), - 1, 1, - compare_same_vectors ) ); + if (data.type == algo::cycle_data) + { + CHECK(compare_to_bin_file< algo::algo_calibration_registers >( + data.cycle_data_p.dsm_regs_orig, + scene_dir, + bin_file("dsmRegsOrig", data.cycle, 4, 1, "double_00.bin"))); + + CHECK(compare_to_bin_file< uint8_t >( + data.cycle_data_p.relevant_pixels_image_rot, + scene_dir, + bin_file("relevantPixelnImage_rot", data.cycle, z_w, z_h, "uint8_00") + ".bin", + z_w, z_h, compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::los_shift_scaling >( + data.cycle_data_p.dsm_pre_process_data.last_los_error, + scene_dir, + bin_file("dsm_los_error_orig", data.cycle, 1, 4, "double_00.bin"))); + + CHECK(compare_to_bin_file< algo::double3 >( + data.cycle_data_p.dsm_pre_process_data.vertices_orig, + scene_dir, bin_file("verticesOrig", data.cycle, 3, md.n_relevant_pixels, "double_00") + ".bin", + md.n_relevant_pixels, 1, compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double2 >( + data.cycle_data_p.dsm_pre_process_data.los_orig, scene_dir, + bin_file("losOrig", data.cycle, 2, md.n_relevant_pixels, "double_00") + ".bin", + md.n_relevant_pixels, 1, compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double2 >( + std::vector< algo::double2 >(1, { data.cycle_data_p.dsm_params_cand.h_scale, data.cycle_data_p.dsm_params_cand.v_scale }), + scene_dir, + bin_file("acDataCand", data.cycle, 2, 1, "double_00.bin"), + 1, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::algo_calibration_registers >( + data.cycle_data_p.dsm_regs_cand, + scene_dir, + bin_file("dsmRegsCand", data.cycle, 4, 1, "double_00.bin"))); + + CHECK(compare_to_bin_file< algo::double2 >( + data.cycle_data_p.los, scene_dir, + bin_file("new_los", data.cycle, 2, md.n_edges, "double_00") + ".bin", + md.n_edges, 1, compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double2 >( + data.cycle_data_p.dsm, scene_dir, + bin_file("dsm", data.cycle, 2, md.n_edges, "double_00") + ".bin", + md.n_edges, 1, compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double3 >( + data.cycle_data_p.vertices, + scene_dir, + bin_file("new_vertices_cycle", data.cycle, 3, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + } + + else + { + CHECK(compare_to_bin_file< double >( + std::vector< double >(std::begin(data.params.curr_p_mat.vals), + std::end(data.params.curr_p_mat.vals)), + scene_dir, + bin_file("p_matrix_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, + 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(1, data.params.cost), + scene_dir, + bin_file("cost_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin"), + 1, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double2 >( + data.uvmap, + scene_dir, + bin_file("uvmap_iteration", data.cycle, data.iteration + 1, 2, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + data.d_vals, + scene_dir, + bin_file("DVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + data.d_vals_x, + scene_dir, + bin_file("DxVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + data.d_vals_y, + scene_dir, + bin_file("DyVals_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::double2 >( + data.xy, + scene_dir, + bin_file("xy_iteration", data.cycle, data.iteration + 1, 2, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + data.rc, + scene_dir, + bin_file("rc_iteration", data.cycle, data.iteration + 1, 1, md.n_edges, "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::p_matrix >(data.coeffs_p.x_coeffs, + scene_dir, + bin_file("xCoeff_P_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, md.n_edges, + "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< algo::p_matrix >(data.coeffs_p.y_coeffs, + scene_dir, + bin_file("yCoeff_P_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, md.n_edges, + "double_00.bin"), + md.n_edges, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(std::begin(data.params.calib_gradients.vals), + std::end(data.params.calib_gradients.vals)), + scene_dir, + bin_file("grad_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(1, data.grads_norma), + scene_dir, + bin_file("grad_norma_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin"), + 1, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(std::begin(data.grads_norm.vals), + std::end(data.grads_norm.vals)), + scene_dir, + bin_file("grads_norm_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + + CHECK(compare_to_bin_file< double >( + std::vector< double >(std::begin(data.normalized_grads.vals), + std::end(data.normalized_grads.vals)), + scene_dir, + bin_file("normalized_grads_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + + CHECK( + compare_to_bin_file< double >(std::vector< double >(std::begin(data.unit_grad.vals), + std::end(data.unit_grad.vals)), + scene_dir, + bin_file("unit_grad_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(1, data.t), + scene_dir, + bin_file("t_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin"), + 1, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(1, data.back_tracking_line_search_iters), + scene_dir, + bin_file("back_tracking_line_iter_count_iteration", + data.cycle, + data.iteration + 1, + 1, 1, + "double_00.bin"), + 1, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(std::begin(data.next_params.curr_p_mat.vals), + std::end(data.next_params.curr_p_mat.vals)), + scene_dir, + bin_file("next_p_matrix_iteration", + data.cycle, + data.iteration + 1, + num_of_p_matrix_elements, 1, + "double_00.bin"), + num_of_p_matrix_elements, 1, + compare_same_vectors)); + + CHECK(compare_to_bin_file< double >( + std::vector< double >(1, data.next_params.cost), + scene_dir, + bin_file("next_cost_iteration", data.cycle, data.iteration + 1, 1, 1, "double_00.bin"), + 1, 1, + compare_same_vectors)); + } + }; // Our code doesn't count the first iteration; the Matlab code starts at 1 even if it doesn't do anything... //REQUIRE( cal.optimize( cb ) + 1 == md.n_iterations ); cal.optimize( cb ); - auto& z = cal.get_z_data(); - auto& p = cal.get_params(); - algo::k_to_DSM k2dsm(ci.dsm_params, ci.cal_info, ci.cal_regs, p.max_scaling_step); - - auto dsm_orig = k2dsm.apply_ac_res_on_dsm_model(ci.dsm_params, ci.cal_regs, algo::ac_to_dsm_dir::inverse); - CHECK(compare_to_bin_file< algo::algo_calibration_registers >(dsm_orig, - scene_dir, "dsmRegsOrig_2_1x4_single_00.bin")); - - k2dsm.convert_new_k_to_DSM(z.orig_intrinsics, z.new_intrinsics, z); - auto pre_process_data = k2dsm.get_pre_process_data(); - CHECK(compare_to_bin_file< uint8_t >(pre_process_data.relevant_pixels_image_rot, scene_dir, "relevantPixelsImageRot_2", z_w, z_h, "uint8_00", compare_same_vectors)); - - - CHECK(compare_to_bin_file< algo::los_shift_scaling >(pre_process_data.last_los_error, - scene_dir, "dsm_los_error_orig_2_4x1_single_00.bin")); - - CHECK(compare_to_bin_file< algo::double3 >(pre_process_data.vertices_orig, scene_dir, bin_file("verticesOrig_2", 3, md.n_relevant_pixels, "double_00") + ".bin", md.n_relevant_pixels, 1, compare_same_vectors)); - CHECK(compare_to_bin_file< algo::double2 >(pre_process_data.los_orig, scene_dir, bin_file("losOrig_2", 2, md.n_relevant_pixels, "double_00") + ".bin", md.n_relevant_pixels, 1, compare_same_vectors)); - auto new_calibration = cal.get_calibration(); auto cost = cal.get_cost(); CHECK( compare_calib_to_bin_file( new_calibration, cost, scene_dir, "new_calib", num_of_calib_elements, 1, "double_00" ) ); -#if 0 +#if 1 //-- TRACE( "\nChecking output validity:" ); // Pixel movement is OK, but some sections have negative cost diff --git a/unit-tests/algo/depth-to-rgb-calibration/scene-data.h b/unit-tests/algo/depth-to-rgb-calibration/scene-data.h index 6ba46418a7..e8755f15da 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/scene-data.h +++ b/unit-tests/algo/depth-to-rgb-calibration/scene-data.h @@ -21,7 +21,11 @@ std::string bin_file( std::string const &prefix, size_t cycle, size_t iteration, + std::to_string( h ) + "x" + std::to_string( w ) + "_" + suffix; } - +std::string bin_file(std::string const &prefix, size_t cycle, size_t w, size_t h, std::string const &suffix) +{ + return prefix + '_' + std::to_string(cycle) + '_' + + std::to_string(h) + "x" + std::to_string(w) + "_" + suffix; +} std::string bin_file( std::string const &prefix, size_t w, size_t h, std::string const &suffix ) { return prefix + "_" + std::to_string( h ) + "x" + std::to_string( w ) + "_" + suffix;