matrixfurry.com
The open source OpenXR runtime
1// Copyright 2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Simple, untemplated, C, float-only, camera (un)projection functions for various camera models.
6 * @author Moshi Turner <moshiturner@protonmail.com>
7 * @ingroup aux_tracking
8 *
9 * Some notes:
10 * These functions should return exactly the same values as basalt-headers, down to floating point bits.
11 *
12 * They were mainly written as an expedient way to stop depending on OpenCV-based (un)projection code in Monado's hand
13 * tracking code, and to encourage compiler optimizations through inlining.
14 *
15 * Current users:
16 *
17 * * Mercury hand tracking
18 */
19
20#pragma once
21#include "math/m_vec2.h"
22#include "math/m_matrix_2x2.h"
23#include "math/m_mathinclude.h"
24
25#include "t_tracking.h"
26
27#include <assert.h>
28
29#ifdef __cplusplus
30extern "C" {
31#endif
32
33/*!
34 * Floating point parameters for @ref T_DISTORTION_FISHEYE_KB4
35 * @ingroup aux_tracking
36 */
37struct t_camera_calibration_kb4_params_float
38{
39 float k1, k2, k3, k4;
40};
41
42/*!
43 * Floating point parameters for @ref T_DISTORTION_OPENCV_RT8, also including metric_radius.
44 * @ingroup aux_tracking
45 */
46struct t_camera_calibration_rt8_params_float
47{
48 float k1, k2, p1, p2, k3, k4, k5, k6, metric_radius;
49};
50
51/*!
52 * Floating point calibration data for a single calibrated camera.
53 * @note This is basically @ref t_camera_calibration, just without some compatibility stuff and using single floats
54 * instead of doubles.
55 * @ingroup aux_tracking
56 */
57struct t_camera_model_params
58{
59 float fx, fy, cx, cy;
60 union {
61 struct t_camera_calibration_kb4_params_float fisheye;
62 struct t_camera_calibration_rt8_params_float rt8;
63 };
64 // This model gets reinterpreted from values in the main t_camera_calibration struct to either
65 // * T_DISTORTION_FISHEYE_KB4
66 // * T_DISTORTION_OPENCV_RADTAN_8
67 enum t_camera_distortion_model model;
68};
69
70
71static const float SQRT_EPSILON = 0.00316; // sqrt(1e-05)
72
73/*
74 * Functions for @ref T_DISTORTION_FISHEYE_KB4 (un)projections
75 */
76
77static inline float
78kb4_calc_r_theta(const struct t_camera_model_params *dist, //
79 const float theta, //
80 const float theta2)
81{
82 float r_theta = dist->fisheye.k4 * theta2;
83 r_theta += dist->fisheye.k3;
84 r_theta *= theta2;
85 r_theta += dist->fisheye.k2;
86 r_theta *= theta2;
87 r_theta += dist->fisheye.k1;
88 r_theta *= theta2;
89 r_theta += 1.0f;
90 r_theta *= theta;
91
92 return r_theta;
93}
94
95static inline bool
96kb4_project(const struct t_camera_model_params *dist, //
97 const float x, //
98 const float y, //
99 const float z, //
100 float *out_x, //
101 float *out_y)
102{
103 bool is_valid = true;
104 const float r2 = x * x + y * y;
105 const float r = sqrtf(r2);
106
107 if (r > SQRT_EPSILON) {
108
109
110 const float theta = atan2(r, z);
111 const float theta2 = theta * theta;
112
113 float r_theta = kb4_calc_r_theta(dist, theta, theta2);
114
115 const float mx = x * r_theta / r;
116 const float my = y * r_theta / r;
117
118 *out_x = dist->fx * mx + dist->cx;
119 *out_y = dist->fy * my + dist->cy;
120 } else {
121 // Check that the point is not cloze to zero norm
122 if (z < SQRT_EPSILON) {
123 is_valid = false;
124 }
125 *out_x = dist->fx * x / z + dist->cx;
126 *out_y = dist->fy * y / z + dist->cy;
127 }
128
129 return is_valid;
130}
131
132static inline float
133kb4_solve_theta(const struct t_camera_model_params *dist, const float *r_theta, float *d_func_d_theta)
134{
135 float theta = *r_theta;
136 for (int i = 4; i > 0; i--) {
137 float theta2 = theta * theta;
138
139 float func = dist->fisheye.k4 * theta2;
140 func += dist->fisheye.k3;
141 func *= theta2;
142 func += dist->fisheye.k2;
143 func *= theta2;
144 func += dist->fisheye.k1;
145 func *= theta2;
146 func += 1.0f;
147 func *= theta;
148
149 *d_func_d_theta = 9.0f * dist->fisheye.k4 * theta2;
150 *d_func_d_theta += 7.0f * dist->fisheye.k3;
151 *d_func_d_theta *= theta2;
152 *d_func_d_theta += 5.0f * dist->fisheye.k2;
153 *d_func_d_theta *= theta2;
154 *d_func_d_theta += 3.0f * dist->fisheye.k1;
155 *d_func_d_theta *= theta2;
156 *d_func_d_theta += 1.0f;
157
158 // Iteration of Newton method
159 theta += ((*r_theta) - func) / (*d_func_d_theta);
160 }
161
162 return theta;
163}
164
165static inline bool
166kb4_unproject(const struct t_camera_model_params *dist, //
167 const float x, //
168 const float y, //
169 float *out_x, //
170 float *out_y, //
171 float *out_z)
172{
173 const float mx = (x - dist->cx) / dist->fx;
174 const float my = (y - dist->cy) / dist->fy;
175
176 float theta = 0.0;
177 float sin_theta = 0.0;
178 float cos_theta = 1.0;
179 float thetad = sqrt(mx * mx + my * my);
180 float scaling = 1.0;
181 float d_func_d_theta = 0.0;
182
183 if (thetad > SQRT_EPSILON) {
184 theta = kb4_solve_theta(dist, &thetad, &d_func_d_theta);
185
186 sin_theta = sin(theta);
187 cos_theta = cos(theta);
188 scaling = sin_theta / thetad;
189 }
190
191 *out_x = mx * scaling;
192 *out_y = my * scaling;
193 *out_z = cos_theta;
194
195 //!@todo I'm not 100% sure if kb4 is always non-injective. basalt-headers always returns true here, so it might
196 //! be wrong too.
197 return true;
198}
199
200static inline void
201kb4_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
211/*
212 * Functions for radial-tangential (un)projections
213 */
214
215static inline bool
216rt8_project(const struct t_camera_model_params *dist, //
217 const float x, //
218 const float y, //
219 const float z, //
220 float *out_x, //
221 float *out_y)
222{
223 const float xp = x / z;
224 const float yp = y / z;
225 const float rp2 = xp * xp + yp * yp;
226 const float cdist = (1.0f + rp2 * (dist->rt8.k1 + rp2 * (dist->rt8.k2 + rp2 * dist->rt8.k3))) /
227 (1.0f + rp2 * (dist->rt8.k4 + rp2 * (dist->rt8.k5 + rp2 * dist->rt8.k6)));
228 const float deltaX = 2.0f * dist->rt8.p1 * xp * yp + dist->rt8.p2 * (rp2 + 2.0f * xp * xp);
229 const float deltaY = 2.0f * dist->rt8.p2 * xp * yp + dist->rt8.p1 * (rp2 + 2.0f * yp * yp);
230 const float xpp = xp * cdist + deltaX;
231 const float ypp = yp * cdist + deltaY;
232 const float u = dist->fx * xpp + dist->cx;
233 const float v = dist->fy * ypp + dist->cy;
234
235 *out_x = u;
236 *out_y = v;
237
238 const float rpmax = dist->rt8.metric_radius;
239 bool positive_z = z >= SQRT_EPSILON; // Sophus::Constants<Scalar>::epsilonSqrt();
240 bool in_injective_area = rpmax == 0.0f ? true : rp2 <= rpmax * rpmax;
241 bool is_valid = positive_z && in_injective_area;
242 return is_valid;
243}
244
245static inline void
246rt8_distort(const struct t_camera_model_params *params,
247 const struct xrt_vec2 *undist,
248 struct xrt_vec2 *dist,
249 struct xrt_matrix_2x2 *d_dist_d_undist)
250{
251 const float k1 = params->rt8.k1;
252 const float k2 = params->rt8.k2;
253 const float p1 = params->rt8.p1;
254 const float p2 = params->rt8.p2;
255 const float k3 = params->rt8.k3;
256 const float k4 = params->rt8.k4;
257 const float k5 = params->rt8.k5;
258 const float k6 = params->rt8.k6;
259
260 const float xp = undist->x;
261 const float yp = undist->y;
262 const float rp2 = xp * xp + yp * yp;
263 const float cdist = (1.0f + rp2 * (k1 + rp2 * (k2 + rp2 * k3))) / (1.0f + rp2 * (k4 + rp2 * (k5 + rp2 * k6)));
264 const float deltaX = 2.0f * p1 * xp * yp + p2 * (rp2 + 2.0f * xp * xp);
265 const float deltaY = 2.0f * p2 * xp * yp + p1 * (rp2 + 2.0f * yp * yp);
266 const float xpp = xp * cdist + deltaX;
267 const float ypp = yp * cdist + deltaY;
268 dist->x = xpp;
269 dist->y = ypp;
270
271 // Jacobian part!
272 // Expressions derived with sympy
273 const float v0 = xp * xp;
274 const float v1 = yp * yp;
275 const float v2 = v0 + v1;
276 const float v3 = k6 * v2;
277 const float v4 = k4 + v2 * (k5 + v3);
278 const float v5 = v2 * v4 + 1.0f;
279 const float v6 = v5 * v5;
280 const float v7 = 1.0f / v6;
281 const float v8 = p1 * yp;
282 const float v9 = p2 * xp;
283 const float v10 = 2.0f * v6;
284 const float v11 = k3 * v2;
285 const float v12 = k1 + v2 * (k2 + v11);
286 const float v13 = v12 * v2 + 1.0f;
287 const float v14 = v13 * (v2 * (k5 + 2.0f * v3) + v4);
288 const float v15 = 2.0f * v14;
289 const float v16 = v12 + v2 * (k2 + 2.0f * v11);
290 const float v17 = 2.0f * v16;
291 const float v18 = xp * yp;
292 const float v19 = 2.0f * v7 * (-v14 * v18 + v16 * v18 * v5 + v6 * (p1 * xp + p2 * yp));
293
294 const float dxpp_dxp = v7 * (-v0 * v15 + v10 * (v8 + 3.0f * v9) + v5 * (v0 * v17 + v13));
295 const float dxpp_dyp = v19;
296 const float dypp_dxp = v19;
297 const float dypp_dyp = v7 * (-v1 * v15 + v10 * (3.0f * v8 + v9) + v5 * (v1 * v17 + v13));
298
299 d_dist_d_undist->v[0] = dxpp_dxp;
300 d_dist_d_undist->v[1] = dxpp_dyp;
301 d_dist_d_undist->v[2] = dypp_dxp;
302 d_dist_d_undist->v[3] = dypp_dyp;
303}
304
305static inline void
306rt8_undistort(const struct t_camera_model_params *hg_dist, const float u, const float v, float *out_x, float *out_y)
307{
308 const float x0 = (u - hg_dist->cx) / hg_dist->fx;
309 const float y0 = (v - hg_dist->cy) / hg_dist->fy;
310
311 //! @todo Decide if besides rpmax, it could be useful to have an rppmax
312 //! field. A good starting point to having this would be using the sqrt of
313 //! the max rpp2 value computed in the optimization of `computeRpmax()`.
314
315 // Newton solver
316 struct xrt_vec2 dist = {x0, y0};
317 struct xrt_vec2 undist = dist;
318
319 const int N = 5; // Max iterations
320 for (int i = 0; i < N; i++) {
321 struct xrt_matrix_2x2 J;
322 struct xrt_vec2 fundist;
323
324 rt8_distort(hg_dist, &undist, &fundist, &J);
325 struct xrt_vec2 residual = m_vec2_sub(fundist, dist);
326
327 // fundist - dist;
328 struct xrt_matrix_2x2 J_inverse;
329
330 m_mat2x2_invert(&J, &J_inverse);
331
332 struct xrt_vec2 undist_sub;
333
334 m_mat2x2_transform_vec2(&J_inverse, &residual, &undist_sub);
335
336 undist = m_vec2_sub(undist, undist_sub);
337 if (m_vec2_len(residual) < SQRT_EPSILON) {
338 break;
339 }
340 }
341 *out_x = undist.x;
342 *out_y = undist.y;
343}
344
345static inline bool
346rt8_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;
350
351 rt8_undistort(hg_dist, u, v, &xp, &yp);
352
353 const float norm_inv = 1.0f / sqrt(xp * xp + yp * yp + 1.0f);
354 *out_x = xp * norm_inv;
355 *out_y = yp * norm_inv;
356 *out_z = norm_inv;
357
358 const float rp2 = xp * xp + yp * yp;
359 bool in_injective_area =
360 hg_dist->rt8.metric_radius == 0.0f ? true : rp2 <= hg_dist->rt8.metric_radius * hg_dist->rt8.metric_radius;
361 bool is_valid = in_injective_area;
362
363 return is_valid;
364}
365
366#if 0
367static inline bool
368zero_distortion_pinhole_project(const struct t_camera_model_params *dist, //
369 const float x, //
370 const float y, //
371 const float z, //
372 float *out_x, //
373 float *out_y)
374{
375 *out_x = ((dist->fx * x / z) + dist->cx);
376 *out_y = ((dist->fy * y / z) + dist->cy);
377
378 bool is_valid = z >= SQRT_EPSILON;
379 return is_valid;
380}
381static inline bool
382zero_distortion_pinhole_unproject(const struct t_camera_model_params *dist, //
383 const float x, //
384 const float y, //
385 float *out_x, //
386 float *out_y, //
387 float *out_z)
388{
389 const float mx = (x - dist->cx) / dist->fx;
390 const float my = (y - dist->cy) / dist->fy;
391
392 const float r2 = mx * mx + my * my;
393
394 const float norm = sqrtf(1.0f + r2);
395
396 const float norm_inv = 1.0f / norm;
397
398 *out_x = mx * norm_inv;
399 *out_y = my * norm_inv;
400 *out_z = norm_inv;
401
402 // Pinhole unprojection is always valid :)
403 return true;
404}
405#endif
406
407/*
408 * Misc functions.
409 */
410
411static inline void
412interpret_as_rt8(const struct t_camera_calibration *cc, struct t_camera_model_params *out_params)
413{
414 // Make a temporary buffer that definitely has zeros in it.
415 double distortion_tmp[XRT_DISTORTION_MAX_DIM] = {0};
416
417 if (cc->distortion_model != T_DISTORTION_OPENCV_RADTAN_8) {
418 U_LOG_W("Reinterpreting %s distortion as %s", t_stringify_camera_distortion_model(cc->distortion_model),
419 t_stringify_camera_distortion_model(T_DISTORTION_OPENCV_RADTAN_8));
420 }
421
422 size_t dist_num = t_num_params_from_distortion_model(cc->distortion_model);
423
424 for (size_t i = 0; i < dist_num; i++) {
425 // Copy only the valid values over. The high indices will be zero, which means that rt4 and rt5
426 // calibrations will work correctly.
427 distortion_tmp[i] = cc->distortion_parameters_as_array[i];
428 }
429
430 int acc_idx = 0;
431 out_params->rt8.k1 = distortion_tmp[acc_idx++];
432 out_params->rt8.k2 = distortion_tmp[acc_idx++];
433 out_params->rt8.p1 = distortion_tmp[acc_idx++];
434 out_params->rt8.p2 = distortion_tmp[acc_idx++];
435 out_params->rt8.k3 = distortion_tmp[acc_idx++];
436 out_params->rt8.k4 = distortion_tmp[acc_idx++];
437 out_params->rt8.k5 = distortion_tmp[acc_idx++];
438 out_params->rt8.k6 = distortion_tmp[acc_idx++];
439
440
441
442 if (cc->distortion_model == T_DISTORTION_WMR) {
443 out_params->rt8.metric_radius = cc->wmr.rpmax;
444 } else {
445 out_params->rt8.metric_radius = 0;
446 }
447 out_params->model = T_DISTORTION_OPENCV_RADTAN_8;
448}
449
450/*
451 * "Exported" functions.
452 */
453
454/*!
455 * Takes a @ref t_camera_calibration through \p cc, and returns a @ref t_camera_model_params that shadows \p cc
456 * 's parameters through \p out_params
457 */
458static inline void
459t_camera_model_params_from_t_camera_calibration(const struct t_camera_calibration *cc,
460 struct t_camera_model_params *out_params)
461{
462 // Paranoia.
463 U_ZERO(out_params);
464
465 // First row, first column.
466 out_params->fx = (float)cc->intrinsics[0][0];
467 // Second row, second column.
468 out_params->fy = (float)cc->intrinsics[1][1];
469 // First row, third column.
470 out_params->cx = (float)cc->intrinsics[0][2];
471 // Second row, third column.
472 out_params->cy = (float)cc->intrinsics[1][2];
473
474 out_params->model = cc->distortion_model;
475
476
477
478 switch (cc->distortion_model) {
479 case T_DISTORTION_FISHEYE_KB4: {
480 out_params->fisheye.k1 = (float)cc->kb4.k1;
481 out_params->fisheye.k2 = (float)cc->kb4.k2;
482 out_params->fisheye.k3 = (float)cc->kb4.k3;
483 out_params->fisheye.k4 = (float)cc->kb4.k4;
484 } break;
485 case T_DISTORTION_OPENCV_RADTAN_14:
486 case T_DISTORTION_OPENCV_RADTAN_5:
487 case T_DISTORTION_OPENCV_RADTAN_8:
488 case T_DISTORTION_WMR: interpret_as_rt8(cc, out_params); break;
489 default:
490 U_LOG_E("t_camera_un_projections doesn't support camera model %s yet!",
491 t_stringify_camera_distortion_model(cc->distortion_model));
492 break;
493 }
494}
495
496/*!
497 * Takes a 2D image-space point through \p x and \p y, unprojects it to a normalized 3D direction, and returns
498 * the result through \p out_x, \p out_y and \p out_z
499 */
500static inline bool
501t_camera_models_unproject(
502 const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y, float *out_z)
503{
504 switch (dist->model) {
505 case T_DISTORTION_OPENCV_RADTAN_8: {
506 return rt8_unproject(dist, x, y, out_x, out_y, out_z);
507 }; break;
508 case T_DISTORTION_FISHEYE_KB4: {
509 return kb4_unproject(dist, x, y, out_x, out_y, out_z);
510 }; break;
511 // Return false so we don't get warnings on Release builds.
512 default: assert(false); return false;
513 }
514}
515
516/*!
517 * Takes a 2D image-space point through \p x and \p y, unprojects it to a normalized 3D direction, flips its Y
518 * and Z values (performing a coordinate space transform from +Z forward -Y up to -Z forward +Y up) and returns the
519 * result through \p out_x, \p out_y and \p out_z
520 */
521static inline bool
522t_camera_models_unproject_and_flip(
523 const struct t_camera_model_params *dist, const float x, const float y, float *out_x, float *out_y, float *out_z)
524{
525 bool ret = t_camera_models_unproject(dist, x, y, out_x, out_y, out_z);
526
527 *out_z *= -1;
528 *out_y *= -1;
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 */
536static inline void
537t_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 }
550}
551
552
553/*!
554 * Takes a 3D point through \p x, \p y, and \p z, projects it into image space, and returns the result
555 * through \p out_x and \p out_y
556 */
557static inline bool
558t_camera_models_project(const struct t_camera_model_params *dist, //
559 const float x, //
560 const float y, //
561 const float z, //
562 float *out_x, //
563 float *out_y)
564{
565 switch (dist->model) {
566 case T_DISTORTION_OPENCV_RADTAN_8: {
567 return rt8_project(dist, x, y, z, out_x, out_y);
568 }; break;
569 case T_DISTORTION_FISHEYE_KB4: {
570 return kb4_project(dist, x, y, z, out_x, out_y);
571 }; break;
572 // Return false so we don't get warnings on Release builds.
573 default: assert(false); return false;
574 }
575}
576
577/*!
578 * Takes a 3D point through \p x, \p y, and \p z, flips its Y and Z values (performing a coordinate space
579 * transform from -Z forward +Y up to +Z forward -Y up), projects it into image space, and returns the result through
580 * \p out_x and \p out_y
581 */
582static inline bool
583t_camera_models_flip_and_project(const struct t_camera_model_params *dist, //
584 const float x, //
585 const float y, //
586 const float z, //
587 float *out_x, //
588 float *out_y)
589{
590 float _y = y * -1;
591 float _z = z * -1;
592
593 return t_camera_models_project(dist, x, _y, _z, out_x, out_y);
594}
595
596#ifdef __cplusplus
597}
598#endif