From 19dffbea754762ab44e6f572c7714758c8ea6754 Mon Sep 17 00:00:00 2001 From: aangerma Date: Mon, 25 May 2020 11:55:40 +0300 Subject: [PATCH 1/6] Added pre_processing to k-to-dsm --- .../depth-to-rgb-calibration/CMakeLists.txt | 2 + .../depth-to-rgb-calibration/frame-data.h | 4 +- .../depth-to-rgb-calibration/k-to-dsm.cpp | 239 +++++++++++++++--- src/algo/depth-to-rgb-calibration/k-to-dsm.h | 99 +++++++- .../depth-to-rgb-calibration/optimizer.cpp | 85 +------ src/algo/depth-to-rgb-calibration/optimizer.h | 2 + src/algo/depth-to-rgb-calibration/utils.cpp | 115 +++++++++ src/algo/depth-to-rgb-calibration/utils.h | 17 ++ src/depth-to-rgb-calibration.cpp | 4 + .../depth-to-rgb-calibration/compare-scene.h | 18 +- .../compare-to-bin-file.h | 19 +- .../depth-to-rgb-calibration/test-scene-2.cpp | 2 +- 12 files changed, 479 insertions(+), 127 deletions(-) create mode 100644 src/algo/depth-to-rgb-calibration/utils.cpp create mode 100644 src/algo/depth-to-rgb-calibration/utils.h diff --git a/src/algo/depth-to-rgb-calibration/CMakeLists.txt b/src/algo/depth-to-rgb-calibration/CMakeLists.txt index 01f6c6897e..12bb4f38ca 100644 --- a/src/algo/depth-to-rgb-calibration/CMakeLists.txt +++ b/src/algo/depth-to-rgb-calibration/CMakeLists.txt @@ -17,6 +17,8 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/optimizer.h" "${CMAKE_CURRENT_LIST_DIR}/k-to-dsm.cpp" "${CMAKE_CURRENT_LIST_DIR}/k-to-dsm.h" + "${CMAKE_CURRENT_LIST_DIR}/utils.cpp" + "${CMAKE_CURRENT_LIST_DIR}/utils.h" "${CMAKE_CURRENT_LIST_DIR}/rotation-in-angles.cpp" "${CMAKE_CURRENT_LIST_DIR}/valid-scene.cpp" "${CMAKE_CURRENT_LIST_DIR}/valid-results.cpp" diff --git a/src/algo/depth-to-rgb-calibration/frame-data.h b/src/algo/depth-to-rgb-calibration/frame-data.h index eb5a3dd9aa..8caa434f66 100644 --- a/src/algo/depth-to-rgb-calibration/frame-data.h +++ b/src/algo/depth-to-rgb-calibration/frame-data.h @@ -8,7 +8,7 @@ #include #include "calibration-types.h" - +#include "k-to-dsm.h" namespace librealsense { namespace algo { @@ -60,6 +60,8 @@ namespace depth_to_rgb_calibration { rs2_intrinsics_double orig_intrinsics; rs2_intrinsics_double new_intrinsics; rs2_dsm_params orig_dsm_params; + DSM_regs dsm_regs; + regs regs; float depth_units; std::vector< z_t > frame; 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 334975b48f..34f6254436 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -3,11 +3,12 @@ #include "k-to-dsm.h" #include "debug.h" +#include "utils.h" using namespace librealsense::algo::depth_to_rgb_calibration; -rs2_intrinsics_double rotate_k_mat(rs2_intrinsics_double k_mat) +rs2_intrinsics_double rotate_k_mat(const rs2_intrinsics_double& k_mat) { rs2_intrinsics_double res = k_mat; res.ppx = k_mat.width - 1 - k_mat.ppx; @@ -16,22 +17,11 @@ rs2_intrinsics_double rotate_k_mat(rs2_intrinsics_double k_mat) return res; } -rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::convert_new_k_to_DSM( - rs2_intrinsics_double old_k, - rs2_intrinsics_double new_k, - rs2_dsm_params orig_dsm_params, - DSM_regs dsm_regs -) -{ - auto old_k_raw = rotate_k_mat(old_k); - auto new_k_raw = rotate_k_mat(new_k); - - return rs2_dsm_params(); -} - -DSM_regs librealsense::algo::depth_to_rgb_calibration::apply_ac_res_on_dsm_model(rs2_dsm_params ac_data, - DSM_regs dsm_regs, - ac_to_dsm_dir type +DSM_regs librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on_dsm_model +( + const rs2_dsm_params& ac_data, + const DSM_regs& dsm_regs, + const ac_to_dsm_dir& type ) { DSM_regs res; @@ -40,15 +30,16 @@ DSM_regs librealsense::algo::depth_to_rgb_calibration::apply_ac_res_on_dsm_model { switch (ac_data.model) { - case 0: // none + case dsm_model::none: // none res = dsm_regs; - case 1: // AOT model + break; + case AOT: // AOT model res.dsm_x_scale = dsm_regs.dsm_x_scale*ac_data.h_scale; res.dsm_y_scale = dsm_regs.dsm_y_scale*ac_data.v_scale; res.dsm_x_offset = (dsm_regs.dsm_x_offset + ac_data.h_offset) / ac_data.h_scale; res.dsm_y_offset = (dsm_regs.dsm_y_offset + ac_data.v_offset) / ac_data.v_scale; break; - case 2: // TOA model + case dsm_model::TOA: // TOA model res.dsm_x_scale = dsm_regs.dsm_x_scale*ac_data.h_scale; res.dsm_y_scale = dsm_regs.dsm_y_scale*ac_data.v_scale; res.dsm_x_offset = (dsm_regs.dsm_x_offset + ac_data.h_offset) / dsm_regs.dsm_x_scale; @@ -63,24 +54,206 @@ DSM_regs librealsense::algo::depth_to_rgb_calibration::apply_ac_res_on_dsm_model { switch (ac_data.flags[0]) { - case 0: // none + case dsm_model::none: // none res = dsm_regs; - case 1: // AOT model - res.dsm_x_scale = dsm_regs.dsm_x_scale/ ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale/ac_data.v_scale; - res.dsm_x_offset = dsm_regs.dsm_x_offset* ac_data.h_scale - ac_data.h_offset; - res.dsm_y_offset = dsm_regs.dsm_y_offset* ac_data.v_scale - ac_data.v_offset; - break; - case 2: // TOA model - res.dsm_x_scale = dsm_regs.dsm_x_scale / ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale / ac_data.v_scale; - res.dsm_x_offset = dsm_regs.dsm_x_offset - ac_data.h_offset / res.dsm_x_scale; - res.dsm_y_offset = dsm_regs.dsm_y_offset - ac_data.v_offset / res.dsm_y_scale; break; + case dsm_model::AOT: // AOT model + res.dsm_x_scale = dsm_regs.dsm_x_scale / ac_data.h_scale; + res.dsm_y_scale = dsm_regs.dsm_y_scale / ac_data.v_scale; + res.dsm_x_offset = dsm_regs.dsm_x_offset* ac_data.h_scale - ac_data.h_offset; + res.dsm_y_offset = dsm_regs.dsm_y_offset* ac_data.v_scale - ac_data.v_offset; + break; + case dsm_model::TOA: // TOA model + res.dsm_x_scale = dsm_regs.dsm_x_scale / ac_data.h_scale; + res.dsm_y_scale = dsm_regs.dsm_y_scale / ac_data.v_scale; + res.dsm_x_offset = dsm_regs.dsm_x_offset - ac_data.h_offset / res.dsm_x_scale; + res.dsm_y_offset = dsm_regs.dsm_y_offset - ac_data.v_offset / res.dsm_y_scale; + break; default: throw std::runtime_error(ac_data.flags[0] + "is not valid model"); break; } } return res; -} \ No newline at end of file +} + +los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_ac_data_to_los_error +( + const DSM_regs& dsm_regs, + const rs2_dsm_params& ac_data +) +{ + los_shift_scaling res; + switch (ac_data.flags[0]) + { + case dsm_model::none: // none + res.los_scaling_x = 1; + res.los_scaling_y = 1; + res.los_shift_x = 0; + 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; + break; + case dsm_model::TOA: + res.los_scaling_x = 1 / ac_data.h_scale; + res.los_scaling_y = 1 / 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.dsm_x_scale - dsm_orig.dsm_x_offset*(1 - res.los_scaling_x); + res.los_shift_y = -ac_data.v_offset / dsm_orig.dsm_y_scale - dsm_orig.dsm_y_offset*(1 - res.los_scaling_y); + break; + } + return res; +} + +los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los +( + const regs & regs, + const DSM_regs & dsm_regs, + const std::vector& vertices +) +{ + + return los_shift_scaling(); +} + +pre_process_data librealsense::algo::depth_to_rgb_calibration::k_to_DSM::pre_processing +( + const regs & regs, + const rs2_dsm_params & ac_data, + const DSM_regs & dsm_regs, + const rs2_intrinsics_double & k_raw, + const std::vector& relevant_pixels_image +) +{ + pre_process_data res; + res.relevant_pixels_image_rot = relevant_pixels_image; + res.last_los_error = convert_ac_data_to_los_error(dsm_regs, ac_data); + + res.vertices_orig = calc_relevant_vertices(relevant_pixels_image, k_raw); + auto dsm_res_orig = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); + + return res; +} + +rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_new_k_to_DSM +( + const rs2_intrinsics_double& old_k, + const rs2_intrinsics_double& new_k, + const rs2_dsm_params& orig_dsm_params, + const DSM_regs& dsm_regs, + const regs& regs, + const std::vector& relevant_pixels_image +) +{ + auto w = old_k.width; + auto h = old_k.height; + + auto old_k_raw = rotate_k_mat(old_k); + auto new_k_raw = rotate_k_mat(new_k); + + auto dsm_orig = apply_ac_res_on_dsm_model(orig_dsm_params, dsm_regs, inverse); + + std::vector relevant_pixels_image_rot(relevant_pixels_image.size(), 0); + rotate_180(relevant_pixels_image.data(), relevant_pixels_image_rot.data(), w, h); + _pre_process_data = pre_processing(regs, orig_dsm_params, dsm_regs, old_k_raw, relevant_pixels_image_rot); + return rs2_dsm_params(); +} + +const pre_process_data& librealsense::algo::depth_to_rgb_calibration::k_to_DSM::get_pre_process_data() const +{ + return _pre_process_data; +} + +std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::calc_relevant_vertices +( + const std::vector& relevant_pixels_image, + const rs2_intrinsics_double & k +) +{ + std::vector res; + + double k_arr[9] = { k.fx, 0, 0, + 0, k.fy, 0, + k.ppx , k.ppy, 1 }; + + double k_arr_inv[9] = { 0 }; + + inv(k_arr, k_arr_inv); + + double k_t[9] = { 0 }; + + transpose(k_arr_inv, k_t); + + for (auto i = 0; i < k.height; i++) + { + for (auto j = 0; j < k.width; j++) + { + if (relevant_pixels_image[i*k.width + j]) + { + double3 ver; + double3 pix = { j, i, 1 }; + + ver.x = pix.x*k_t[0] + pix.y*k_t[1] + pix.z*k_t[2]; + ver.y = pix.x*k_t[3] + pix.y*k_t[4] + pix.z*k_t[5]; + ver.z = pix.x*k_t[6] + pix.y*k_t[7] + pix.z*k_t[8]; + + res.push_back(ver); + } + } + } + return res; +} + +los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los +( + const regs & regs, + const DSM_regs & dsm_regs, + std::vector vertices +) +{ + const int angle = 45; + los_shift_scaling res; + auto directions = transform_to_direction(vertices); + + auto fovex_indicent_direction = directions; + if (regs.frmw.fovex_existence_flag) + { + fovex_indicent_direction.resize(0); + std::vector ang_post_exp(fovex_indicent_direction.size(), 0); + for (auto i = 0; i < ang_post_exp.size(); i++) + { + ang_post_exp[i] = std::acos(fovex_indicent_direction[i].z); + } + + std::vector ang_out_on_grid(angle, 0); + + for (auto i = 0; i < angle; i++) + { + auto fovex_nominal = regs.frmw.fovex_nominal[0] + + 2 * regs.frmw.fovex_nominal[1] + + 3 * regs.frmw.fovex_nominal[2] + + 4 * regs.frmw.fovex_nominal[3]; + + ang_out_on_grid[i] = i + std::pow(i, fovex_nominal); + } + } + return res; +} + +std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::transform_to_direction(std::vector vec) +{ + std::vector res(vec.size()); + + for (auto i = 0; i < vec.size(); i++) + { + auto norm = sqrt(vec[i].x*vec[i].x + vec[i].y*vec[i].y + vec[i].z*vec[i].z); + res[i] = { vec[i].x / norm, vec[i].y / norm, vec[i].z / norm }; + } + return res; +} 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 c995419617..a264c11bd7 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -4,6 +4,7 @@ #pragma once #include "calibration.h" +#include "../../../include/librealsense2/h/rs_types.h" #include // librealsense types (intr/extr) @@ -25,12 +26,100 @@ namespace depth_to_rgb_calibration { inverse }; - DSM_regs apply_ac_res_on_dsm_model(rs2_dsm_params dsm_params, DSM_regs, ac_to_dsm_dir type); + enum dsm_model + { + none = 0, + AOT = 1, + TOA = 2 + }; + + struct los_shift_scaling + { + float los_scaling_x; + float los_scaling_y; + float los_shift_x; + float los_shift_y; + }; + + struct DEST + { + + }; - rs2_dsm_params convert_new_k_to_DSM(rs2_intrinsics_double old_k, - rs2_intrinsics_double new_k, - rs2_dsm_params orig_dsm_params, - DSM_regs dsm_regs); + struct DIGG + { + + }; + + struct EXTL + { + + }; + + struct FRMW + { + bool fovex_existence_flag = 1; + float fovex_nominal[4] = { 0.080740497, 0.0030212000, -0.00012760000, 3.5999999e-06 }; + float laserangleH = 1.0480000; + float laserangleV = -0.3659388; + double mirrorMovmentMode = 1; + float xfov[5] = { 66.056244, 66.056244, 66.056244, 66.056244, 66.056244 }; + float yfov[5] = { 56.511749, 56.511749, 56.511749, 56.511749, 56.511749 }; + float polyVars[3] = { 0, 74.501328, 0 }; + float undistAngHorz[4] = { 3.8601303, -21.056725, -12.682472, 39.103699 }; + float pitchFixFactor = 66.7550049; + }; + + struct regs + { + DEST dest; + DIGG digg; + EXTL extl; + FRMW frmw; + }; + + struct pre_process_data + { + los_shift_scaling last_los_error; + std::vector vertices_orig; + std::vector relevant_pixels_image_rot; + + }; + + class k_to_DSM + { + public: + DSM_regs apply_ac_res_on_dsm_model(const rs2_dsm_params& ac_data, const DSM_regs& regs, const ac_to_dsm_dir& type); + + los_shift_scaling convert_ac_data_to_los_error(const DSM_regs& dsm_regs, const rs2_dsm_params& ac_data); + + los_shift_scaling convert_norm_vertices_to_los(const regs& regs, + const DSM_regs& dsm_regs, + const std::vector& vertices); + + pre_process_data pre_processing(const regs& regs, + const rs2_dsm_params& ac_data, + const DSM_regs& dsm_regs, + const rs2_intrinsics_double& k_raw, + const std::vector& relevant_pixels_image); + + rs2_dsm_params convert_new_k_to_DSM(const rs2_intrinsics_double& old_k, + const rs2_intrinsics_double& new_k, + const rs2_dsm_params& orig_dsm_params, + const DSM_regs& dsm_regs, + const regs& regs, + const std::vector& relevant_pixels_image); + + const pre_process_data& get_pre_process_data() const; + + private: + std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); + los_shift_scaling convert_norm_vertices_to_los(const regs& regs, const DSM_regs& dsm_regs, std::vector vertices); + std::vector transform_to_direction(std::vector); + + pre_process_data _pre_process_data; + }; + } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index baf24d552b..5921bf7960 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -10,6 +10,7 @@ #include "uvmap.h" #include "k-to-dsm.h" #include "debug.h" +#include "utils.h" using namespace librealsense::algo::depth_to_rgb_calibration; @@ -811,87 +812,6 @@ double3x3 cholesky3x3(double3x3 mat) return res; } -void inv(const double x[9], double y[9]) -{ - double b_x[9]; - int p1; - int p2; - int p3; - double absx11; - double absx21; - double absx31; - int itmp; - memcpy(&b_x[0], &x[0], 9U * sizeof(double)); - p1 = 0; - p2 = 3; - p3 = 6; - absx11 = std::abs(x[0]); - absx21 = std::abs(x[1]); - absx31 = std::abs(x[2]); - if ((absx21 > absx11) && (absx21 > absx31)) { - p1 = 3; - p2 = 0; - b_x[0] = x[1]; - b_x[1] = x[0]; - b_x[3] = x[4]; - b_x[4] = x[3]; - b_x[6] = x[7]; - b_x[7] = x[6]; - } - else { - if (absx31 > absx11) { - p1 = 6; - p3 = 0; - b_x[0] = x[2]; - b_x[2] = x[0]; - b_x[3] = x[5]; - b_x[5] = x[3]; - b_x[6] = x[8]; - b_x[8] = x[6]; - } - } - - b_x[1] /= b_x[0]; - b_x[2] /= b_x[0]; - b_x[4] -= b_x[1] * b_x[3]; - b_x[5] -= b_x[2] * b_x[3]; - b_x[7] -= b_x[1] * b_x[6]; - b_x[8] -= b_x[2] * b_x[6]; - if (std::abs(b_x[5]) > std::abs(b_x[4])) { - itmp = p2; - p2 = p3; - p3 = itmp; - absx11 = b_x[1]; - b_x[1] = b_x[2]; - b_x[2] = absx11; - absx11 = b_x[4]; - b_x[4] = b_x[5]; - b_x[5] = absx11; - absx11 = b_x[7]; - b_x[7] = b_x[8]; - b_x[8] = absx11; - } - - b_x[5] /= b_x[4]; - b_x[8] -= b_x[5] * b_x[7]; - absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; - absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; - y[p1] = ((1.0 - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; - y[p1 + 1] = absx21; - y[p1 + 2] = absx11; - absx11 = -b_x[5] / b_x[8]; - absx21 = (1.0 - b_x[7] * absx11) / b_x[4]; - y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; - y[p2 + 1] = absx21; - y[p2 + 2] = absx11; - absx11 = 1.0 / b_x[8]; - absx21 = -b_x[7] * absx11 / b_x[4]; - y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; - y[p3 + 1] = absx21; - y[p3 + 2] = absx11; -} - - void librealsense::algo::depth_to_rgb_calibration::optimizer::decompose_p_mat() { auto p_mat = _params_curr.curr_p_mat.vals; @@ -1691,7 +1611,8 @@ size_t optimizer::optimize( std::function< void( iteration_data_collect const & } decompose_p_mat(); - convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z.orig_dsm_params, {}); + + //_k_to_DSM.convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z.orig_dsm_params, _z.dsm_regs, _z.regs, _z.relevant_pixels_image); return n_iterations; } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index 40b21e80b2..ae71523ecf 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -11,6 +11,7 @@ #include "calibration.h" #include "coeffs.h" #include "frame-data.h" +#include "k-to-dsm.h" namespace librealsense { @@ -351,6 +352,7 @@ namespace depth_to_rgb_calibration { calib _final_calibration; // starting state of auto-calibration calib _factory_calibration; // factory default calibration of the camera optimaization_params _params_curr; // last-known setting + k_to_DSM _k_to_DSM; }; } // librealsense::algo::depth_to_rgb_calibration diff --git a/src/algo/depth-to-rgb-calibration/utils.cpp b/src/algo/depth-to-rgb-calibration/utils.cpp new file mode 100644 index 0000000000..b9c78cae80 --- /dev/null +++ b/src/algo/depth-to-rgb-calibration/utils.cpp @@ -0,0 +1,115 @@ +//// License: Apache 2.0. See LICENSE file in root directory. +//// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "calibration.h" +#include "debug.h" + +#include "utils.h" + +namespace librealsense { +namespace algo { +namespace depth_to_rgb_calibration { + + void inv(const double x[9], double y[9]) + { + double b_x[9]; + int p1; + int p2; + int p3; + double absx11; + double absx21; + double absx31; + int itmp; + memcpy(&b_x[0], &x[0], 9U * sizeof(double)); + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = std::abs(x[0]); + absx21 = std::abs(x[1]); + absx31 = std::abs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } + else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + b_x[1] /= b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= b_x[1] * b_x[3]; + b_x[5] -= b_x[2] * b_x[3]; + b_x[7] -= b_x[1] * b_x[6]; + b_x[8] -= b_x[2] * b_x[6]; + if (std::abs(b_x[5]) > std::abs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + absx11 = b_x[1]; + b_x[1] = b_x[2]; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + b_x[5] /= b_x[4]; + b_x[8] -= b_x[5] * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0 - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0 - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0 / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; + } + + void transpose(const double x[9], double y[9]) + { + for (auto i = 0; i < 3; i++) + for (auto j = 0; j < 3; j++) + y[i * 3 + j] = x[j * 3 + i]; + } + + void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h) + { + int j; + int i; + + /* ROT Summary of this function goes here */ + /* Detailed explanation goes here */ + for (j = 0; j < w; j++) { + for (i = 0; i < h; i++) { + B[i + h * j] = A[(h * (w - 1 - j) - i) + h - 1]; + } + } + } +} +} +} \ No newline at end of file diff --git a/src/algo/depth-to-rgb-calibration/utils.h b/src/algo/depth-to-rgb-calibration/utils.h new file mode 100644 index 0000000000..df1ca212f6 --- /dev/null +++ b/src/algo/depth-to-rgb-calibration/utils.h @@ -0,0 +1,17 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + + +namespace librealsense { +namespace algo { +namespace depth_to_rgb_calibration { + + void inv(const double x[9], double y[9]); + void transpose(const double x[9], double y[9]); + void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h); +} +} +} + diff --git a/src/depth-to-rgb-calibration.cpp b/src/depth-to-rgb-calibration.cpp index 936461ed9a..e4a40defc6 100644 --- a/src/depth-to-rgb-calibration.cpp +++ b/src/depth-to-rgb-calibration.cpp @@ -74,6 +74,10 @@ depth_to_rgb_calibration::depth_to_rgb_calibration( } _dsm_params = cs->get_dsm_params(); + //TODO + algo::depth_to_rgb_calibration::DSM_regs dsm_regs; + algo::depth_to_rgb_calibration::regs regs; + AC_LOG( DEBUG, "... setting z data" ); auto z_profile = depth.get_profile().as< rs2::video_stream_profile >(); auto z_data = (impl::z_t const *) depth.get_data(); 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 bd49d4a4f3..eaae293e4e 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -213,10 +213,26 @@ void compare_scene( std::string const & scene_dir ) // 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 ); - auto dsm_orig = algo::apply_ac_res_on_dsm_model(dsm.dsm_params, dsm.dsm_regs, algo::ac_to_dsm_dir::inverse); + auto& z = cal.get_z_data(); + + algo::k_to_DSM k2dsm; + + auto dsm_orig = k2dsm.apply_ac_res_on_dsm_model(dsm.dsm_params, dsm.dsm_regs, algo::ac_to_dsm_dir::inverse); CHECK(compare_to_bin_file< algo::DSM_regs >(dsm_orig, scene_dir, "ac1x\\dsmRegsOrig_1x4_single_00.bin")); + k2dsm.convert_new_k_to_DSM(z.orig_intrinsics, z.new_intrinsics, dsm.dsm_params, dsm.dsm_regs, regs, z.relevant_pixels_image); + + 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, "ac1x\\relevantPixelsImageRot", 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, "ac1x\\dsm_los_error_orig_4x1_single_00.bin")); + + CHECK(compare_to_bin_file< algo::double3 >(pre_process_data.vertices_orig, scene_dir, bin_file("ac1x\\verticesOrig", 3, 2480, "double_00") + ".bin", 2480, 1, compare_same_vectors)); + + auto new_calibration = cal.get_calibration(); auto cost = cal.get_cost(); diff --git a/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h b/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h index 6831ca552d..9e2d15d00d 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h @@ -314,14 +314,25 @@ bool operator==(const algo::DSM_regs& first, const algo::DSM_regs& second) { bool ok = true; - ok &= compare_and_trace(first.dsm_x_offset, second.dsm_x_offset, "second.dsm_x_offset"); - ok &= compare_and_trace(first.dsm_y_offset, second.dsm_y_offset, "second.dsm_y_offset"); - ok &= compare_and_trace(first.dsm_x_scale, second.dsm_x_scale, "second.dsm_x_scale"); - ok &= compare_and_trace(first.dsm_y_scale, second.dsm_y_scale, "second.dsm_y_scale"); + ok &= compare_and_trace(first.dsm_x_offset, second.dsm_x_offset, "dsm_x_offset"); + ok &= compare_and_trace(first.dsm_y_offset, second.dsm_y_offset, "dsm_y_offset"); + ok &= compare_and_trace(first.dsm_x_scale, second.dsm_x_scale, "dsm_x_scale"); + ok &= compare_and_trace(first.dsm_y_scale, second.dsm_y_scale, "dsm_y_scale"); return ok; } +bool operator==(const algo::los_shift_scaling& first, const algo::los_shift_scaling& second) +{ + bool ok = true; + + ok &= compare_and_trace(first.los_scaling_x, second.los_scaling_x, "los_scaling_x"); + ok &= compare_and_trace(first.los_scaling_y, second.los_scaling_y, "los_scaling_y"); + ok &= compare_and_trace(first.los_shift_x, second.los_shift_x, "los_shift_x"); + ok &= compare_and_trace(first.los_shift_y, second.los_shift_y, "los_shift_x"); + + return ok; +} template< typename D> // F=in bin; D=in memory bool compare_to_bin_file( D const & obj_cpp, diff --git a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp index 64add5c7a7..d1698ec53f 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp +++ b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp @@ -10,7 +10,7 @@ TEST_CASE("Scene 2", "[d2rgb]") { - std::string scene_dir("..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); + std::string scene_dir("C:\\work\\librealsense\\build\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); // std::string scene_dir( "C:\\work\\autocal" ); scene_dir += "\\F9440687\\LongRange_D_768x1024_RGB_1920x1080\\2\\"; From 0545ced6da62ff42c8ff30382fa976eb7ac086c5 Mon Sep 17 00:00:00 2001 From: aangerma Date: Mon, 25 May 2020 19:32:10 +0300 Subject: [PATCH 2/6] finished pre_processing --- .../depth-to-rgb-calibration/k-to-dsm.cpp | 115 ++++++++++++++---- src/algo/depth-to-rgb-calibration/k-to-dsm.h | 8 +- src/algo/depth-to-rgb-calibration/utils.cpp | 38 +++++- src/algo/depth-to-rgb-calibration/utils.h | 6 +- .../depth-to-rgb-calibration/compare-scene.h | 4 +- .../depth-to-rgb-calibration/scene-data.h | 2 + 6 files changed, 141 insertions(+), 32 deletions(-) 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 34f6254436..f385255e46 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -111,17 +111,6 @@ los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::conver return res; } -los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los -( - const regs & regs, - const DSM_regs & dsm_regs, - const std::vector& vertices -) -{ - - return los_shift_scaling(); -} - pre_process_data librealsense::algo::depth_to_rgb_calibration::k_to_DSM::pre_processing ( const regs & regs, @@ -137,7 +126,7 @@ pre_process_data librealsense::algo::depth_to_rgb_calibration::k_to_DSM::pre_pro res.vertices_orig = calc_relevant_vertices(relevant_pixels_image, k_raw); auto dsm_res_orig = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); - + res.los_orig = convert_norm_vertices_to_los(regs, dsm_res_orig, res.vertices_orig); return res; } @@ -210,7 +199,7 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::cal return res; } -los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los +std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los ( const regs & regs, const DSM_regs & dsm_regs, @@ -218,31 +207,115 @@ los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::conver ) { const int angle = 45; - los_shift_scaling res; auto directions = transform_to_direction(vertices); auto fovex_indicent_direction = directions; if (regs.frmw.fovex_existence_flag) { - fovex_indicent_direction.resize(0); + std::fill(fovex_indicent_direction.begin(), fovex_indicent_direction.end(), double3{ 0,0,0 }); std::vector ang_post_exp(fovex_indicent_direction.size(), 0); for (auto i = 0; i < ang_post_exp.size(); i++) { - ang_post_exp[i] = std::acos(fovex_indicent_direction[i].z); + ang_post_exp[i] = std::acos(directions[i].z)* 180.0 / PI; } + std::vector ang_grid(angle, 0); std::vector ang_out_on_grid(angle, 0); for (auto i = 0; i < angle; i++) { - auto fovex_nominal = regs.frmw.fovex_nominal[0] + - 2 * regs.frmw.fovex_nominal[1] + - 3 * regs.frmw.fovex_nominal[2] + - 4 * regs.frmw.fovex_nominal[3]; + ang_grid[i] = i; + auto fovex_nominal = std::pow(i,1)* (double)regs.frmw.fovex_nominal[0] + + std::pow(i, 2)* (double)regs.frmw.fovex_nominal[1] + + std::pow(i, 3)* (double)regs.frmw.fovex_nominal[2] + + std::pow(i, 4)* (double)regs.frmw.fovex_nominal[3]; - ang_out_on_grid[i] = i + std::pow(i, fovex_nominal); + ang_out_on_grid[i] = i + fovex_nominal; + } + std::vector ang_pre_exp; + ang_pre_exp = interp1(ang_out_on_grid, ang_grid, ang_post_exp); + //std::vector xy_norm(directions.size(), 0); + for (auto i = 0; i < fovex_indicent_direction.size(); i++) + { + fovex_indicent_direction[i].z = std::cos(ang_pre_exp[i] * PI / 180.0); + auto xy_norm = directions[i].x*directions[i].x + directions[i].y*directions[i].y; + auto xy_factor = sqrt((1 - (fovex_indicent_direction[i].z* fovex_indicent_direction[i].z)) / xy_norm); + fovex_indicent_direction[i].x = directions[i].x*xy_factor; + fovex_indicent_direction[i].y = directions[i].y*xy_factor; } } + + auto x = (double)regs.frmw.laserangleH* PI / 180.0; + auto y = (double)(regs.frmw.laserangleV + 180)* PI / 180.0; + + double laser_incident_direction[3] = { std::cos(y)*std::sin(x), + std::sin(y), + std::cos(y)*std::cos(x) }; + + std::vector mirror_normal_direction(fovex_indicent_direction.size()); + std::vector dsm_x_corr(fovex_indicent_direction.size()); + std::vector dsm_y_corr(fovex_indicent_direction.size()); + for (auto i = 0; i < fovex_indicent_direction.size(); i++) + { + mirror_normal_direction[i].x = fovex_indicent_direction[i].x - laser_incident_direction[0]; + mirror_normal_direction[i].y = fovex_indicent_direction[i].y - laser_incident_direction[1]; + mirror_normal_direction[i].z = fovex_indicent_direction[i].z - laser_incident_direction[2]; + + auto norm = sqrt(mirror_normal_direction[i].x*mirror_normal_direction[i].x + + mirror_normal_direction[i].y* mirror_normal_direction[i].y + + mirror_normal_direction[i].z* mirror_normal_direction[i].z); + + mirror_normal_direction[i].x /= norm; + mirror_normal_direction[i].y /= norm; + mirror_normal_direction[i].z /= norm; + + auto ang_x = std::atan(mirror_normal_direction[i].x / mirror_normal_direction[i].z)* 180.0 / PI; + auto ang_y = std::asin(mirror_normal_direction[i].y)* 180.0 / PI; + + int mirror_mode = regs.frmw.mirrorMovmentMode; + + dsm_x_corr[i] = ang_x / (double)(regs.frmw.xfov[mirror_mode - 1] * 0.25 / 2047); + dsm_y_corr[i] = ang_y / (double)(regs.frmw.yfov[mirror_mode - 1] * 0.25 / 2047); + + } + + std::vector dsm_grid(421); + + auto ind = 0; + for (auto i = -2100; i <= 2100; i += 10) + { + dsm_grid[ind++] = i; + } + + std::vector dsm_x_coarse_on_grid(dsm_grid.size()); + std::vector dsm_x_corr_on_grid(dsm_grid.size()); + for (auto i = 0; i < dsm_grid.size(); i ++) + { + double vals[3] = { std::pow((dsm_grid[i] / 2047), 1), std::pow((dsm_grid[i] / 2047), 2), std::pow((dsm_grid[i] / 2047), 3) }; + dsm_x_coarse_on_grid[i] = dsm_grid[i] + vals[0] * (double)regs.frmw.polyVars[0] + vals[1] * (double)regs.frmw.polyVars[1] + vals[2] * (double)regs.frmw.polyVars[2]; + auto val = dsm_x_coarse_on_grid[i] / 2047; + double vals2[4] = { std::pow(val , 1),std::pow(val , 2), std::pow(val , 3) , std::pow(val , 4) }; + dsm_x_corr_on_grid[i] = dsm_x_coarse_on_grid[i] + vals2[0] * (double)regs.frmw.undistAngHorz[0] + + vals2[1] * (double)regs.frmw.undistAngHorz[1] + + vals2[2] * (double)regs.frmw.undistAngHorz[2] + + vals2[3] * (double)regs.frmw.undistAngHorz[3]; + } + + auto dsm_x = interp1(dsm_x_corr_on_grid, dsm_grid, dsm_x_corr); + std::vector dsm_y(dsm_x.size()); + + for (auto i = 0; i < dsm_y.size(); i++) + { + dsm_y[i] = dsm_y_corr[i] - (dsm_x[i] / 2047)*(double)regs.frmw.pitchFixFactor; + } + + std::vector res(dsm_x.size()); + + for (auto i = 0; i < res.size(); i++) + { + res[i].x = (dsm_x[i] + 2047) / (double)dsm_regs.dsm_x_scale - (double)dsm_regs.dsm_x_offset; + res[i].y = (dsm_y[i] + 2047) / (double)dsm_regs.dsm_y_scale - (double)dsm_regs.dsm_y_offset; + } return res; } 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 a264c11bd7..c5b4826b09 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -7,7 +7,6 @@ #include "../../../include/librealsense2/h/rs_types.h" #include // librealsense types (intr/extr) - namespace librealsense { namespace algo { namespace depth_to_rgb_calibration { @@ -83,6 +82,7 @@ namespace depth_to_rgb_calibration { los_shift_scaling last_los_error; std::vector vertices_orig; std::vector relevant_pixels_image_rot; + std::vector los_orig; }; @@ -93,10 +93,6 @@ namespace depth_to_rgb_calibration { los_shift_scaling convert_ac_data_to_los_error(const DSM_regs& dsm_regs, const rs2_dsm_params& ac_data); - los_shift_scaling convert_norm_vertices_to_los(const regs& regs, - const DSM_regs& dsm_regs, - const std::vector& vertices); - pre_process_data pre_processing(const regs& regs, const rs2_dsm_params& ac_data, const DSM_regs& dsm_regs, @@ -114,7 +110,7 @@ namespace depth_to_rgb_calibration { private: std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); - los_shift_scaling convert_norm_vertices_to_los(const regs& regs, const DSM_regs& dsm_regs, std::vector vertices); + std::vector convert_norm_vertices_to_los(const regs& regs, const DSM_regs& dsm_regs, std::vector vertices); std::vector transform_to_direction(std::vector); pre_process_data _pre_process_data; diff --git a/src/algo/depth-to-rgb-calibration/utils.cpp b/src/algo/depth-to-rgb-calibration/utils.cpp index b9c78cae80..d1b1669ea5 100644 --- a/src/algo/depth-to-rgb-calibration/utils.cpp +++ b/src/algo/depth-to-rgb-calibration/utils.cpp @@ -3,7 +3,6 @@ #include "calibration.h" #include "debug.h" - #include "utils.h" namespace librealsense { @@ -110,6 +109,43 @@ namespace depth_to_rgb_calibration { } } } + const std::vector interp1(const std::vector ind, const std::vector vals, const std::vector intrp) + { + std::vector res(intrp.size(), 0); + + for (auto i = 0; i < intrp.size(); i++) + { + auto value = intrp[i]; + auto it = std::lower_bound(ind.begin(), ind.end(), value); + if (it == ind.begin()) + { + if (*it == ind.front()) + res[i] = ind.front(); + else + res[i] = std::numeric_limits::max(); + } + else if (it == ind.end()) + { + if (*it == ind.back()) + res[i] = ind.back(); + else + res[i] = std::numeric_limits::max(); + } + else + { + auto val1 = *(--it); + auto ind1 = std::distance(ind.begin(), it); + auto val2= *(++it); + auto ind2 = std::distance(ind.begin(), it); + + auto target_val1 = vals[ind1]; + auto target_val2 = vals[ind2]; + + res[i] = ((val2 - value) / (val2 - val1))*target_val1 + ((value - val1) / (val2 - val1))*target_val2; + } + } + return res; + } } } } \ No newline at end of file diff --git a/src/algo/depth-to-rgb-calibration/utils.h b/src/algo/depth-to-rgb-calibration/utils.h index df1ca212f6..ae40e55015 100644 --- a/src/algo/depth-to-rgb-calibration/utils.h +++ b/src/algo/depth-to-rgb-calibration/utils.h @@ -3,14 +3,16 @@ #pragma once +#define PI 3.14159265359 namespace librealsense { namespace algo { namespace depth_to_rgb_calibration { - void inv(const double x[9], double y[9]); + void inv(const double x[9], double y[9]); //generated code void transpose(const double x[9], double y[9]); - void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h); + void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h); //generated code + const std::vector interp1(const std::vector ind, const std::vector vals, const std::vector intrp); } } } 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 eaae293e4e..490cd1050d 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -230,8 +230,8 @@ void compare_scene( std::string const & scene_dir ) CHECK(compare_to_bin_file< algo::los_shift_scaling >(pre_process_data.last_los_error, scene_dir, "ac1x\\dsm_los_error_orig_4x1_single_00.bin")); - CHECK(compare_to_bin_file< algo::double3 >(pre_process_data.vertices_orig, scene_dir, bin_file("ac1x\\verticesOrig", 3, 2480, "double_00") + ".bin", 2480, 1, compare_same_vectors)); - + CHECK(compare_to_bin_file< algo::double3 >(pre_process_data.vertices_orig, scene_dir, bin_file("ac1x\\verticesOrig", 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("ac1x\\losOrig", 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(); 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 883c48852f..66eeec8045 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/scene-data.h +++ b/unit-tests/algo/depth-to-rgb-calibration/scene-data.h @@ -126,6 +126,7 @@ struct scene_metadata double correction_in_pixels; // XY movement uint64_t n_edges; // strong edges, i.e. after suppression uint64_t n_valid_pixels; + uint64_t n_relevant_pixels; uint64_t n_valid_ir_edges; bool is_scene_valid; bool is_output_valid; @@ -149,6 +150,7 @@ struct scene_metadata f.read( (char *)&n_edges, sizeof( n_edges ) ); f.read( (char *)&n_valid_ir_edges, sizeof( n_valid_ir_edges ) ); f.read( (char *)&n_valid_pixels, sizeof( n_valid_pixels ) ); + f.read((char *)&n_relevant_pixels, sizeof(n_relevant_pixels)); f.read( (char *)&n_iterations, sizeof( n_iterations ) ); byte b; f.read( (char *)&b, 1 ); From a7965963e679ffc1a2a4f4f42c7b0e5287234fcb Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 26 May 2020 11:23:35 +0300 Subject: [PATCH 3/6] Replaced dsm structures. --- .../depth-to-rgb-calibration/frame-data.h | 4 +- .../depth-to-rgb-calibration/k-to-dsm.cpp | 116 +++++++------ src/algo/depth-to-rgb-calibration/k-to-dsm.h | 156 ++++++++++++------ .../depth-to-rgb-calibration/optimizer.cpp | 4 +- src/algo/depth-to-rgb-calibration/optimizer.h | 98 +---------- src/depth-to-rgb-calibration.cpp | 4 - .../depth-to-rgb-calibration/compare-scene.h | 11 +- .../compare-to-bin-file.h | 10 +- .../depth-to-rgb-calibration/scene-data.h | 66 +++++++- 9 files changed, 249 insertions(+), 220 deletions(-) diff --git a/src/algo/depth-to-rgb-calibration/frame-data.h b/src/algo/depth-to-rgb-calibration/frame-data.h index 8caa434f66..c16c0ed712 100644 --- a/src/algo/depth-to-rgb-calibration/frame-data.h +++ b/src/algo/depth-to-rgb-calibration/frame-data.h @@ -60,8 +60,8 @@ namespace depth_to_rgb_calibration { rs2_intrinsics_double orig_intrinsics; rs2_intrinsics_double new_intrinsics; rs2_dsm_params orig_dsm_params; - DSM_regs dsm_regs; - regs regs; + /*algo_calibration_registers algo_calibration_registers; + regs regs;*/ float depth_units; std::vector< z_t > frame; 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 f385255e46..3af5780f39 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -17,14 +17,25 @@ rs2_intrinsics_double rotate_k_mat(const rs2_intrinsics_double& k_mat) return res; } -DSM_regs librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on_dsm_model +librealsense::algo::depth_to_rgb_calibration::k_to_DSM::k_to_DSM +( + const rs2_dsm_params & 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) + :_orig_dsm_params(orig_dsm_params), + _cal_info(cal_info), + _cal_regs(cal_regs) +{ +} + +algo_calibration_registers librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on_dsm_model ( const rs2_dsm_params& ac_data, - const DSM_regs& dsm_regs, + const algo_calibration_registers& dsm_regs, const ac_to_dsm_dir& type ) { - DSM_regs res; + algo_calibration_registers res; if (type == direct) // convert from original model to modified model { @@ -34,16 +45,16 @@ DSM_regs librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on res = dsm_regs; break; case AOT: // AOT model - res.dsm_x_scale = dsm_regs.dsm_x_scale*ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale*ac_data.v_scale; - res.dsm_x_offset = (dsm_regs.dsm_x_offset + ac_data.h_offset) / ac_data.h_scale; - res.dsm_y_offset = (dsm_regs.dsm_y_offset + ac_data.v_offset) / ac_data.v_scale; + 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) / ac_data.h_scale; + res.EXTLdsmYoffset = (dsm_regs.EXTLdsmYoffset + ac_data.v_offset) / ac_data.v_scale; break; case dsm_model::TOA: // TOA model - res.dsm_x_scale = dsm_regs.dsm_x_scale*ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale*ac_data.v_scale; - res.dsm_x_offset = (dsm_regs.dsm_x_offset + ac_data.h_offset) / dsm_regs.dsm_x_scale; - res.dsm_y_offset = (dsm_regs.dsm_y_offset + ac_data.v_offset) / dsm_regs.dsm_y_scale; + 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) / dsm_regs.EXTLdsmXscale; + res.EXTLdsmYoffset = (dsm_regs.EXTLdsmYoffset + ac_data.v_offset) / dsm_regs.EXTLdsmYscale; break; default: throw std::runtime_error(ac_data.flags[0] + "is not valid model"); @@ -58,16 +69,16 @@ DSM_regs librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on res = dsm_regs; break; case dsm_model::AOT: // AOT model - res.dsm_x_scale = dsm_regs.dsm_x_scale / ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale / ac_data.v_scale; - res.dsm_x_offset = dsm_regs.dsm_x_offset* ac_data.h_scale - ac_data.h_offset; - res.dsm_y_offset = dsm_regs.dsm_y_offset* ac_data.v_scale - ac_data.v_offset; + 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; break; case dsm_model::TOA: // TOA model - res.dsm_x_scale = dsm_regs.dsm_x_scale / ac_data.h_scale; - res.dsm_y_scale = dsm_regs.dsm_y_scale / ac_data.v_scale; - res.dsm_x_offset = dsm_regs.dsm_x_offset - ac_data.h_offset / res.dsm_x_scale; - res.dsm_y_offset = dsm_regs.dsm_y_offset - ac_data.v_offset / res.dsm_y_scale; + 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; break; default: throw std::runtime_error(ac_data.flags[0] + "is not valid model"); @@ -79,7 +90,7 @@ DSM_regs librealsense::algo::depth_to_rgb_calibration::k_to_DSM::apply_ac_res_on los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_ac_data_to_los_error ( - const DSM_regs& dsm_regs, + const algo_calibration_registers& algo_calibration_registers, const rs2_dsm_params& ac_data ) { @@ -102,10 +113,10 @@ los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::conver res.los_scaling_x = 1 / ac_data.h_scale; res.los_scaling_y = 1 / ac_data.v_scale; - auto dsm_orig = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); + auto dsm_orig = apply_ac_res_on_dsm_model(ac_data, algo_calibration_registers, inverse); - res.los_shift_x = -ac_data.h_offset / dsm_orig.dsm_x_scale - dsm_orig.dsm_x_offset*(1 - res.los_scaling_x); - res.los_shift_y = -ac_data.v_offset / dsm_orig.dsm_y_scale - dsm_orig.dsm_y_offset*(1 - res.los_scaling_y); + 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); break; } return res; @@ -113,19 +124,19 @@ los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::conver pre_process_data librealsense::algo::depth_to_rgb_calibration::k_to_DSM::pre_processing ( - const regs & regs, - const rs2_dsm_params & ac_data, - const DSM_regs & dsm_regs, - const rs2_intrinsics_double & k_raw, + const algo_calibration_info& regs, + const rs2_dsm_params& ac_data, + const algo_calibration_registers& algo_calibration_registers, + const rs2_intrinsics_double& k_raw, const std::vector& relevant_pixels_image ) { pre_process_data res; res.relevant_pixels_image_rot = relevant_pixels_image; - res.last_los_error = convert_ac_data_to_los_error(dsm_regs, ac_data); + res.last_los_error = convert_ac_data_to_los_error(algo_calibration_registers, ac_data); res.vertices_orig = calc_relevant_vertices(relevant_pixels_image, k_raw); - auto dsm_res_orig = apply_ac_res_on_dsm_model(ac_data, dsm_regs, inverse); + auto dsm_res_orig = apply_ac_res_on_dsm_model(ac_data, algo_calibration_registers, inverse); res.los_orig = convert_norm_vertices_to_los(regs, dsm_res_orig, res.vertices_orig); return res; } @@ -134,9 +145,6 @@ rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_n ( const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const rs2_dsm_params& orig_dsm_params, - const DSM_regs& dsm_regs, - const regs& regs, const std::vector& relevant_pixels_image ) { @@ -146,11 +154,11 @@ rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_n auto old_k_raw = rotate_k_mat(old_k); auto new_k_raw = rotate_k_mat(new_k); - auto dsm_orig = apply_ac_res_on_dsm_model(orig_dsm_params, dsm_regs, inverse); + auto dsm_orig = apply_ac_res_on_dsm_model(_orig_dsm_params, _cal_regs, inverse); std::vector relevant_pixels_image_rot(relevant_pixels_image.size(), 0); rotate_180(relevant_pixels_image.data(), relevant_pixels_image_rot.data(), w, h); - _pre_process_data = pre_processing(regs, orig_dsm_params, dsm_regs, old_k_raw, relevant_pixels_image_rot); + _pre_process_data = pre_processing(_cal_info, _orig_dsm_params, _cal_regs, old_k_raw, relevant_pixels_image_rot); return rs2_dsm_params(); } @@ -201,8 +209,8 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::cal std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los ( - const regs & regs, - const DSM_regs & dsm_regs, + const algo_calibration_info& regs, + const algo_calibration_registers& algo_calibration_registers, std::vector vertices ) { @@ -210,7 +218,7 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con auto directions = transform_to_direction(vertices); auto fovex_indicent_direction = directions; - if (regs.frmw.fovex_existence_flag) + if (regs.FRMWfovexExistenceFlag) { std::fill(fovex_indicent_direction.begin(), fovex_indicent_direction.end(), double3{ 0,0,0 }); std::vector ang_post_exp(fovex_indicent_direction.size(), 0); @@ -225,10 +233,10 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con for (auto i = 0; i < angle; i++) { ang_grid[i] = i; - auto fovex_nominal = std::pow(i,1)* (double)regs.frmw.fovex_nominal[0] + - std::pow(i, 2)* (double)regs.frmw.fovex_nominal[1] + - std::pow(i, 3)* (double)regs.frmw.fovex_nominal[2] + - std::pow(i, 4)* (double)regs.frmw.fovex_nominal[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; } @@ -245,8 +253,8 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con } } - auto x = (double)regs.frmw.laserangleH* PI / 180.0; - auto y = (double)(regs.frmw.laserangleV + 180)* PI / 180.0; + auto x = (double)regs.FRMWlaserangleH* PI / 180.0; + auto y = (double)(regs.FRMWlaserangleV + 180)* PI / 180.0; double laser_incident_direction[3] = { std::cos(y)*std::sin(x), std::sin(y), @@ -272,10 +280,10 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con auto ang_x = std::atan(mirror_normal_direction[i].x / mirror_normal_direction[i].z)* 180.0 / PI; auto ang_y = std::asin(mirror_normal_direction[i].y)* 180.0 / PI; - int mirror_mode = regs.frmw.mirrorMovmentMode; + int mirror_mode = 1/*regs.frmw.mirrorMovmentMode*/; - dsm_x_corr[i] = ang_x / (double)(regs.frmw.xfov[mirror_mode - 1] * 0.25 / 2047); - dsm_y_corr[i] = ang_y / (double)(regs.frmw.yfov[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); } @@ -292,13 +300,13 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con for (auto i = 0; i < dsm_grid.size(); i ++) { double vals[3] = { std::pow((dsm_grid[i] / 2047), 1), std::pow((dsm_grid[i] / 2047), 2), std::pow((dsm_grid[i] / 2047), 3) }; - dsm_x_coarse_on_grid[i] = dsm_grid[i] + vals[0] * (double)regs.frmw.polyVars[0] + vals[1] * (double)regs.frmw.polyVars[1] + vals[2] * (double)regs.frmw.polyVars[2]; + dsm_x_coarse_on_grid[i] = dsm_grid[i] + vals[0] * (double)regs.FRMWpolyVars[0] + vals[1] * (double)regs.FRMWpolyVars[1] + vals[2] * (double)regs.FRMWpolyVars[2]; auto val = dsm_x_coarse_on_grid[i] / 2047; double vals2[4] = { std::pow(val , 1),std::pow(val , 2), std::pow(val , 3) , std::pow(val , 4) }; - dsm_x_corr_on_grid[i] = dsm_x_coarse_on_grid[i] + vals2[0] * (double)regs.frmw.undistAngHorz[0] + - vals2[1] * (double)regs.frmw.undistAngHorz[1] + - vals2[2] * (double)regs.frmw.undistAngHorz[2] + - vals2[3] * (double)regs.frmw.undistAngHorz[3]; + dsm_x_corr_on_grid[i] = dsm_x_coarse_on_grid[i] + vals2[0] * (double)regs.FRMWundistAngHorz[0] + + vals2[1] * (double)regs.FRMWundistAngHorz[1] + + vals2[2] * (double)regs.FRMWundistAngHorz[2] + + vals2[3] * (double)regs.FRMWundistAngHorz[3]; } auto dsm_x = interp1(dsm_x_corr_on_grid, dsm_grid, dsm_x_corr); @@ -306,15 +314,15 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con for (auto i = 0; i < dsm_y.size(); i++) { - dsm_y[i] = dsm_y_corr[i] - (dsm_x[i] / 2047)*(double)regs.frmw.pitchFixFactor; + dsm_y[i] = dsm_y_corr[i] - (dsm_x[i] / 2047)*(double)regs.FRMWpitchFixFactor; } std::vector res(dsm_x.size()); for (auto i = 0; i < res.size(); i++) { - res[i].x = (dsm_x[i] + 2047) / (double)dsm_regs.dsm_x_scale - (double)dsm_regs.dsm_x_offset; - res[i].y = (dsm_y[i] + 2047) / (double)dsm_regs.dsm_y_scale - (double)dsm_regs.dsm_y_offset; + res[i].x = (dsm_x[i] + 2047) / (double)algo_calibration_registers.EXTLdsmXscale - (double)algo_calibration_registers.EXTLdsmXoffset; + res[i].y = (dsm_y[i] + 2047) / (double)algo_calibration_registers.EXTLdsmYscale - (double)algo_calibration_registers.EXTLdsmYoffset; } return res; } 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 c5b4826b09..af0933347f 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -5,20 +5,13 @@ #include "calibration.h" #include "../../../include/librealsense2/h/rs_types.h" + #include // librealsense types (intr/extr) namespace librealsense { namespace algo { namespace depth_to_rgb_calibration { - struct DSM_regs - { - float dsm_x_scale; - float dsm_y_scale; - float dsm_x_offset; - float dsm_y_offset; - }; - enum ac_to_dsm_dir { direct, @@ -40,42 +33,97 @@ namespace depth_to_rgb_calibration { float los_shift_y; }; - struct DEST - { - - }; - - struct DIGG - { - - }; - - struct EXTL - { - - }; - - struct FRMW +#pragma pack(push, 1) + // This table is read from FW and is used in the optimizer + struct algo_calibration_info { - bool fovex_existence_flag = 1; - float fovex_nominal[4] = { 0.080740497, 0.0030212000, -0.00012760000, 3.5999999e-06 }; - float laserangleH = 1.0480000; - float laserangleV = -0.3659388; - double mirrorMovmentMode = 1; - float xfov[5] = { 66.056244, 66.056244, 66.056244, 66.056244, 66.056244 }; - float yfov[5] = { 56.511749, 56.511749, 56.511749, 56.511749, 56.511749 }; - float polyVars[3] = { 0, 74.501328, 0 }; - float undistAngHorz[4] = { 3.8601303, -21.056725, -12.682472, 39.103699 }; - float pitchFixFactor = 66.7550049; + static const int table_id = 0x313; + + uint16_t version; // = 0x0100 + uint16_t id; // = table_id + uint32_t size; // = 0x1F0 + uint32_t full_version; // = 0xFFFFFFFF + uint32_t crc32; // of the following data: + uint32_t DIGGundistFx; + uint32_t DIGGundistFy; + int32_t DIGGundistX0; + int32_t DIGGundistY0; + uint8_t DESThbaseline; + float DESTbaseline; + float FRMWxfov[5]; + float FRMWyfov[5]; + float FRMWprojectionYshear[5]; + float FRMWlaserangleH; + float FRMWlaserangleV; + uint16_t FRMWcalMarginL; + uint16_t FRMWcalMarginR; + uint16_t FRMWcalMarginT; + uint16_t FRMWcalMarginB; + uint8_t FRMWxR2L; + uint8_t FRMWyflip; + float EXTLdsmXscale; + float EXTLdsmYscale; + float EXTLdsmXoffset; + float EXTLdsmYoffset; + uint32_t EXTLconLocDelaySlow; + uint32_t EXTLconLocDelayFastC; + uint32_t EXTLconLocDelayFastF; + uint16_t FRMWcalImgHsize; + uint16_t FRMWcalImgVsize; + float FRMWpolyVars[3]; + float FRMWpitchFixFactor; + uint32_t FRMWzoRawCol[5]; + uint32_t FRMWzoRawRow[5]; + float FRMWdfzCalTmp; + float FRMWdfzVbias[3]; + float FRMWdfzIbias[3]; + float FRMWdfzApdCalTmp; + float FRMWatlMinVbias1; + float FRMWatlMaxVbias1; + float FRMWatlMinVbias2; + float FRMWatlMaxVbias2; + float FRMWatlMinVbias3; + float FRMWatlMaxVbias3; + float FRMWundistAngHorz[4]; + float FRMWundistAngVert[4]; + uint8_t FRMWfovexExistenceFlag; + float FRMWfovexNominal[4]; + uint8_t FRMWfovexLensDistFlag; + float FRMWfovexRadialK[3]; + float FRMWfovexTangentP[2]; + float FRMWfovexCenter[2]; + uint32_t FRMWcalibVersion; + uint32_t FRMWconfigVersion; + uint32_t FRMWeepromVersion; + float FRMWconLocDelaySlowSlope; + float FRMWconLocDelayFastSlope; + int16_t FRMWatlMinAngXL; + int16_t FRMWatlMinAngXR; + int16_t FRMWatlMaxAngXL; + int16_t FRMWatlMaxAngXR; + int16_t FRMWatlMinAngYU; + int16_t FRMWatlMinAngYB; + int16_t FRMWatlMaxAngYU; + int16_t FRMWatlMaxAngYB; + float FRMWatlSlopeMA; + float FRMWatlMaCalTmp; + uint8_t FRMWvddVoltValues[3]; // this one is really 2 uint12 values, or 24 bits total; not in use! + int16_t FRMWvdd2RtdDiff; + int16_t FRMWvdd3RtdDiff; + int16_t FRMWvdd4RtdDiff; + float FRMWdfzCalibrationLddTemp; + float FRMWdfzCalibrationVddVal; + float FRMWhumidApdTempDiff; + uint8_t reserved[94]; }; - - struct regs + struct algo_calibration_registers { - DEST dest; - DIGG digg; - EXTL extl; - FRMW frmw; + float EXTLdsmXscale; + float EXTLdsmYscale; + float EXTLdsmXoffset; + float EXTLdsmYoffset; }; +#pragma pack(pop) struct pre_process_data { @@ -89,31 +137,41 @@ namespace depth_to_rgb_calibration { class k_to_DSM { public: - DSM_regs apply_ac_res_on_dsm_model(const rs2_dsm_params& ac_data, const DSM_regs& regs, const ac_to_dsm_dir& type); + k_to_DSM(const rs2_dsm_params& 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); + + 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); - los_shift_scaling convert_ac_data_to_los_error(const DSM_regs& dsm_regs, 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& ac_data); - pre_process_data pre_processing(const regs& regs, + pre_process_data pre_processing(const algo_calibration_info& regs, const rs2_dsm_params& ac_data, - const DSM_regs& dsm_regs, + 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(const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const rs2_dsm_params& orig_dsm_params, - const DSM_regs& dsm_regs, - const regs& regs, const std::vector& relevant_pixels_image); const pre_process_data& get_pre_process_data() const; private: - std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); - std::vector convert_norm_vertices_to_los(const regs& regs, const DSM_regs& dsm_regs, std::vector vertices); + std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, + const rs2_intrinsics_double& k); + + std::vector convert_norm_vertices_to_los(const algo_calibration_info& regs, + const algo_calibration_registers& algo_calibration_registers, + std::vector vertices); + std::vector transform_to_direction(std::vector); pre_process_data _pre_process_data; + + rs2_dsm_params _orig_dsm_params; + algo::depth_to_rgb_calibration::algo_calibration_info _cal_info; + algo::depth_to_rgb_calibration::algo_calibration_registers _cal_regs; }; diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index 5921bf7960..69d94902e1 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -395,6 +395,8 @@ void optimizer::set_z_data( std::vector< z_t > && depth_data, algo_calibration_info const & cal_info, algo_calibration_registers const & cal_regs, float depth_units ) { + _k_to_DSM = std::make_shared(dsm_params, cal_info, cal_regs); + /*[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; */ @@ -1612,7 +1614,7 @@ size_t optimizer::optimize( std::function< void( iteration_data_collect const & decompose_p_mat(); - //_k_to_DSM.convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z.orig_dsm_params, _z.dsm_regs, _z.regs, _z.relevant_pixels_image); + //_k_to_DSM.convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z.orig_dsm_params, _z.algo_calibration_registers, _z.regs, _z.relevant_pixels_image); return n_iterations; } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index ae71523ecf..1af1194f13 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -162,102 +162,6 @@ namespace depth_to_rgb_calibration { optimaization_params next_params; }; -#pragma pack(push, 1) - // This table is read from FW and is used in the optimizer - struct algo_calibration_info - { - static const int table_id = 0x313; - - uint16_t version; // = 0x0100 - uint16_t id; // = table_id - uint32_t size; // = 0x1F0 - uint32_t full_version; // = 0xFFFFFFFF - uint32_t crc32; // of the following data: - uint32_t DIGGundistFx; - uint32_t DIGGundistFy; - int32_t DIGGundistX0; - int32_t DIGGundistY0; - uint8_t DESThbaseline; - float DESTbaseline; - float FRMWxfov[5]; - float FRMWyfov[5]; - float FRMWprojectionYshear[5]; - float FRMWlaserangleH; - float FRMWlaserangleV; - uint16_t FRMWcalMarginL; - uint16_t FRMWcalMarginR; - uint16_t FRMWcalMarginT; - uint16_t FRMWcalMarginB; - uint8_t FRMWxR2L; - uint8_t FRMWyflip; - float EXTLdsmXscale; - float EXTLdsmYscale; - float EXTLdsmXoffset; - float EXTLdsmYoffset; - uint32_t EXTLconLocDelaySlow; - uint32_t EXTLconLocDelayFastC; - uint32_t EXTLconLocDelayFastF; - uint16_t FRMWcalImgHsize; - uint16_t FRMWcalImgVsize; - float FRMWpolyVars[3]; - float FRMWpitchFixFactor; - uint32_t FRMWzoRawCol[5]; - uint32_t FRMWzoRawRow[5]; - float FRMWdfzCalTmp; - float FRMWdfzVbias[3]; - float FRMWdfzIbias[3]; - float FRMWdfzApdCalTmp; - float FRMWatlMinVbias1; - float FRMWatlMaxVbias1; - float FRMWatlMinVbias2; - float FRMWatlMaxVbias2; - float FRMWatlMinVbias3; - float FRMWatlMaxVbias3; - float FRMWundistAngHorz[4]; - float FRMWundistAngVert[4]; - uint8_t FRMWfovexExistenceFlag; - float FRMWfovexNominal[4]; - uint8_t FRMWfovexLensDistFlag; - float FRMWfovexRadialK[3]; - float FRMWfovexTangentP[2]; - float FRMWfovexCenter[2]; - uint32_t FRMWcalibVersion; - uint32_t FRMWconfigVersion; - uint32_t FRMWeepromVersion; - float FRMWconLocDelaySlowSlope; - float FRMWconLocDelayFastSlope; - int16_t FRMWatlMinAngXL; - int16_t FRMWatlMinAngXR; - int16_t FRMWatlMaxAngXL; - int16_t FRMWatlMaxAngXR; - int16_t FRMWatlMinAngYU; - int16_t FRMWatlMinAngYB; - int16_t FRMWatlMaxAngYU; - int16_t FRMWatlMaxAngYB; - float FRMWatlSlopeMA; - float FRMWatlMaCalTmp; - uint8_t FRMWvddVoltValues[3]; // this one is really 2 uint12 values, or 24 bits total; not in use! - int16_t FRMWvdd2RtdDiff; - int16_t FRMWvdd3RtdDiff; - int16_t FRMWvdd4RtdDiff; - float FRMWdfzCalibrationLddTemp; - float FRMWdfzCalibrationVddVal; - float FRMWhumidApdTempDiff; - uint8_t reserved[94]; - - algo_calibration_info() - { - memset( this, 0, sizeof( algo_calibration_info ) ); - } - }; - struct algo_calibration_registers - { - float EXTLdsmXscale; - float EXTLdsmYscale; - float EXTLdsmXoffset; - float EXTLdsmYoffset; - }; -#pragma pack(pop) class optimizer { @@ -352,7 +256,7 @@ namespace depth_to_rgb_calibration { calib _final_calibration; // starting state of auto-calibration calib _factory_calibration; // factory default calibration of the camera optimaization_params _params_curr; // last-known setting - k_to_DSM _k_to_DSM; + std::shared_ptr _k_to_DSM; }; } // librealsense::algo::depth_to_rgb_calibration diff --git a/src/depth-to-rgb-calibration.cpp b/src/depth-to-rgb-calibration.cpp index e4a40defc6..936461ed9a 100644 --- a/src/depth-to-rgb-calibration.cpp +++ b/src/depth-to-rgb-calibration.cpp @@ -74,10 +74,6 @@ depth_to_rgb_calibration::depth_to_rgb_calibration( } _dsm_params = cs->get_dsm_params(); - //TODO - algo::depth_to_rgb_calibration::DSM_regs dsm_regs; - algo::depth_to_rgb_calibration::regs regs; - AC_LOG( DEBUG, "... setting z data" ); auto z_profile = depth.get_profile().as< rs2::video_stream_profile >(); auto z_data = (impl::z_t const *) depth.get_data(); 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 490cd1050d..ba7d7b3803 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -9,6 +9,9 @@ void compare_scene( std::string const & scene_dir ) camera_params ci = read_camera_params( scene_dir, "ac1x\\camera_params" ); dsm_params dsm = read_dsm_params(scene_dir, "ac1x\\DSM_params"); ci.dsm_params = dsm.dsm_params; + ci.cal_info = dsm.regs; + ci.cal_regs = dsm.algo_calibration_registers; + scene_metadata md( scene_dir ); algo::optimizer cal; @@ -215,13 +218,13 @@ void compare_scene( std::string const & scene_dir ) auto& z = cal.get_z_data(); - algo::k_to_DSM k2dsm; + algo::k_to_DSM k2dsm(ci.dsm_params, ci.cal_info, ci.cal_regs); - auto dsm_orig = k2dsm.apply_ac_res_on_dsm_model(dsm.dsm_params, dsm.dsm_regs, algo::ac_to_dsm_dir::inverse); - CHECK(compare_to_bin_file< algo::DSM_regs >(dsm_orig, + auto dsm_orig = k2dsm.apply_ac_res_on_dsm_model(dsm.dsm_params, dsm.algo_calibration_registers, algo::ac_to_dsm_dir::inverse); + CHECK(compare_to_bin_file< algo::algo_calibration_registers >(dsm_orig, scene_dir, "ac1x\\dsmRegsOrig_1x4_single_00.bin")); - k2dsm.convert_new_k_to_DSM(z.orig_intrinsics, z.new_intrinsics, dsm.dsm_params, dsm.dsm_regs, regs, z.relevant_pixels_image); + k2dsm.convert_new_k_to_DSM(z.orig_intrinsics, z.new_intrinsics, z.relevant_pixels_image); 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, "ac1x\\relevantPixelsImageRot", z_w, z_h, "uint8_00", compare_same_vectors)); diff --git a/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h b/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h index 9e2d15d00d..b04f948e52 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-to-bin-file.h @@ -310,14 +310,14 @@ bool compare_calib_to_bin_file( return compare_calib_to_bin_file( calib, cost, scene_dir, filename, gradient ); } -bool operator==(const algo::DSM_regs& first, const algo::DSM_regs& second) +bool operator==(const algo::algo_calibration_registers& first, const algo::algo_calibration_registers& second) { bool ok = true; - ok &= compare_and_trace(first.dsm_x_offset, second.dsm_x_offset, "dsm_x_offset"); - ok &= compare_and_trace(first.dsm_y_offset, second.dsm_y_offset, "dsm_y_offset"); - ok &= compare_and_trace(first.dsm_x_scale, second.dsm_x_scale, "dsm_x_scale"); - ok &= compare_and_trace(first.dsm_y_scale, second.dsm_y_scale, "dsm_y_scale"); + ok &= compare_and_trace(first.EXTLdsmXoffset, second.EXTLdsmXoffset, "dsm_x_offset"); + ok &= compare_and_trace(first.EXTLdsmYoffset, second.EXTLdsmYoffset, "dsm_y_offset"); + ok &= compare_and_trace(first.EXTLdsmXscale, second.EXTLdsmXscale, "dsm_x_scale"); + ok &= compare_and_trace(first.EXTLdsmYscale, second.EXTLdsmYscale, "dsm_y_scale"); return ok; } 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 66eeec8045..704b5fc032 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/scene-data.h +++ b/unit-tests/algo/depth-to-rgb-calibration/scene-data.h @@ -227,13 +227,71 @@ camera_params read_camera_params( std::string const &scene_dir, std::string cons struct dsm_params { rs2_dsm_params dsm_params; - librealsense::algo::depth_to_rgb_calibration::DSM_regs dsm_regs; + librealsense::algo::depth_to_rgb_calibration::algo_calibration_registers algo_calibration_registers; + librealsense::algo::depth_to_rgb_calibration::algo_calibration_info regs; }; dsm_params read_dsm_params(std::string const &scene_dir, std::string const &filename) { - dsm_params params; - read_data_from(bin_dir(scene_dir) + filename, ¶ms); + dsm_params res; - return params; +#pragma pack(push, 1) + struct algo_calibration + { + uint8_t fovexExistenceFlag; + float fovexNominal[4]; + float laserangleH; + float laserangleV; + float xfov[5]; + float yfov[5]; + float polyVars[3]; + float undistAngHorz[4]; + float pitchFixFactor; + + }; +#pragma pack(pop) + + rs2_dsm_params dsm_params; + librealsense::algo::depth_to_rgb_calibration::algo_calibration_registers algo_calibration_registers; + algo_calibration algo_calib; + + std::string dsmparams = bin_dir( scene_dir ) + filename; + std::fstream f = std::fstream(dsmparams, std::ios::in | std::ios::binary ); + if( !f ) + throw std::runtime_error( "failed to read file:\n" + dsmparams); + f.read( (char *)&dsm_params, sizeof(rs2_dsm_params) ); + f.read((char *)&algo_calibration_registers, sizeof(librealsense::algo::depth_to_rgb_calibration::algo_calibration_registers)); + f.read((char *)&algo_calib, sizeof(algo_calibration)); + + f.close(); + + res.dsm_params = dsm_params; + res.algo_calibration_registers = algo_calibration_registers; + + + res.regs.FRMWfovexExistenceFlag = algo_calib.fovexExistenceFlag; + + for (auto i = 0; i < 4; i++) + { + res.regs.FRMWfovexNominal[i] = algo_calib.fovexNominal[i]; + res.regs.FRMWundistAngHorz[i] = algo_calib.undistAngHorz[i]; + } + + res.regs.FRMWlaserangleH = algo_calib.laserangleH; + res.regs.FRMWlaserangleV = algo_calib.laserangleV; + + for (auto i = 0; i < 5; i++) + { + res.regs.FRMWxfov[i] = algo_calib.xfov[i]; + res.regs.FRMWyfov[i] = algo_calib.yfov[i]; + } + + for (auto i = 0; i < 3; i++) + { + res.regs.FRMWpolyVars[i] = algo_calib.polyVars[i]; + } + + res.regs.FRMWpitchFixFactor = algo_calib.pitchFixFactor; + + return res; } From dd25744198e33035a19d97d5a7e6cb09e99069c9 Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 26 May 2020 12:20:18 +0300 Subject: [PATCH 4/6] Fix to mac and android compilation issue. --- src/algo/depth-to-rgb-calibration/k-to-dsm.cpp | 9 ++++++++- src/algo/depth-to-rgb-calibration/k-to-dsm.h | 7 +++++++ src/algo/depth-to-rgb-calibration/optimizer.cpp | 2 -- 3 files changed, 15 insertions(+), 3 deletions(-) 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 3af5780f39..741e0d2dcf 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -159,6 +159,8 @@ rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_n std::vector relevant_pixels_image_rot(relevant_pixels_image.size(), 0); rotate_180(relevant_pixels_image.data(), relevant_pixels_image_rot.data(), w, h); _pre_process_data = pre_processing(_cal_info, _orig_dsm_params, _cal_regs, old_k_raw, relevant_pixels_image_rot); + + //convert_k_to_los_error return rs2_dsm_params(); } @@ -167,6 +169,11 @@ const pre_process_data& librealsense::algo::depth_to_rgb_calibration::k_to_DSM:: return _pre_process_data; } +los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_k_to_los_error(pre_process_data const & pre_process_data, rs2_intrinsics_double const & k_raw) +{ + return los_shift_scaling(); +} + std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::calc_relevant_vertices ( const std::vector& relevant_pixels_image, @@ -194,7 +201,7 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::cal if (relevant_pixels_image[i*k.width + j]) { double3 ver; - double3 pix = { j, i, 1 }; + double3 pix = { (double)j, (double)i, (double)1 }; ver.x = pix.x*k_t[0] + pix.y*k_t[1] + pix.z*k_t[2]; ver.y = pix.x*k_t[3] + pix.y*k_t[4] + pix.z*k_t[5]; 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 af0933347f..2658c61897 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -158,6 +158,8 @@ namespace depth_to_rgb_calibration { const pre_process_data& get_pre_process_data() const; private: + los_shift_scaling convert_k_to_los_error(pre_process_data const & pre_process_data, rs2_intrinsics_double const & k_raw); + std::vector calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); @@ -169,9 +171,14 @@ namespace depth_to_rgb_calibration { pre_process_data _pre_process_data; + //debug data + los_shift_scaling _new_k_los; + + //input camera params rs2_dsm_params _orig_dsm_params; algo::depth_to_rgb_calibration::algo_calibration_info _cal_info; algo::depth_to_rgb_calibration::algo_calibration_registers _cal_regs; + }; diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index 69d94902e1..2d19eec83b 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -880,7 +880,6 @@ void optimizer::zero_invalid_edges( z_frame_data & z_data, ir_frame_data const & std::vector< direction > optimizer::get_direction( std::vector gradient_x, std::vector gradient_y ) { -#define PI 3.14159265 std::vector res( gradient_x.size(), deg_none ); std::map angle_dir_map = { {0, deg_0}, {45,deg_45} , {90,deg_90}, {135,deg_135} }; @@ -902,7 +901,6 @@ std::vector< direction > optimizer::get_direction( std::vector gradient_ } std::vector< direction > optimizer::get_direction2(std::vector gradient_x, std::vector gradient_y) { -#define PI 3.14159265 std::vector res(gradient_x.size(), deg_none); std::map angle_dir_map = { {0, deg_0}, {45,deg_45} , {90,deg_90}, {135,deg_135} , { 180,deg_180 }, { 225,deg_225 }, { 270,deg_270 }, { 315,deg_315 } }; From 0df91cb57fd5f3c336775193e02beadcfe513db3 Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 26 May 2020 13:50:35 +0300 Subject: [PATCH 5/6] fix scene_dir path --- unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp index d1698ec53f..64add5c7a7 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp +++ b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp @@ -10,7 +10,7 @@ TEST_CASE("Scene 2", "[d2rgb]") { - std::string scene_dir("C:\\work\\librealsense\\build\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); + std::string scene_dir("..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); // std::string scene_dir( "C:\\work\\autocal" ); scene_dir += "\\F9440687\\LongRange_D_768x1024_RGB_1920x1080\\2\\"; From df7dab811f0cb12af0144c43c5b847e1987df943 Mon Sep 17 00:00:00 2001 From: aangerma Date: Wed, 27 May 2020 01:53:44 +0300 Subject: [PATCH 6/6] K to DSM --- .../calibration-types.h | 58 +- .../depth-to-rgb-calibration/frame-data.h | 2 +- .../depth-to-rgb-calibration/k-to-dsm.cpp | 521 +++++++++++++++--- src/algo/depth-to-rgb-calibration/k-to-dsm.h | 50 +- .../depth-to-rgb-calibration/optimizer.cpp | 4 +- src/algo/depth-to-rgb-calibration/optimizer.h | 2 + src/algo/depth-to-rgb-calibration/utils.cpp | 180 ++++++ src/algo/depth-to-rgb-calibration/utils.h | 13 +- .../depth-to-rgb-calibration/compare-scene.h | 13 +- .../depth-to-rgb-calibration/d2rgb-common.h | 1 + .../depth-to-rgb-calibration/test-scene-2.cpp | 3 +- 11 files changed, 749 insertions(+), 98 deletions(-) diff --git a/src/algo/depth-to-rgb-calibration/calibration-types.h b/src/algo/depth-to-rgb-calibration/calibration-types.h index 7ea853ca2c..392d1c817e 100644 --- a/src/algo/depth-to-rgb-calibration/calibration-types.h +++ b/src/algo/depth-to-rgb-calibration/calibration-types.h @@ -18,6 +18,22 @@ namespace depth_to_rgb_calibration { double & operator [] ( int i ) { return (&x)[i]; } bool operator == (const double3 d) { return x == d.x && y == d.y && z == d.z; } bool operator != (const double3 d) { return !(*this == d); } + double3 operator-(const double3& other) { + return { x - other.x, y - other.y , z - other.z }; + } + + double get_norm() + { + return x*x + y * y + z * z; + } + + double3 operator*(const double& scalar) { + return { x * scalar , y * scalar , z * scalar }; + } + + double operator*(const double3& s) { + return x * s.x + y * s.y + z * s.z; + } }; struct double2 @@ -30,8 +46,18 @@ namespace depth_to_rgb_calibration { struct double3x3 { - double mat[3][3]; - double3x3 transpose() + /* double3x3(double arr[9]) + { + for (auto i = 0; i < 3; i++) + { + for (auto j = 0; j < 3; j++) + { + mat[i][j] = arr[i * 3 + j]; + } + } + }*/ + double mat[3][3]; + double3x3 transpose() { double3x3 res = { 0 }; @@ -45,7 +71,7 @@ namespace depth_to_rgb_calibration { return res; } - double3x3 operator*(const double3x3& other) + double3x3 operator*(const double3x3& other) { double3x3 res = { 0 }; @@ -64,7 +90,7 @@ namespace depth_to_rgb_calibration { return res; } - double3 operator*(const double3& other) + double3 operator*(const double3& other) { double3 res = { 0 }; @@ -74,18 +100,18 @@ namespace depth_to_rgb_calibration { return res; } - std::vector to_vector() - { - std::vector res; - for (auto i = 0; i < 3; i++) - { - for (auto j = 0; j < 3; j++) - { - res.push_back(mat[i][j]); - } - } - return res; - } + std::vector to_vector() + { + std::vector res; + for (auto i = 0; i < 3; i++) + { + for (auto j = 0; j < 3; j++) + { + res.push_back(mat[i][j]); + } + } + return res; + } }; enum direction :uint8_t diff --git a/src/algo/depth-to-rgb-calibration/frame-data.h b/src/algo/depth-to-rgb-calibration/frame-data.h index c16c0ed712..2e40c5b876 100644 --- a/src/algo/depth-to-rgb-calibration/frame-data.h +++ b/src/algo/depth-to-rgb-calibration/frame-data.h @@ -8,7 +8,7 @@ #include #include "calibration-types.h" -#include "k-to-dsm.h" + namespace librealsense { namespace algo { 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 741e0d2dcf..f61386fcc7 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.cpp @@ -21,10 +21,12 @@ librealsense::algo::depth_to_rgb_calibration::k_to_DSM::k_to_DSM ( const rs2_dsm_params & 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) - :_orig_dsm_params(orig_dsm_params), - _cal_info(cal_info), - _cal_regs(cal_regs) + algo::depth_to_rgb_calibration::algo_calibration_registers const & cal__regs, + const double& max_scaling_step) + :_ac_data(orig_dsm_params), + _regs(cal_info), + _dsm_regs(cal__regs), + _max_scaling_step(max_scaling_step) { } @@ -45,25 +47,25 @@ algo_calibration_registers librealsense::algo::depth_to_rgb_calibration::k_to_DS res = dsm_regs; break; case 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_offset) / ac_data.h_scale; - res.EXTLdsmYoffset = (dsm_regs.EXTLdsmYoffset + ac_data.v_offset) / ac_data.v_scale; + 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)ac_data.h_scale; + res.EXTLdsmYoffset = ((double)dsm_regs.EXTLdsmYoffset + (double)ac_data.v_offset) / (double)ac_data.v_scale; 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) / dsm_regs.EXTLdsmXscale; - res.EXTLdsmYoffset = (dsm_regs.EXTLdsmYoffset + ac_data.v_offset) / dsm_regs.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)dsm_regs.EXTLdsmXscale; + res.EXTLdsmYoffset = ((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 valid model"); + throw std::runtime_error(_ac_data.flags[0] + "is not valid model"); break; } } else if (type == inverse) // revert from modified model to original model { - switch (ac_data.flags[0]) + switch (ac_data.model) { case dsm_model::none: // none res = dsm_regs; @@ -90,12 +92,12 @@ algo_calibration_registers librealsense::algo::depth_to_rgb_calibration::k_to_DS los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_ac_data_to_los_error ( - const algo_calibration_registers& algo_calibration_registers, + const algo_calibration_registers& dsm_regs, const rs2_dsm_params& ac_data ) { los_shift_scaling res; - switch (ac_data.flags[0]) + switch (_ac_data.flags[0]) { case dsm_model::none: // none res.los_scaling_x = 1; @@ -104,19 +106,19 @@ los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::conver 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/ _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; 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 / _ac_data.h_scale; + res.los_scaling_y = 1 / _ac_data.v_scale; - auto dsm_orig = apply_ac_res_on_dsm_model(ac_data, algo_calibration_registers, inverse); + 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 = -_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); break; } return res; @@ -127,15 +129,16 @@ pre_process_data librealsense::algo::depth_to_rgb_calibration::k_to_DSM::pre_pro const algo_calibration_info& regs, const rs2_dsm_params& ac_data, const algo_calibration_registers& algo_calibration_registers, - const rs2_intrinsics_double& k_raw, + const rs2_intrinsics_double& orig_k_raw, const std::vector& relevant_pixels_image ) { pre_process_data res; + res.orig_k = orig_k_raw; res.relevant_pixels_image_rot = relevant_pixels_image; res.last_los_error = convert_ac_data_to_los_error(algo_calibration_registers, ac_data); - res.vertices_orig = calc_relevant_vertices(relevant_pixels_image, k_raw); + res.vertices_orig = calc_relevant_vertices(relevant_pixels_image, orig_k_raw); auto dsm_res_orig = apply_ac_res_on_dsm_model(ac_data, algo_calibration_registers, inverse); res.los_orig = convert_norm_vertices_to_los(regs, dsm_res_orig, res.vertices_orig); return res; @@ -145,7 +148,7 @@ rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_n ( const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const std::vector& relevant_pixels_image + const z_frame_data& z ) { auto w = old_k.width; @@ -154,14 +157,52 @@ rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_n auto old_k_raw = rotate_k_mat(old_k); auto new_k_raw = rotate_k_mat(new_k); - auto dsm_orig = apply_ac_res_on_dsm_model(_orig_dsm_params, _cal_regs, inverse); + auto dsm_orig = apply_ac_res_on_dsm_model(_ac_data, _dsm_regs, inverse); + + 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); + double2 los_shift = { 0 }; + 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; + + for (auto i = 0; i < sc_vertices.size(); i++) + { + sc_vertices[i].x *= -1; + 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); + + for (auto i = 0; i < new_vertices.size(); i++) + { + new_vertices[i].x = new_vertices[i].x / new_vertices[i].z* sc_vertices[i].z; + new_vertices[i].y = new_vertices[i].y / new_vertices[i].z* sc_vertices[i].z; + new_vertices[i].z = new_vertices[i].z / new_vertices[i].z* sc_vertices[i].z; + + new_vertices[i].x *= -1; + new_vertices[i].y *= -1; + } + + std::vector projed(new_vertices.size()); + std::vector xim_new(new_vertices.size()); + std::vector yim_new(new_vertices.size()); - std::vector relevant_pixels_image_rot(relevant_pixels_image.size(), 0); - rotate_180(relevant_pixels_image.data(), relevant_pixels_image_rot.data(), w, h); - _pre_process_data = pre_processing(_cal_info, _orig_dsm_params, _cal_regs, old_k_raw, relevant_pixels_image_rot); + for (auto i = 0; i < new_vertices.size(); i++) + { + projed[i].x = new_vertices[i].x*old_k.fx + new_vertices[i].z*old_k.ppx; + projed[i].y = new_vertices[i].y*old_k.fy + new_vertices[i].z*old_k.ppy; + projed[i].z = new_vertices[i].z; - //convert_k_to_los_error - return rs2_dsm_params(); + xim_new[i] = projed[i].x / projed[i].z; + yim_new[i] = projed[i].y / projed[i].z; + } + + return ac_data_cand; } const pre_process_data& librealsense::algo::depth_to_rgb_calibration::k_to_DSM::get_pre_process_data() const @@ -169,9 +210,358 @@ const pre_process_data& librealsense::algo::depth_to_rgb_calibration::k_to_DSM:: return _pre_process_data; } -los_shift_scaling librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_k_to_los_error(pre_process_data const & pre_process_data, rs2_intrinsics_double const & k_raw) +double2 librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_k_to_los_error +( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + rs2_intrinsics_double const & new_k_raw +) +{ + double2 focal_scaling; + + focal_scaling.x = new_k_raw.fx / _pre_process_data.orig_k.fx; + focal_scaling.y = new_k_raw.fy / _pre_process_data.orig_k.fy; + + double coarse_grid[5] = { -1, -0.5, 0, 0.5, 1 }; + double fine_grid[5] = { -1, -0.5, 0, 0.5, 1 }; + + double coarse_grid_x[5]; + double coarse_grid_y[5]; + + for (auto i = 0; i < 5; i++) + { + coarse_grid_x[i] = coarse_grid[i] * _max_scaling_step + (double)_pre_process_data.last_los_error.los_scaling_x; + coarse_grid_y[i] = coarse_grid[i] * _max_scaling_step + (double)_pre_process_data.last_los_error.los_scaling_y; + fine_grid[i] = fine_grid[i] * 0.6* _max_scaling_step; + } + + double grid_y[25]; + double grid_x[25]; + ndgrid_my(coarse_grid_y, coarse_grid_x, grid_y, grid_x); + + auto opt_scaling = run_scaling_optimization_step(regs, dsm_regs, grid_x, grid_y, focal_scaling); + + double fine_grid_x[5] = { 0 }; + double fine_grid_y[5] = { 0 }; + + for (auto i = 0; i < 5; i++) + { + fine_grid_x[i] = fine_grid[i] + opt_scaling.x; + fine_grid_y[i] = fine_grid[i] + opt_scaling.y; + } + + ndgrid_my(fine_grid_y, fine_grid_x, grid_y, grid_x); + auto los_scaling = run_scaling_optimization_step(regs, dsm_regs, grid_x, grid_y, focal_scaling); + + auto max_step_with_margin = 1.01*_max_scaling_step; + + los_scaling.x = std::min(std::max(los_scaling.x, _pre_process_data.last_los_error.los_scaling_x - max_step_with_margin), _pre_process_data.last_los_error.los_scaling_x + max_step_with_margin); + los_scaling.y = std::min(std::max(los_scaling.y, _pre_process_data.last_los_error.los_scaling_y - max_step_with_margin), _pre_process_data.last_los_error.los_scaling_y + max_step_with_margin); + + return los_scaling; +} + +rs2_dsm_params librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_los_error_to_ac_data +( + const rs2_dsm_params& 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; + switch (ac_data_out.model) + { + case dsm_model::none: + ac_data_out.h_scale = 1; + ac_data_out.v_scale = 1; + ac_data_out.h_offset = 0; + ac_data_out.v_offset = 0; + break; + case dsm_model::AOT: + 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 = 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 = -(_dsm_regs.EXTLdsmXoffset*(1 - los_scaling.x) + los_shift.x)*_dsm_regs.EXTLdsmXscale; + ac_data_out.v_offset = -(_dsm_regs.EXTLdsmYoffset*(1 - los_scaling.y) + los_shift.y)*_dsm_regs.EXTLdsmYscale; + break; + + } + return ac_data_out; +} + +double2 librealsense::algo::depth_to_rgb_calibration::k_to_DSM::run_scaling_optimization_step +( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + double scaling_grid_x[25], + double scaling_grid_y[25], + double2 focal_scaling) +{ + auto opt_k = optimize_k_under_los_error(regs, dsm_regs, scaling_grid_x, scaling_grid_y); + + double fx_scaling_on_grid[25]; + double fy_scaling_on_grid[25]; + + for (auto i = 0; i < 25; i++) + { + fx_scaling_on_grid[i] = opt_k[i].mat[0][0] / _pre_process_data.orig_k.fx; + fy_scaling_on_grid[i] = opt_k[i].mat[1][1] / _pre_process_data.orig_k.fy; + } + double err_l2[25]; + for (auto i = 0; i < 25; i++) + { + err_l2[i] = std::sqrt(std::pow(fx_scaling_on_grid[i] - focal_scaling.x, 2) + std::pow(fy_scaling_on_grid[i] - focal_scaling.y, 2)); + } + + double sg_mat[25][6]; + + for (auto i = 0; i < 25; i++) + { + sg_mat[i][0] = scaling_grid_x[i] * scaling_grid_x[i]; + sg_mat[i][1] = scaling_grid_y[i] * scaling_grid_y[i]; + sg_mat[i][2] = scaling_grid_x[i] * scaling_grid_y[i]; + sg_mat[i][3] = scaling_grid_x[i]; + sg_mat[i][4] = scaling_grid_y[i]; + sg_mat[i][5] = 1; + } + + double sg_mat_tag_x_sg_mat[36] = { 0 }; + double sg_mat_tag_x_err_l2[6] = { 0 }; + + for (auto i = 0; i < 6; i++) + { + for (auto j = 0; j < 6; j++) + { + for (auto l = 0; l < 25; l++) + { + sg_mat_tag_x_sg_mat[i * 6 + j] += sg_mat[l][i] * sg_mat[l][j]; + } + + } + } + + for (auto j = 0; j < 6; j++) + { + for (auto l = 0; l < 25; l++) + { + sg_mat_tag_x_err_l2[j] += sg_mat[l][j] * err_l2[l]; + } + } + + double quad_coef[6]; + direct_inv_6x6(sg_mat_tag_x_sg_mat, sg_mat_tag_x_err_l2, quad_coef); + + double A[4] = { quad_coef[0], quad_coef[2] / 2, quad_coef[2] / 2, quad_coef[1] }; + double B[2] = { quad_coef[3] / 2, quad_coef[4] / 2 }; + double opt_scaling[2]; + + direct_inv_2x2(A, B, opt_scaling); + opt_scaling[0] = -opt_scaling[0]; + opt_scaling[1] = -opt_scaling[1]; + + // sanity check + + double min_x, min_y, max_x, max_y; + min_x = max_x = scaling_grid_x[0]; + min_y = max_y = scaling_grid_y[0]; + + for (auto i = 0; i < 25; i++) + { + if (min_x > scaling_grid_x[i]) + min_x = scaling_grid_x[i]; + + if (min_y > scaling_grid_y[i]) + min_y = scaling_grid_y[i]; + + if (max_x < scaling_grid_x[i]) + max_x = scaling_grid_x[i]; + + if (max_y < scaling_grid_y[i]) + max_y = scaling_grid_y[i]; + } + + auto is_pos_def = (quad_coef[0] + quad_coef[1]) > 0 && (quad_coef[0] * quad_coef[1] - quad_coef[2] * quad_coef[2] / 4) > 0; + auto is_with_in_lims = (opt_scaling[0] > min_x) && (opt_scaling[0] < max_y) && (opt_scaling[1] > min_y) && (opt_scaling[1] < max_y); + + if (!is_pos_def || !is_with_in_lims) + { + double min_err = err_l2[0]; + int ind_min = 0; + for (auto i = 0; i < 25; i++) + { + if (min_err > err_l2[i]) + { + min_err = err_l2[i]; + ind_min = i; + } + + } + opt_scaling[0] = scaling_grid_x[ind_min]; + opt_scaling[1] = scaling_grid_y[ind_min]; + } + return { opt_scaling[0], opt_scaling[1] }; +} + +std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::optimize_k_under_los_error +( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + double scaling_grid_x[25], + double scaling_grid_y[25] +) +{ + auto orig_k = _pre_process_data.orig_k; + + std::vector res(25, { 0 }); + + for (auto i = 0; i < 25; i++) + { + std::vector los(_pre_process_data.los_orig.size()); + std::vector updated_pixels(_pre_process_data.los_orig.size()); + + for (auto j = 0; j < updated_pixels.size(); j++) + { + los[j].x = scaling_grid_x[i] * _pre_process_data.los_orig[j].x; + los[j].y = scaling_grid_y[i] * _pre_process_data.los_orig[j].y; + } + auto updated_vertices = convert_los_to_norm_vertices(regs, dsm_regs, los); + + for (auto i = 0; i < updated_vertices.size(); i++) + { + updated_pixels[i].x = updated_vertices[i].x*orig_k.fx + updated_vertices[i].z*orig_k.ppx; + updated_pixels[i].y = updated_vertices[i].y*orig_k.fy + updated_vertices[i].z*orig_k.ppy; + updated_pixels[i].z = updated_vertices[i].z; + } + + std::vector v1(updated_pixels.size()); + std::vector v2(updated_pixels.size()); + std::vector p1(updated_pixels.size()); + std::vector p2(updated_pixels.size()); + + for (auto i = 0; i < v1.size(); i++) + { + v1[i].x = _pre_process_data.vertices_orig[i].x; + v1[i].y = 0; + + v2[i].x = 0; + v2[i].y = _pre_process_data.vertices_orig[i].y; + + p1[i] = updated_pixels[i].x - orig_k.ppx*_pre_process_data.vertices_orig[i].z; + p2[i] = updated_pixels[i].y - orig_k.ppy*_pre_process_data.vertices_orig[i].z; + } + v1.insert(v1.end(), v2.begin(), v2.end()); + p1.insert(p1.end(), p2.begin(), p2.end()); + + double vtag_x_v[4] = { 0 }; + double vtag_x_p[2] = { 0 }; + + for (auto i = 0; i < v1.size(); i++) + { + vtag_x_v[0] += v1[i].x*v1[i].x; + vtag_x_v[1] += v1[i].x*v1[i].y; + vtag_x_v[2] += v1[i].y*v1[i].x; + vtag_x_v[3] += v1[i].y*v1[i].y; + + vtag_x_p[0] += v1[i].x*p1[i]; + vtag_x_p[1] += v1[i].y*p1[i]; + } + + double inv[2]; + + direct_inv_2x2(vtag_x_v, vtag_x_p, inv); + + double3x3 mat = { inv[0],0.0,orig_k.ppx , + 0.0,inv[1],orig_k.ppy , + 0.0,0.0,1.0 }; + + res[i] = mat; + } + return res; +} + +std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_los_to_norm_vertices +( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + std::vector los +) { - return los_shift_scaling(); + 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 }); + + 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; + + 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_y_corr_coarse = dsm_y + (dsm_x / 2047)*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_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 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)); + } + + 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()); + fove_x_indicent_direction[i].y /= sqrt(fove_x_indicent_direction[i].get_norm()); + fove_x_indicent_direction[i].z /= sqrt(fove_x_indicent_direction[i].get_norm()); + } + + auto outbound_direction = fove_x_indicent_direction; + 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]; + + 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 + + fove_x_indicent_direction[i].y*fove_x_indicent_direction[i].y; + + auto xy_factor = std::sqrt((1 - outbound_direction[i].z*outbound_direction[i].z) / xy_norm); + outbound_direction[i].x = fove_x_indicent_direction[i].x*xy_factor; + outbound_direction[i].y = fove_x_indicent_direction[i].y*xy_factor; + }; + + } + for (auto i = 0; i < outbound_direction.size(); i++) + { + outbound_direction[i].x /= outbound_direction[i].z; + outbound_direction[i].y /= outbound_direction[i].z; + outbound_direction[i].z /= outbound_direction[i].z; + } + return outbound_direction; } std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::calc_relevant_vertices @@ -216,8 +606,8 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::cal std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::convert_norm_vertices_to_los ( - const algo_calibration_info& regs, - const algo_calibration_registers& algo_calibration_registers, + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, std::vector vertices ) { @@ -225,7 +615,7 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con auto directions = transform_to_direction(vertices); auto fovex_indicent_direction = directions; - if (regs.FRMWfovexExistenceFlag) + if (_regs.FRMWfovexExistenceFlag) { std::fill(fovex_indicent_direction.begin(), fovex_indicent_direction.end(), double3{ 0,0,0 }); std::vector ang_post_exp(fovex_indicent_direction.size(), 0); @@ -240,10 +630,10 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con 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; } @@ -260,21 +650,16 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con } } - auto x = (double)regs.FRMWlaserangleH* PI / 180.0; - auto y = (double)(regs.FRMWlaserangleV + 180)* PI / 180.0; - - double laser_incident_direction[3] = { std::cos(y)*std::sin(x), - std::sin(y), - std::cos(y)*std::cos(x) }; + auto laser_incident = laser_incident_direction({ _regs.FRMWlaserangleH , _regs.FRMWlaserangleV + 180 }); std::vector mirror_normal_direction(fovex_indicent_direction.size()); std::vector dsm_x_corr(fovex_indicent_direction.size()); std::vector dsm_y_corr(fovex_indicent_direction.size()); for (auto i = 0; i < fovex_indicent_direction.size(); i++) { - mirror_normal_direction[i].x = fovex_indicent_direction[i].x - laser_incident_direction[0]; - mirror_normal_direction[i].y = fovex_indicent_direction[i].y - laser_incident_direction[1]; - mirror_normal_direction[i].z = fovex_indicent_direction[i].z - laser_incident_direction[2]; + mirror_normal_direction[i].x = fovex_indicent_direction[i].x - laser_incident[0]; + mirror_normal_direction[i].y = fovex_indicent_direction[i].y - laser_incident[1]; + mirror_normal_direction[i].z = fovex_indicent_direction[i].z - laser_incident[2]; auto norm = sqrt(mirror_normal_direction[i].x*mirror_normal_direction[i].x + mirror_normal_direction[i].y* mirror_normal_direction[i].y + @@ -287,10 +672,10 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con auto ang_x = std::atan(mirror_normal_direction[i].x / mirror_normal_direction[i].z)* 180.0 / PI; auto ang_y = std::asin(mirror_normal_direction[i].y)* 180.0 / PI; - int mirror_mode = 1/*regs.frmw.mirrorMovmentMode*/; + 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); } @@ -307,13 +692,13 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con for (auto i = 0; i < dsm_grid.size(); i ++) { double vals[3] = { std::pow((dsm_grid[i] / 2047), 1), std::pow((dsm_grid[i] / 2047), 2), std::pow((dsm_grid[i] / 2047), 3) }; - dsm_x_coarse_on_grid[i] = dsm_grid[i] + vals[0] * (double)regs.FRMWpolyVars[0] + vals[1] * (double)regs.FRMWpolyVars[1] + vals[2] * (double)regs.FRMWpolyVars[2]; + dsm_x_coarse_on_grid[i] = dsm_grid[i] + vals[0] * (double)_regs.FRMWpolyVars[0] + vals[1] * (double)_regs.FRMWpolyVars[1] + vals[2] * (double)_regs.FRMWpolyVars[2]; auto val = dsm_x_coarse_on_grid[i] / 2047; double vals2[4] = { std::pow(val , 1),std::pow(val , 2), std::pow(val , 3) , std::pow(val , 4) }; - dsm_x_corr_on_grid[i] = dsm_x_coarse_on_grid[i] + vals2[0] * (double)regs.FRMWundistAngHorz[0] + - vals2[1] * (double)regs.FRMWundistAngHorz[1] + - vals2[2] * (double)regs.FRMWundistAngHorz[2] + - vals2[3] * (double)regs.FRMWundistAngHorz[3]; + dsm_x_corr_on_grid[i] = dsm_x_coarse_on_grid[i] + vals2[0] * (double)_regs.FRMWundistAngHorz[0] + + vals2[1] * (double)_regs.FRMWundistAngHorz[1] + + vals2[2] * (double)_regs.FRMWundistAngHorz[2] + + vals2[3] * (double)_regs.FRMWundistAngHorz[3]; } auto dsm_x = interp1(dsm_x_corr_on_grid, dsm_grid, dsm_x_corr); @@ -321,19 +706,29 @@ std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::con for (auto i = 0; i < dsm_y.size(); i++) { - dsm_y[i] = dsm_y_corr[i] - (dsm_x[i] / 2047)*(double)regs.FRMWpitchFixFactor; + dsm_y[i] = dsm_y_corr[i] - (dsm_x[i] / 2047)*(double)_regs.FRMWpitchFixFactor; } std::vector res(dsm_x.size()); for (auto i = 0; i < res.size(); i++) { - res[i].x = (dsm_x[i] + 2047) / (double)algo_calibration_registers.EXTLdsmXscale - (double)algo_calibration_registers.EXTLdsmXoffset; - res[i].y = (dsm_y[i] + 2047) / (double)algo_calibration_registers.EXTLdsmYscale - (double)algo_calibration_registers.EXTLdsmYoffset; + res[i].x = (dsm_x[i] + 2047) / (double)_dsm_regs.EXTLdsmXscale - (double)_dsm_regs.EXTLdsmXoffset; + res[i].y = (dsm_y[i] + 2047) / (double)_dsm_regs.EXTLdsmYscale - (double)_dsm_regs.EXTLdsmYoffset; } return res; } +double3 librealsense::algo::depth_to_rgb_calibration::k_to_DSM::laser_incident_direction(double2 angle_rad) +{ + double2 angle_deg = { angle_rad.x* PI / 180.0, (angle_rad.y)* PI / 180.0 }; + double3 laser_incident_direction = { std::cos(angle_deg.y)*std::sin(angle_deg.x), + std::sin(angle_deg.y), + std::cos(angle_deg.y)*std::cos(angle_deg.x) }; + + return laser_incident_direction; +} + std::vector librealsense::algo::depth_to_rgb_calibration::k_to_DSM::transform_to_direction(std::vector vec) { std::vector res(vec.size()); 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 2658c61897..38ae498cc7 100644 --- a/src/algo/depth-to-rgb-calibration/k-to-dsm.h +++ b/src/algo/depth-to-rgb-calibration/k-to-dsm.h @@ -4,6 +4,7 @@ #pragma once #include "calibration.h" +#include "frame-data.h" #include "../../../include/librealsense2/h/rs_types.h" #include // librealsense types (intr/extr) @@ -25,6 +26,7 @@ namespace depth_to_rgb_calibration { TOA = 2 }; + struct los_shift_scaling { float los_scaling_x; @@ -127,6 +129,7 @@ namespace depth_to_rgb_calibration { struct pre_process_data { + rs2_intrinsics_double orig_k; los_shift_scaling last_los_error; std::vector vertices_orig; std::vector relevant_pixels_image_rot; @@ -139,7 +142,8 @@ namespace depth_to_rgb_calibration { public: k_to_DSM(const rs2_dsm_params& 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); + 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); @@ -151,14 +155,41 @@ namespace depth_to_rgb_calibration { const rs2_intrinsics_double& k_raw, const std::vector& relevant_pixels_image); - rs2_dsm_params convert_new_k_to_DSM(const rs2_intrinsics_double& old_k, + rs2_dsm_params convert_new_k_to_DSM( + const rs2_intrinsics_double& old_k, const rs2_intrinsics_double& new_k, - const std::vector& relevant_pixels_image); + const z_frame_data& z); const pre_process_data& get_pre_process_data() const; private: - los_shift_scaling convert_k_to_los_error(pre_process_data const & pre_process_data, rs2_intrinsics_double const & k_raw); + double2 convert_k_to_los_error( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + 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, + const algo_calibration_registers& dsm_regs, + double2 los_shift, double2 los_scaling); + + double2 run_scaling_optimization_step( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + double scaling_grid_x[25], + double scaling_grid_y[25], + double2 focal_scaling); + + std::vector optimize_k_under_los_error( + algo::depth_to_rgb_calibration::algo_calibration_info const & regs, + algo_calibration_registers const &dsm_regs, + double scaling_grid_x[25], + double scaling_grid_y[25] ); + + 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 calc_relevant_vertices(const std::vector& relevant_pixels_image, const rs2_intrinsics_double& k); @@ -167,18 +198,21 @@ namespace depth_to_rgb_calibration { const algo_calibration_registers& algo_calibration_registers, std::vector vertices); + double3 laser_incident_direction(double2 angle_rad); std::vector transform_to_direction(std::vector); pre_process_data _pre_process_data; //debug data - los_shift_scaling _new_k_los; + double2 _new_los_scaling; //input camera params - rs2_dsm_params _orig_dsm_params; - algo::depth_to_rgb_calibration::algo_calibration_info _cal_info; - algo::depth_to_rgb_calibration::algo_calibration_registers _cal_regs; + rs2_dsm_params _ac_data; + algo::depth_to_rgb_calibration::algo_calibration_info _regs; + algo::depth_to_rgb_calibration::algo_calibration_registers _dsm_regs; + //algorithem params + double _max_scaling_step; }; diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index 2d19eec83b..92153c16c1 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -395,7 +395,7 @@ void optimizer::set_z_data( std::vector< z_t > && depth_data, algo_calibration_info const & cal_info, algo_calibration_registers const & cal_regs, float depth_units ) { - _k_to_DSM = std::make_shared(dsm_params, cal_info, cal_regs); + _k_to_DSM = std::make_shared(dsm_params, cal_info, cal_regs, _params.max_scaling_step); /*[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 @@ -1612,7 +1612,7 @@ size_t optimizer::optimize( std::function< void( iteration_data_collect const & decompose_p_mat(); - //_k_to_DSM.convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z.orig_dsm_params, _z.algo_calibration_registers, _z.regs, _z.relevant_pixels_image); + _k_to_DSM->convert_new_k_to_DSM(_z.orig_intrinsics, _z.new_intrinsics, _z); return n_iterations; } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index 1af1194f13..073355ba03 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -73,6 +73,7 @@ namespace depth_to_rgb_calibration { // output validation 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; }; // svm struct decision_params @@ -203,6 +204,7 @@ namespace depth_to_rgb_calibration { ir_frame_data const & get_ir_data() const { return _ir; } decision_params const& get_decision_params() const { return _decision_params; }; std::vector< double >const& get_extracted_features() const { return _extracted_features; }; + params const & get_params() const { return _params; } private: void decompose_p_mat(); diff --git a/src/algo/depth-to-rgb-calibration/utils.cpp b/src/algo/depth-to-rgb-calibration/utils.cpp index d1b1669ea5..15d505b8e2 100644 --- a/src/algo/depth-to-rgb-calibration/utils.cpp +++ b/src/algo/depth-to-rgb-calibration/utils.cpp @@ -9,6 +9,185 @@ namespace librealsense { namespace algo { namespace depth_to_rgb_calibration { + double get_norma(const std::vector& vec) + { + double sum = 0; + std::for_each(vec.begin(), vec.end(), [&](double3 v) {sum += v.get_norm(); }); + return std::sqrt(sum); + } + + double rad_to_deg(double rad) + { + return rad * 180.0 / PI; + } + + double deg_to_rad(double deg) + { + return deg * PI / 180.0; + } + + void direct_inv_6x6(const double A[36], const double B[6], double C[6]) + { + double b_A[36]; + int i0; + int j; + signed char ipiv[6]; + int iy; + int b; + int jj; + int jy; + int jp1j; + int n; + int k; + double smax; + int ix; + double s; + + /* INV_MY Summary of this function goes here */ + /* Detailed explanation goes here */ + memcpy(&b_A[0], &A[0], 36U * sizeof(double)); + for (i0 = 0; i0 < 6; i0++) { + ipiv[i0] = (signed char)(1 + i0); + } + + for (j = 0; j < 5; j++) { + b = j * 7; + jj = j * 7; + jp1j = b + 2; + n = 6 - j; + jy = 0; + ix = b; + smax = std::abs(b_A[b]); + for (k = 2; k <= n; k++) { + ix++; + s = std::abs(b_A[ix]); + if (s > smax) { + jy = k - 1; + smax = s; + } + } + + if (b_A[jj + jy] != 0.0) { + if (jy != 0) { + iy = j + jy; + ipiv[j] = (signed char)(iy + 1); + ix = j; + for (k = 0; k < 6; k++) { + smax = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = smax; + ix += 6; + iy += 6; + } + } + + i0 = (jj - j) + 6; + for (iy = jp1j; iy <= i0; iy++) { + b_A[iy - 1] /= b_A[jj]; + } + } + + n = 4 - j; + jy = b + 6; + iy = jj; + for (b = 0; b <= n; b++) { + smax = b_A[jy]; + if (b_A[jy] != 0.0) { + ix = jj + 1; + i0 = iy + 8; + jp1j = (iy - j) + 12; + for (k = i0; k <= jp1j; k++) { + b_A[k - 1] += b_A[ix] * -smax; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (iy = 0; iy < 6; iy++) { + C[iy] = B[iy]; + } + + for (jy = 0; jy < 5; jy++) { + if (ipiv[jy] != jy + 1) { + smax = C[jy]; + iy = ipiv[jy] - 1; + C[jy] = C[iy]; + C[iy] = smax; + } + } + + for (k = 0; k < 6; k++) { + jy = 6 * k; + if (C[k] != 0.0) { + i0 = k + 2; + for (iy = i0; iy < 7; iy++) { + C[iy - 1] -= C[k] * b_A[(iy + jy) - 1]; + } + } + } + + for (k = 5; k >= 0; k--) { + jy = 6 * k; + if (C[k] != 0.0) { + C[k] /= b_A[k + jy]; + for (iy = 0; iy < k; iy++) { + C[iy] -= C[k] * b_A[iy + jy]; + } + } + } + } + + void direct_inv_2x2(const double A[4], const double B[2], double C[2]) + { + int r1; + int r2; + double a21; + double C_tmp; + + /* INV_MY Summary of this function goes here */ + /* Detailed explanation goes here */ + if (std::abs(A[1]) > std::abs(A[0])) { + r1 = 1; + r2 = 0; + } + else { + r1 = 0; + r2 = 1; + } + + a21 = A[r2] / A[r1]; + C_tmp = A[2 + r1]; + C[1] = (B[r2] - B[r1] * a21) / (A[2 + r2] - a21 * C_tmp); + C[0] = (B[r1] - C[1] * C_tmp) / A[r1]; + } + + void ndgrid_my(const double vec1[5], const double vec2[5], double yScalingGrid + [25], double xScalingGrid[25]) + { + int k; + int b_k; + + /* NDGRID_MY Summary of this function goes here */ + /* Detailed explanation goes here */ + for (k = 0; k < 5; k++) { + for (b_k = 0; b_k < 5; b_k++) { + yScalingGrid[b_k + 5 * k] = vec1[b_k]; + } + } + + for (k = 0; k < 5; k++) { + for (b_k = 0; b_k < 5; b_k++) { + xScalingGrid[b_k + 5 * k] = vec2[k]; + } + } + + /* search around last estimated scaling */ + } + void inv(const double x[9], double y[9]) { double b_x[9]; @@ -109,6 +288,7 @@ namespace depth_to_rgb_calibration { } } } + const std::vector interp1(const std::vector ind, const std::vector vals, const std::vector intrp) { std::vector res(intrp.size(), 0); diff --git a/src/algo/depth-to-rgb-calibration/utils.h b/src/algo/depth-to-rgb-calibration/utils.h index ae40e55015..f881c99c09 100644 --- a/src/algo/depth-to-rgb-calibration/utils.h +++ b/src/algo/depth-to-rgb-calibration/utils.h @@ -9,10 +9,17 @@ namespace librealsense { namespace algo { namespace depth_to_rgb_calibration { - void inv(const double x[9], double y[9]); //generated code + void ndgrid_my(const double vec1[5], const double vec2[5], double yScalingGrid[25], double xScalingGrid[25]); //auto generated code + void inv(const double x[9], double y[9]); //auto generated code void transpose(const double x[9], double y[9]); - void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h); //generated code - const std::vector interp1(const std::vector ind, const std::vector vals, const std::vector intrp); + void rotate_180(const uint8_t* A, uint8_t* B, uint32_t w, uint32_t h); //auto generated code + const std::vector interp1(const std::vector ind, const std::vector vals, const std::vector intrp);//todo const& + double get_norma(const std::vector& vec); + double rad_to_deg(double rad); + double deg_to_rad(double deg); + void direct_inv_2x2(const double A[4], const double B[2], double C[2]); + void direct_inv_6x6(const double A[36], const double B[6], double C[6]); + } } } 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 ba7d7b3803..1a4b05afd0 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -16,6 +16,11 @@ void compare_scene( std::string const & scene_dir ) algo::optimizer cal; + /*std::vector in = { 1,2,3,4 }; + std::vectorout(4); + + algo::direct_inv(in,out,2);*/ + init_algo( cal, scene_dir, md.rgb_file, md.rgb_prev_file, @@ -217,14 +222,14 @@ void compare_scene( std::string const & scene_dir ) REQUIRE( cal.optimize( cb ) + 1 == md.n_iterations ); 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); - algo::k_to_DSM k2dsm(ci.dsm_params, ci.cal_info, ci.cal_regs); - - auto dsm_orig = k2dsm.apply_ac_res_on_dsm_model(dsm.dsm_params, dsm.algo_calibration_registers, algo::ac_to_dsm_dir::inverse); + 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, "ac1x\\dsmRegsOrig_1x4_single_00.bin")); - k2dsm.convert_new_k_to_DSM(z.orig_intrinsics, z.new_intrinsics, z.relevant_pixels_image); + 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, "ac1x\\relevantPixelsImageRot", z_w, z_h, "uint8_00", compare_same_vectors)); diff --git a/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h b/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h index 743ce36832..787df62c2c 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h +++ b/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h @@ -7,6 +7,7 @@ #include "../../../src/algo/depth-to-rgb-calibration/optimizer.h" #include "scene-data.h" #include "../../../src/algo/depth-to-rgb-calibration/k-to-dsm.h" +#include "../../../src/algo/depth-to-rgb-calibration/utils.h" #include "ac-logger.h" diff --git a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp index 64add5c7a7..67ba97467d 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp +++ b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp @@ -10,7 +10,8 @@ TEST_CASE("Scene 2", "[d2rgb]") { - std::string scene_dir("..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); + std::string scene_dir("C:\\work\\librealsense\\build\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); + //std::string scene_dir("..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20"); // std::string scene_dir( "C:\\work\\autocal" ); scene_dir += "\\F9440687\\LongRange_D_768x1024_RGB_1920x1080\\2\\";