diff --git a/src/ds/d500/d500-private.cpp b/src/ds/d500/d500-private.cpp index 2e8f45f74ec..3626ff52b18 100644 --- a/src/ds/d500/d500-private.cpp +++ b/src/ds/d500/d500-private.cpp @@ -157,8 +157,8 @@ namespace librealsense { auto& rgb_coefficients_table = rgb_calib_table.rgb_coefficients_table; - // checking if the fisheye distortion is needed - if (rgb_coefficients_table.distortion_model == RS2_DISTORTION_BROWN_CONRADY) + // checking if the fisheye distortion (2 in calibration table) is needed + if( rgb_coefficients_table.distortion_model != d500_calibration_distortion::brown_and_fisheye ) return; // matrix with the intrinsics - after they have been adapted to required resolution @@ -191,13 +191,11 @@ namespace librealsense intrinsics.ppy = k_brown.z.y; intrinsics.fx = k_brown.x.x; intrinsics.fy = k_brown.y.y; - intrinsics.model = RS2_DISTORTION_BROWN_CONRADY; // update values in the distortion params of the calibration table - rgb_coefficients_table.distortion_model = RS2_DISTORTION_BROWN_CONRADY; + rgb_coefficients_table.distortion_model = d500_calibration_distortion::brown; - // Since we override the table we need an indication that the table had changed from - // outside this function Decided to use a reserve field for that + // Since we override the table we need an indication that the table has changed if( rgb_coefficients_table.reserved[3] != 0 ) throw invalid_value_exception( "reserved field read from RGB distortion model table is expected to be zero" ); @@ -231,11 +229,12 @@ namespace librealsense intrinsics.height = height; // For D555e, model will be brown and we need the unrectified intrinsics - // We use reserved[3] as a flag here to indicate the full distortion module is not - // really brown but we treat it as such because the HW fixes fisheye distortion. + // For SC, model will be brown_and_fisheye and we need the rectified + // NOTE that update_table_to_correct_fisheye_distortion() changes the model to brown, so we use reserved[3] + // as a flag to indicate this happened bool use_base_intrinsics - = ( table->rgb_coefficients_table.distortion_model == RS2_DISTORTION_BROWN_CONRADY - && table->rgb_coefficients_table.reserved[3] == 0 ); + = table->rgb_coefficients_table.distortion_model == d500_calibration_distortion::brown + && table->rgb_coefficients_table.reserved[3] == 0; auto rect_params = compute_rect_params_from_resolution( use_base_intrinsics ? table->rgb_coefficients_table.base_instrinsics @@ -248,7 +247,7 @@ namespace librealsense intrinsics.fy = rect_params[1]; intrinsics.ppx = rect_params[2]; intrinsics.ppy = rect_params[3]; - intrinsics.model = table->rgb_coefficients_table.distortion_model; + intrinsics.model = RS2_DISTORTION_BROWN_CONRADY; std::memcpy( intrinsics.coeffs, table->rgb_coefficients_table.distortion_coeffs, sizeof( intrinsics.coeffs ) ); diff --git a/src/ds/d500/d500-private.h b/src/ds/d500/d500-private.h index 0e5a5929f3c..59e6ed24a44 100644 --- a/src/ds/d500/d500-private.h +++ b/src/ds/d500/d500-private.h @@ -107,12 +107,20 @@ namespace librealsense float fy; /**< Focal length of the image plane, as a multiple of pixel height */ }; + // These are the possible values for the calibration table 'distortion_model' field + enum class d500_calibration_distortion + { + none = 0, + brown = 1, + brown_and_fisheye = 2 + }; + struct single_sensor_coef_table { mini_intrinsics base_instrinsics; uint32_t distortion_non_parametric; - rs2_distortion distortion_model; /**< Distortion model of the image */ - float distortion_coeffs[13]; /**< Distortion coefficients. Order for Brown-Conrady: [k1, k2, p1, p2, k3]. Order for F-Theta Fish-eye: [k1, k2, k3, k4, 0]. Other models are subject to their own interpretations */ + d500_calibration_distortion distortion_model; + float distortion_coeffs[13]; // [k1,k2,p1,p2,k3] for Brown, followed by [k1,k2,k3,k4] with fisheye uint8_t reserved[4]; float radial_distortion_lut_range_degs; float radial_distortion_lut_focal_length;