The open source OpenXR runtime
h/mercury: Dynamically allocate arrays for stereographic projection
Also factor out stereographic_unprojection into its own file because we're gonna need it elsewhere
+110
-73
+82
-73
src/xrt/tracking/hand/mercury/hg_image_distorter.cpp
+82
-73
src/xrt/tracking/hand/mercury/hg_image_distorter.cpp
···
29
29
#include <iostream>
30
30
#include <opencv2/opencv.hpp>
31
31
32
-
//!@todo This is the wrong way of doing it; we should probably be allocating some space up front for each
33
-
//! keypoint estimator to scratch into. Patches welcome!
34
-
#define EIGEN_STACK_ALLOCATION_LIMIT 128 * 128 * 4 * 3
35
-
36
32
#include "math/m_eigen_interop.hpp"
37
33
#include "hg_sync.hpp"
34
+
#include "hg_stereographic_unprojection.hpp"
38
35
39
36
namespace xrt::tracking::hand::mercury {
40
37
···
43
40
template <typename T> using OutputSizedArray = Eigen::Array<T, wsize, wsize, Eigen::RowMajor>;
44
41
using OutputSizedFloatArray = OutputSizedArray<float>;
45
42
43
+
#define ARRAY_STACK_SIZE 20
44
+
45
+
struct ArrayStack
46
+
{
47
+
OutputSizedFloatArray arrays[ARRAY_STACK_SIZE];
48
+
size_t array_idx = 0;
49
+
50
+
OutputSizedFloatArray &
51
+
get()
52
+
{
53
+
if (array_idx == ARRAY_STACK_SIZE) {
54
+
abort();
55
+
}
56
+
return this->arrays[this->array_idx++];
57
+
};
58
+
59
+
void
60
+
dropAll()
61
+
{
62
+
this->array_idx = 0;
63
+
}
64
+
};
65
+
46
66
struct projection_state
47
67
{
48
68
cv::Mat &input;
···
51
71
52
72
const projection_instructions &instructions;
53
73
74
+
ArrayStack stack = {};
75
+
76
+
OutputSizedArray<int16_t> image_x = {};
77
+
OutputSizedArray<int16_t> image_y = {};
78
+
54
79
projection_state(const projection_instructions &instructions, cv::Mat &input, cv::Mat &output)
55
80
: input(input), distorted_image_eigen(output.data, 128, 128), instructions(instructions){};
56
81
};
···
58
83
59
84
// A private, purpose-optimized version of the Kannalla-Brandt projection function.
60
85
static void
61
-
project_kb4(const t_camera_model_params &dist, //
62
-
const OutputSizedFloatArray &x, //
63
-
const OutputSizedFloatArray &y, //
64
-
const OutputSizedFloatArray &z, //
65
-
OutputSizedFloatArray &out_x, //
86
+
project_kb4(projection_state &mi,
87
+
const OutputSizedFloatArray &x, //
88
+
const OutputSizedFloatArray &y, //
89
+
const OutputSizedFloatArray &z, //
90
+
OutputSizedFloatArray &out_x, //
66
91
OutputSizedFloatArray &out_y)
67
92
{
93
+
const t_camera_model_params &dist = mi.dist;
94
+
OutputSizedFloatArray r2 = mi.stack.get();
95
+
OutputSizedFloatArray r = mi.stack.get();
68
96
69
-
const OutputSizedFloatArray r2 = x * x + y * y;
70
-
const OutputSizedFloatArray r = sqrt(r2);
97
+
r2 = x * x + y * y;
98
+
r = sqrt(r2);
71
99
72
100
#if 0
73
101
const I theta = atan2(r, z);
···
81
109
// If neither of these were true we'd definitely need atan2.
82
110
//
83
111
// Grrr, we really need a good library for fast approximations of trigonometric functions.
84
-
85
-
const OutputSizedFloatArray theta = atan(r / z);
112
+
OutputSizedFloatArray theta = mi.stack.get();
113
+
theta = atan(r / z);
86
114
#endif
87
115
88
-
const OutputSizedFloatArray theta2 = theta * theta;
116
+
OutputSizedFloatArray theta2 = mi.stack.get();
117
+
theta2 = theta * theta;
89
118
90
119
91
120
#if 0
···
101
130
#else
102
131
// This version gives the compiler more options to do FMAs and avoid temporaries. Down to floating point
103
132
// precision this should give the same result as the above.
104
-
OutputSizedFloatArray r_theta =
133
+
OutputSizedFloatArray r_theta = mi.stack.get();
134
+
r_theta =
105
135
(((((dist.fisheye.k4 * theta2) + dist.fisheye.k3) * theta2 + dist.fisheye.k2) * theta2 + dist.fisheye.k1) *
106
136
theta2 +
107
137
1) *
108
138
theta;
109
139
#endif
110
140
111
-
const OutputSizedFloatArray mx = x * r_theta / r;
112
-
const OutputSizedFloatArray my = y * r_theta / r;
141
+
OutputSizedFloatArray mx = mi.stack.get();
142
+
mx = x * r_theta / r;
143
+
OutputSizedFloatArray my = mi.stack.get();
144
+
my = y * r_theta / r;
113
145
114
146
out_x = dist.fx * mx + dist.cx;
115
147
out_y = dist.fy * my + dist.cy;
116
148
}
117
149
118
-
119
-
120
-
Eigen::Vector3f
121
-
stereographic_to_direction(float sg_x, float sg_y)
122
-
{
123
-
float X = sg_x;
124
-
float Y = sg_y;
125
-
126
-
float denom = (1 + X * X + Y * Y);
127
-
128
-
float x = (2 * X) / denom;
129
-
float y = (2 * Y) / denom;
130
-
float z = (-1 + X * X + Y * Y) / denom;
131
-
132
-
// forward is -z
133
-
return {x, y, z};
134
-
// return {x / -z, y / -z};
135
-
}
136
-
137
-
138
-
139
150
template <typename T>
140
151
T
141
152
map_ranges(T value, T from_low, T from_high, T to_low, T to_high)
···
177
188
{
178
189
XRT_TRACE_MARKER();
179
190
180
-
OutputSizedFloatArray sg_x;
181
-
OutputSizedFloatArray sg_y;
191
+
OutputSizedFloatArray &sg_x = mi.stack.get();
192
+
OutputSizedFloatArray &sg_y = mi.stack.get();
182
193
183
194
// Please vectorize me?
184
195
if (mi.instructions.flip) {
···
205
216
// STEREOGRAPHIC DIRECTION TO 3D DIRECTION
206
217
// Note: we do not normalize the direction, because we don't need to. :)
207
218
208
-
OutputSizedFloatArray dir_x;
209
-
OutputSizedFloatArray dir_y;
210
-
OutputSizedFloatArray dir_z;
219
+
OutputSizedFloatArray &dir_x = mi.stack.get();
220
+
OutputSizedFloatArray &dir_y = mi.stack.get();
221
+
OutputSizedFloatArray &dir_z = mi.stack.get();
211
222
212
223
213
224
#if 0
···
224
235
// END STEREOGRAPHIC DIRECTION TO 3D DIRECTION
225
236
226
237
// QUATERNION ROTATING VECTOR
227
-
Eigen::Matrix<OutputSizedFloatArray, 3, 1> rot_dir;
238
+
OutputSizedFloatArray &rot_dir_x = mi.stack.get();
239
+
OutputSizedFloatArray &rot_dir_y = mi.stack.get();
240
+
OutputSizedFloatArray &rot_dir_z = mi.stack.get();
228
241
229
-
OutputSizedFloatArray uv0;
230
-
OutputSizedFloatArray uv1;
231
-
OutputSizedFloatArray uv2;
242
+
OutputSizedFloatArray &uv0 = mi.stack.get();
243
+
OutputSizedFloatArray &uv1 = mi.stack.get();
244
+
OutputSizedFloatArray &uv2 = mi.stack.get();
232
245
233
246
const Eigen::Quaternionf &q = mi.instructions.rot_quat;
234
247
···
240
253
uv1 += uv1;
241
254
uv2 += uv2;
242
255
243
-
rot_dir.x() = dir_x + q.w() * uv0;
244
-
rot_dir.y() = dir_y + q.w() * uv1;
245
-
rot_dir.z() = dir_z + q.w() * uv2;
256
+
rot_dir_x = dir_x + q.w() * uv0;
257
+
rot_dir_y = dir_y + q.w() * uv1;
258
+
rot_dir_z = dir_z + q.w() * uv2;
246
259
247
-
rot_dir.x() += q.y() * uv2 - q.z() * uv1;
248
-
rot_dir.y() += q.z() * uv0 - q.x() * uv2;
249
-
rot_dir.z() += q.x() * uv1 - q.y() * uv0;
260
+
rot_dir_x += q.y() * uv2 - q.z() * uv1;
261
+
rot_dir_y += q.z() * uv0 - q.x() * uv2;
262
+
rot_dir_z += q.x() * uv1 - q.y() * uv0;
250
263
// END QUATERNION ROTATING VECTOR
251
264
252
265
253
266
254
-
OutputSizedArray<int16_t> image_x;
255
-
OutputSizedArray<int16_t> image_y;
256
-
257
-
OutputSizedFloatArray image_x_f;
258
-
OutputSizedFloatArray image_y_f;
267
+
OutputSizedFloatArray &image_x_f = mi.stack.get();
268
+
OutputSizedFloatArray &image_y_f = mi.stack.get();
259
269
260
270
261
271
//!@todo optimize
262
-
rot_dir.y() *= -1;
263
-
rot_dir.z() *= -1;
272
+
rot_dir_y *= -1;
273
+
rot_dir_z *= -1;
264
274
265
275
{
266
276
XRT_TRACE_IDENT(camera_projection);
···
268
278
switch (mi.dist.model) {
269
279
case T_DISTORTION_FISHEYE_KB4:
270
280
// This takes 250us vs 500 because of the removed atan2.
271
-
project_kb4(mi.dist, rot_dir.x(), rot_dir.y(), rot_dir.z(), image_x_f, image_y_f);
281
+
project_kb4(mi, rot_dir_x, rot_dir_y, rot_dir_z, image_x_f, image_y_f);
272
282
break;
273
283
case T_DISTORTION_OPENCV_RADTAN_8:
274
284
// Regular C is plenty fast for radtan :)
275
285
for (int i = 0; i < image_x_f.rows(); i++) {
276
286
for (int j = 0; j < image_x_f.cols(); j++) {
277
-
t_camera_models_project(&mi.dist, rot_dir.x()(i, j), rot_dir.y()(i, j),
278
-
rot_dir.z()(i, j), &image_x_f(i, j), &image_y_f(i, j));
287
+
t_camera_models_project(&mi.dist, rot_dir_x(i, j), rot_dir_y(i, j),
288
+
rot_dir_z(i, j), &image_x_f(i, j), &image_y_f(i, j));
279
289
}
280
290
}
281
291
break;
···
283
293
}
284
294
}
285
295
286
-
image_x = image_x_f.cast<int16_t>();
287
-
image_y = image_y_f.cast<int16_t>();
296
+
mi.image_x = image_x_f.cast<int16_t>();
297
+
mi.image_y = image_y_f.cast<int16_t>();
288
298
289
-
naive_remap(image_x, image_y, mi.input, mi.distorted_image_eigen);
299
+
naive_remap(mi.image_x, mi.image_y, mi.input, mi.distorted_image_eigen);
290
300
}
291
301
292
302
···
300
310
float sg_y =
301
311
map_ranges<float>(y, 0, wsize, mi.instructions.stereographic_radius, -mi.instructions.stereographic_radius);
302
312
303
-
Eigen::Vector3f dir = stereographic_to_direction(sg_x, sg_y);
313
+
Eigen::Vector3f dir = stereographic_unprojection(sg_x, sg_y);
304
314
305
315
dir = mi.instructions.rot_quat * dir;
306
316
···
519
529
float r = fmax(center.x - min.x, center.y - min.y);
520
530
out_instructions.stereographic_radius = r;
521
531
522
-
Eigen::Vector3f new_direction = stereographic_to_direction(center.x, center.y);
532
+
Eigen::Vector3f new_direction = stereographic_unprojection(center.x, center.y);
523
533
524
534
new_direction = out_instructions.rot_quat * new_direction;
525
535
···
598
608
599
609
{
600
610
out = cv::Mat(cv::Size(wsize, wsize), CV_8U);
601
-
projection_state mi(instructions, input_image, out);
611
+
projection_state *mi_ptr = new projection_state(instructions, input_image, out);
612
+
projection_state &mi = *mi_ptr;
613
+
602
614
mi.dist = dist;
603
-
// Why am I doing it like this????
604
-
// mi.stereographic_radius = instructions.stereographic_radius;
605
-
// mi.rot_quat = instructions.rot_quat;
606
-
// mi.flip = instructions.flip;
607
615
608
616
StereographicDistort(mi);
609
617
610
618
if (debug_image) {
611
619
draw_boundary(mi, boundary_color, *debug_image);
612
620
}
621
+
delete mi_ptr;
613
622
}
614
623
} // namespace xrt::tracking::hand::mercury
+28
src/xrt/tracking/hand/mercury/hg_stereographic_unprojection.hpp
+28
src/xrt/tracking/hand/mercury/hg_stereographic_unprojection.hpp
···
1
+
// Copyright 2023, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Stereographic unprojection
6
+
* @author Moshi Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
10
+
#pragma once
11
+
#include "math/m_eigen_interop.hpp"
12
+
13
+
static inline Eigen::Vector3f
14
+
stereographic_unprojection(float sg_x, float sg_y)
15
+
{
16
+
float X = sg_x;
17
+
float Y = sg_y;
18
+
19
+
float denom = (1 + X * X + Y * Y);
20
+
21
+
float x = (2 * X) / denom;
22
+
float y = (2 * Y) / denom;
23
+
float z = (-1 + X * X + Y * Y) / denom;
24
+
25
+
// forward is -z
26
+
return {x, y, z};
27
+
// return {x / -z, y / -z};
28
+
}