Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Development improve performance on zero dist frames #13657

Merged
merged 10 commits into from
Jan 7, 2025
Merged

Development improve performance on zero dist frames #13657

Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
210c2e9
ff
gilpazintel Dec 24, 2024
89b95f8
before trying the exact code of the submitter
gilpazintel Dec 24, 2024
4183a3f
after applying the exact code from Wen - still I do not see an improv…
gilpazintel Dec 24, 2024
ec6fbb5
forcing the code to go though the loop
gilpazintel Jan 1, 2025
fddfd7c
disable BUILD_WITH_CPU_EXTENSIONS
gilpazintel Jan 6, 2025
d26ec5c
clean code
gilpazintel Jan 7, 2025
e338df2
avoid loop in case intrinsic coefficient are zero
gilpazintel Jan 7, 2025
5e1a37d
in case coeff[0] is zero, avoid division by zero
gilpazintel Jan 7, 2025
2308519
add fabs to protect from negative numbers
gilpazintel Jan 7, 2025
e2a18b2
changed abs to std::fabs, also changed double numeric limit to float …
gilpazintel Jan 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 39 additions & 29 deletions src/rs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down
Loading