The open source OpenXR runtime
a/tracking: add t_camera_models_undistort()
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>
authored by
Jan Schmidt
and committed by
Marge Bot
8a535d0a
ed67c9c9
+43
-8
+43
-8
src/xrt/auxiliary/tracking/t_camera_models.h
+43
-8
src/xrt/auxiliary/tracking/t_camera_models.h
···
197
197
return true;
198
198
}
199
199
200
+
static inline void
201
+
kb4_undistort(const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y)
202
+
{
203
+
float xp, yp, zp;
204
+
205
+
kb4_unproject(dist, x, y, &xp, &yp, &zp);
206
+
207
+
*out_x = xp / zp;
208
+
*out_y = yp / zp;
209
+
}
210
+
200
211
/*
201
212
* Functions for radial-tangential (un)projections
202
213
*/
···
291
302
d_dist_d_undist->v[3] = dypp_dyp;
292
303
}
293
304
294
-
static inline bool
295
-
rt8_unproject(
296
-
const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y, float *out_z)
305
+
static inline void
306
+
rt8_undistort(const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y)
297
307
{
298
-
299
308
const float x0 = (u - hg_dist->cx) / hg_dist->fx;
300
309
const float y0 = (v - hg_dist->cy) / hg_dist->fy;
301
310
···
329
338
break;
330
339
}
331
340
}
332
-
const float xp = undist.x;
333
-
const float yp = undist.y;
341
+
*out_x = undist.x;
342
+
*out_y = undist.y;
343
+
}
344
+
345
+
static inline bool
346
+
rt8_unproject(
347
+
const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y, float *out_z)
348
+
{
349
+
float xp, yp;
334
350
351
+
rt8_undistort(hg_dist, u, v, &xp, &yp);
335
352
336
353
const float norm_inv = 1.0f / sqrt(xp * xp + yp * yp + 1.0f);
337
354
*out_x = xp * norm_inv;
338
355
*out_y = yp * norm_inv;
339
356
*out_z = norm_inv;
340
-
341
-
342
357
343
358
const float rp2 = xp * xp + yp * yp;
344
359
bool in_injective_area =
···
512
527
*out_z *= -1;
513
528
*out_y *= -1;
514
529
return ret;
530
+
}
531
+
532
+
/*!
533
+
* Takes a distorted 2D point through \p x and \p y and computes the undistorted point into
534
+
* \p out_x and \p out_y
535
+
*/
536
+
static inline void
537
+
t_camera_models_undistort(
538
+
const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y)
539
+
{
540
+
switch (dist->model) {
541
+
case T_DISTORTION_OPENCV_RADTAN_8: {
542
+
rt8_undistort(dist, x, y, out_x, out_y);
543
+
}; break;
544
+
case T_DISTORTION_FISHEYE_KB4: {
545
+
kb4_undistort(dist, x, y, out_x, out_y);
546
+
}; break;
547
+
// Return false so we don't get warnings on Release builds.
548
+
default: assert(false);
549
+
}
515
550
}
516
551
517
552