mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-29 18:08:29 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/tracking: add t_camera_models_undistort()

Browse source 
Add a method to convert distorted image points to
undistorted homogeneous points like cv::undistortPoints()

Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2188>
This commit is contained in:
Jan Schmidt 2023-08-10 04:27:38 +10:00 committed by Marge Bot
parent ed67c9c95b
commit 8a535d0a87
1 changed files with 43 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

51
src/xrt/auxiliary/tracking/t_camera_models.h
View file

@ -197,6 +197,17 @@ kb4_unproject(const struct t_camera_model_params *dist, //
return true;
}
static inline void
kb4_undistort(const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y)
{
float xp, yp, zp;
kb4_unproject(dist, x, y, &xp, &yp, &zp);
*out_x = xp / zp;
*out_y = yp / zp;
}
/*
* Functions for radial-tangential (un)projections
*/
@ -291,11 +302,9 @@ rt8_distort(const struct t_camera_model_params *params,
d_dist_d_undist->v[3] = dypp_dyp;
}
static inline bool
rt8_unproject(
const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y, float *out_z)
static inline void
rt8_undistort(const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y)
{
const float x0 = (u - hg_dist->cx) / hg_dist->fx;
const float y0 = (v - hg_dist->cy) / hg_dist->fy;
@ -329,17 +338,23 @@ rt8_unproject(
break;
}
}
const float xp = undist.x;
const float yp = undist.y;
*out_x = undist.x;
*out_y = undist.y;
}
static inline bool
rt8_unproject(
const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y, float *out_z)
{
float xp, yp;
rt8_undistort(hg_dist, u, v, &xp, &yp);
const float norm_inv = 1.0f / sqrt(xp * xp + yp * yp + 1.0f);
*out_x = xp * norm_inv;
*out_y = yp * norm_inv;
*out_z = norm_inv;
const float rp2 = xp * xp + yp * yp;
bool in_injective_area =
hg_dist->rt8.metric_radius == 0.0f ? true : rp2 <= hg_dist->rt8.metric_radius * hg_dist->rt8.metric_radius;
@ -514,6 +529,26 @@ t_camera_models_unproject_and_flip(
return ret;
}
/*!
* Takes a distorted 2D point through \p x and \p y and computes the undistorted point into
* \p out_x and \p out_y
*/
static inline void
t_camera_models_undistort(
const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y)
{
switch (dist->model) {
case T_DISTORTION_OPENCV_RADTAN_8: {
rt8_undistort(dist, x, y, out_x, out_y);
}; break;
case T_DISTORTION_FISHEYE_KB4: {
kb4_undistort(dist, x, y, out_x, out_y);
}; break;
// Return false so we don't get warnings on Release builds.
default: assert(false);
}
}
/*!
* Takes a 3D point through \p x, \p y, and \p z, projects it into image space, and returns the result