diff --git a/src/rs.cpp b/src/rs.cpp index 6add8361f1..d274bdcd7e 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -4105,48 +4105,58 @@ void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics* int pixel[1] = y * intrin->fy + intrin->ppy; } NOEXCEPT_RETURN(, pixel) +/* Helper inner function (not part of the API) */ +inline bool is_intrinsics_distortion_zero(const struct rs2_intrinsics* intrin) +{ + return (std::fabs(intrin->coeffs[0]) < FLT_EPSILON && std::fabs(intrin->coeffs[1]) < FLT_EPSILON && + std::fabs(intrin->coeffs[2]) < FLT_EPSILON && std::fabs(intrin->coeffs[3]) < FLT_EPSILON && + std::fabs(intrin->coeffs[4]) < FLT_EPSILON); +} void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics* intrin, const float pixel[2], float depth) BEGIN_API_CALL { assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image - //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model - + float x = (pixel[0] - intrin->ppx) / intrin->fx; float y = (pixel[1] - intrin->ppy) / intrin->fy; float xo = x; float yo = y; + - if (intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY) + if (!is_intrinsics_distortion_zero(intrin)) { - // need to loop until convergence - // 10 iterations determined empirically - for (int i = 0; i < 10; i++) + if (intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY) { - float r2 = x * x + y * y; - float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2); - float xq = x / icdist; - float yq = y / icdist; - float delta_x = 2 * intrin->coeffs[2] * xq * yq + intrin->coeffs[3] * (r2 + 2 * xq * xq); - float delta_y = 2 * intrin->coeffs[3] * xq * yq + intrin->coeffs[2] * (r2 + 2 * yq * yq); - x = (xo - delta_x) * icdist; - y = (yo - delta_y) * icdist; + // need to loop until convergence + // 10 iterations determined empirically + for (int i = 0; i < 10; i++) + { + float r2 = x * x + y * y; + float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2); + float xq = x / icdist; + float yq = y / icdist; + float delta_x = 2 * intrin->coeffs[2] * xq * yq + intrin->coeffs[3] * (r2 + 2 * xq * xq); + float delta_y = 2 * intrin->coeffs[3] * xq * yq + intrin->coeffs[2] * (r2 + 2 * yq * yq); + x = (xo - delta_x) * icdist; + y = (yo - delta_y) * icdist; + } } - } - if (intrin->model == RS2_DISTORTION_BROWN_CONRADY) - { - // need to loop until convergence - // 10 iterations determined empirically - for (int i = 0; i < 10; i++) + if (intrin->model == RS2_DISTORTION_BROWN_CONRADY) { - float r2 = x * x + y * y; - float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2); - float delta_x = 2 * intrin->coeffs[2] * x * y + intrin->coeffs[3] * (r2 + 2 * x * x); - float delta_y = 2 * intrin->coeffs[3] * x * y + intrin->coeffs[2] * (r2 + 2 * y * y); - x = (xo - delta_x) * icdist; - y = (yo - delta_y) * icdist; - } + // need to loop until convergence + // 10 iterations determined empirically + for (int i = 0; i < 10; i++) + { + float r2 = x * x + y * y; + float icdist = (float)1 / (float)(1 + ((intrin->coeffs[4] * r2 + intrin->coeffs[1]) * r2 + intrin->coeffs[0]) * r2); + float delta_x = 2 * intrin->coeffs[2] * x * y + intrin->coeffs[3] * (r2 + 2 * x * x); + float delta_y = 2 * intrin->coeffs[3] * x * y + intrin->coeffs[2] * (r2 + 2 * y * y); + x = (xo - delta_x) * icdist; + y = (yo - delta_y) * icdist; + } + } } if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4) { @@ -4161,7 +4171,7 @@ void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics* i for (int i = 0; i < 4; i++) { float f = theta * (1 + theta2 * (intrin->coeffs[0] + theta2 * (intrin->coeffs[1] + theta2 * (intrin->coeffs[2] + theta2 * intrin->coeffs[3])))) - rd; - if (fabs(f) < FLT_EPSILON) + if (std::fabs(f) < FLT_EPSILON) { break; } @@ -4180,7 +4190,7 @@ void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics* i { rd = FLT_EPSILON; } - float r = (float)(tan(intrin->coeffs[0] * rd) / atan(2 * tan(intrin->coeffs[0] / 2.0f))); + float r = (std::fabs(intrin->coeffs[0]) < FLT_EPSILON) ? 0.0f : (float)(tan(intrin->coeffs[0] * rd) / atan(2 * tan(intrin->coeffs[0] / 2.0f))); x *= r / rd; y *= r / rd; }