The open source OpenXR runtime
aux/tracking: Port to u_logging.
authored by
Lubosz Sarnecki
and committed by
Jakob Bornecrantz
58ecf675
e350e42c
+62
-84
+10
-11
src/xrt/auxiliary/tracking/t_calibration.cpp
+10
-11
src/xrt/auxiliary/tracking/t_calibration.cpp
···
654
654
cv::Mat distortion_fisheye_mat = {};
655
655
656
656
if (c.dump_measurements) {
657
-
printf("...measured = (ArrayOfMeasurements){\n");
657
+
U_LOG_RAW("...measured = (ArrayOfMeasurements){");
658
658
for (MeasurementF32 &m : view.measured_f32) {
659
-
printf(" {\n");
659
+
U_LOG_RAW(" {");
660
660
for (cv::Point2f &p : m) {
661
-
printf(" {%+ff, %+ff},\n", p.x, p.y);
661
+
U_LOG_RAW(" {%+ff, %+ff},", p.x, p.y);
662
662
}
663
-
printf(" },\n");
663
+
U_LOG_RAW(" },");
664
664
}
665
-
printf("};\n");
665
+
U_LOG_RAW("};");
666
666
}
667
667
668
668
if (c.use_fisheye) {
···
1149
1149
c.gray = cv::imread(buf, cv::IMREAD_GRAYSCALE);
1150
1150
1151
1151
if (c.gray.rows == 0 || c.gray.cols == 0) {
1152
-
fprintf(stderr, "Could not find image '%s'!\n", buf);
1152
+
U_LOG_E("Could not find image '%s'!", buf);
1153
1153
continue;
1154
1154
}
1155
1155
1156
1156
if (c.gray.rows != (int)xf->height ||
1157
1157
c.gray.cols != (int)xf->width) {
1158
-
fprintf(stderr,
1159
-
"Image size does not match frame size! Image: "
1160
-
"(%ix%i) Frame: (%ux%u)\n",
1161
-
c.gray.cols, c.gray.rows, xf->width,
1162
-
xf->height);
1158
+
U_LOG_E(
1159
+
"Image size does not match frame size! Image: "
1160
+
"(%ix%i) Frame: (%ux%u)",
1161
+
c.gray.cols, c.gray.rows, xf->width, xf->height);
1163
1162
continue;
1164
1163
}
1165
1164
+1
-4
src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp
+1
-4
src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp
···
141
141
switch (xf->format) {
142
142
case XRT_FORMAT_YUV888: process_frame_yuv(d, xf); break;
143
143
case XRT_FORMAT_YUYV422: process_frame_yuyv(d, xf); break;
144
-
default:
145
-
fprintf(stderr, "ERROR: Bad format '%s'",
146
-
u_format_str(xf->format));
147
-
break;
144
+
default: U_LOG_E("Bad format '%s'", u_format_str(xf->format)); break;
148
145
}
149
146
}
150
147
+16
-16
src/xrt/auxiliary/tracking/t_file.cpp
+16
-16
src/xrt/auxiliary/tracking/t_file.cpp
···
11
11
12
12
#include "tracking/t_calibration_opencv.hpp"
13
13
#include "util/u_misc.h"
14
-
14
+
#include "util/u_logging.h"
15
15
16
16
/*
17
17
*
···
213
213
result = result && read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
214
214
215
215
if (!result) {
216
-
fprintf(stderr, "\tRe-run calibration!\n");
216
+
U_LOG_W("Re-run calibration!");
217
217
return false;
218
218
}
219
219
wrapped.view[0].image_size_pixels.w = uint32_t(mat_image_size(0, 0));
···
226
226
}
227
227
228
228
if (!read_cv_mat(calib_file, &wrapped.camera_translation_mat, "translation")) {
229
-
fprintf(stderr, "\tRe-run calibration!\n");
229
+
U_LOG_W("Re-run calibration!");
230
230
}
231
231
if (!read_cv_mat(calib_file, &wrapped.camera_rotation_mat, "rotation")) {
232
-
fprintf(stderr, "\tRe-run calibration!\n");
232
+
U_LOG_W("Re-run calibration!");
233
233
}
234
234
if (!read_cv_mat(calib_file, &wrapped.camera_essential_mat, "essential")) {
235
-
fprintf(stderr, "\tRe-run calibration!\n");
235
+
U_LOG_W("Re-run calibration!");
236
236
}
237
237
if (!read_cv_mat(calib_file, &wrapped.camera_fundamental_mat, "fundamental")) {
238
-
fprintf(stderr, "\tRe-run calibration!\n");
238
+
U_LOG_W("Re-run calibration!");
239
239
}
240
240
241
241
cv::Mat_<float> mat_use_fisheye(1, 1);
242
242
if (!read_cv_mat(calib_file, &mat_use_fisheye, "use_fisheye")) {
243
243
wrapped.view[0].use_fisheye = false;
244
-
fprintf(stderr, "\tRe-run calibration! (Assuming not fisheye)\n");
244
+
U_LOG_W("Re-run calibration! (Assuming not fisheye)");
245
245
} else {
246
246
wrapped.view[0].use_fisheye = mat_use_fisheye(0, 0) != 0.0f;
247
247
}
···
338
338
cv::Mat temp;
339
339
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
340
340
if (read != 3) {
341
-
printf("Failed to read mat header: '%i' '%s'\n", (int)read,
342
-
name);
341
+
U_LOG_E("Failed to read mat header: '%i' '%s'", (int)read,
342
+
name);
343
343
return false;
344
344
}
345
345
···
358
358
read = fread(static_cast<void *>(temp.data), header[0],
359
359
header[1] * header[2], f);
360
360
if (read != (header[1] * header[2])) {
361
-
printf("Failed to read mat body: '%i' '%s'\n", (int)read, name);
361
+
U_LOG_E("Failed to read mat body: '%i' '%s'", (int)read, name);
362
362
return false;
363
363
}
364
364
if (m->empty()) {
365
365
m->create(header[1], header[2], temp.type());
366
366
}
367
367
if (temp.type() != m->type()) {
368
-
printf("Mat body type does not match: %i vs %i for '%s'\n",
369
-
(int)temp.type(), (int)m->type(), name);
368
+
U_LOG_E("Mat body type does not match: %i vs %i for '%s'",
369
+
(int)temp.type(), (int)m->type(), name);
370
370
return false;
371
371
}
372
372
if (temp.total() != m->total()) {
373
-
printf("Mat total size does not match: %i vs %i for '%s'\n",
374
-
(int)temp.total(), (int)m->total(), name);
373
+
U_LOG_E("Mat total size does not match: %i vs %i for '%s'",
374
+
(int)temp.total(), (int)m->total(), name);
375
375
return false;
376
376
}
377
377
if (temp.size() == m->size()) {
···
381
381
}
382
382
if (temp.size().width == m->size().height &&
383
383
temp.size().height == m->size().width) {
384
-
printf("Mat transposing on load: '%s'\n", name);
384
+
U_LOG_W("Mat transposing on load: '%s'", name);
385
385
// needs transpose
386
386
cv::transpose(temp, *m);
387
387
return true;
388
388
}
389
389
// highly unlikely so minimally-helpful error message.
390
-
printf("Mat dimension unknown mismatch: '%s'\n", name);
390
+
U_LOG_E("Mat dimension unknown mismatch: '%s'", name);
391
391
return false;
392
392
}
+1
-4
src/xrt/auxiliary/tracking/t_hsv_filter.c
+1
-4
src/xrt/auxiliary/tracking/t_hsv_filter.c
···
306
306
ensure_buf_allocated(f, xf);
307
307
process_frame_yuyv(f, xf);
308
308
break;
309
-
default:
310
-
fprintf(stderr, "ERROR: Bad format '%s'",
311
-
u_format_str(xf->format));
312
-
return;
309
+
default: U_LOG_E("Bad format '%s'", u_format_str(xf->format)); return;
313
310
}
314
311
315
312
push_buf(f, xf, f->sinks[0], &f->frame0);
+11
-23
src/xrt/auxiliary/tracking/t_imu_fusion.hpp
+11
-23
src/xrt/auxiliary/tracking/t_imu_fusion.hpp
···
18
18
#include "math/m_api.h"
19
19
#include "util/u_time.h"
20
20
#include "util/u_debug.h"
21
+
#include "util/u_logging.h"
21
22
22
23
#include <Eigen/Core>
23
24
#include <Eigen/Geometry>
24
25
25
26
#include "flexkalman/EigenQuatExponentialMap.h"
26
27
27
-
DEBUG_GET_ONCE_BOOL_OPTION(simple_imu_debug, "SIMPLE_IMU_DEBUG", false)
28
-
DEBUG_GET_ONCE_BOOL_OPTION(simple_imu_spew, "SIMPLE_IMU_SPEW", false)
28
+
DEBUG_GET_ONCE_LOG_OPTION(simple_imu_log, "SIMPLE_IMU_LOG", U_LOGGING_WARN)
29
29
30
-
#define SIMPLE_IMU_DEBUG(MSG) \
31
-
do { \
32
-
if (debug_) { \
33
-
printf("SimpleIMU(%p): " MSG "\n", \
34
-
(const void *)this); \
35
-
} \
36
-
} while (0)
37
-
38
-
#define SIMPLE_IMU_SPEW(MSG) \
39
-
do { \
40
-
if (spew_) { \
41
-
printf("SimpleIMU(%p): " MSG "\n", \
42
-
(const void *)this); \
43
-
} \
44
-
} while (0)
30
+
#define SIMPLE_IMU_TRACE(...) U_LOG_IFL_T(ll, __VA_ARGS__)
31
+
#define SIMPLE_IMU_DEBUG(...) U_LOG_IFL_D(ll, __VA_ARGS__)
32
+
#define SIMPLE_IMU_INFO(...) U_LOG_IFL_I(ll, __VA_ARGS__)
33
+
#define SIMPLE_IMU_WARN(...) U_LOG_IFL_W(ll, __VA_ARGS__)
34
+
#define SIMPLE_IMU_ERROR(...) U_LOG_IFL_E(ll, __VA_ARGS__)
45
35
46
36
namespace xrt_fusion {
47
37
class SimpleIMUFusion
···
54
44
*/
55
45
explicit SimpleIMUFusion(double gravity_rate = 0.9)
56
46
: gravity_scale_(gravity_rate),
57
-
debug_(debug_get_bool_option_simple_imu_debug()),
58
-
spew_(debug_get_bool_option_simple_imu_spew())
47
+
ll(debug_get_log_option_simple_imu_log())
59
48
{
60
49
SIMPLE_IMU_DEBUG("Creating instance");
61
50
}
···
212
201
uint64_t last_gyro_timestamp_{0};
213
202
double gyro_min_squared_length_{1.e-8};
214
203
bool started_{false};
215
-
bool debug_{false};
216
-
bool spew_{false};
204
+
enum u_logging_level ll;
217
205
};
218
206
219
207
inline Eigen::Quaterniond
···
254
242
Eigen::Vector3d incRot = gyro * dt;
255
243
// Crude handling of "approximately zero"
256
244
if (incRot.squaredNorm() < gyro_min_squared_length_) {
257
-
SIMPLE_IMU_SPEW(
245
+
SIMPLE_IMU_TRACE(
258
246
"Discarding gyro data that is approximately zero");
259
247
return false;
260
248
}
···
310
298
if (scale <= 0) {
311
299
// Too far from gravity to be useful/trusted for orientation
312
300
// purposes.
313
-
SIMPLE_IMU_SPEW("Too far from gravity to be useful/trusted.");
301
+
SIMPLE_IMU_TRACE("Too far from gravity to be useful/trusted.");
314
302
return false;
315
303
}
316
304
+2
-4
src/xrt/auxiliary/tracking/t_tracker_hand.cpp
+2
-4
src/xrt/auxiliary/tracking/t_tracker_hand.cpp
···
163
163
int rect_rows = t.view[0].undistort_rectify_map_x.rows;
164
164
165
165
if (cols != rect_cols || rows != rect_rows) {
166
-
fprintf(stderr,
167
-
"Error: %dx%d rectification matrix does not fit %dx%d "
168
-
"Image\n",
166
+
U_LOG_E("%dx%d rectification matrix does not fit %dx%d Image",
169
167
rect_cols, rect_rows, cols, rows);
170
168
return;
171
169
}
···
347
345
struct xrt_tracked_hand **out_xth,
348
346
struct xrt_frame_sink **out_sink)
349
347
{
350
-
fprintf(stderr, "%s\n", __func__);
348
+
U_LOG_D("Creating hand tracker.");
351
349
352
350
auto &t = *(new TrackerHand());
353
351
int ret;
+1
-1
src/xrt/auxiliary/tracking/t_tracker_psmv.cpp
+1
-1
src/xrt/auxiliary/tracking/t_tracker_psmv.cpp
+12
-13
src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp
+12
-13
src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp
···
142
142
orientation_state.tracked = true;
143
143
orientation_state.valid = true;
144
144
} else {
145
-
fprintf(stderr,
146
-
"Got non-finite something when filtering IMU - "
147
-
"resetting filter and IMU fusion!\n");
145
+
U_LOG_E(
146
+
"Got non-finite something when filtering IMU - "
147
+
"resetting filter and IMU fusion!");
148
148
reset_filter_and_imu();
149
149
}
150
150
// 7200 deg/sec
151
151
constexpr double max_rad_per_sec = 20 * double(EIGEN_PI) * 2;
152
152
if (filter_state.angularVelocity().squaredNorm() >
153
153
max_rad_per_sec * max_rad_per_sec) {
154
-
fprintf(stderr,
155
-
"Got excessive angular velocity when filtering "
156
-
"IMU - resetting filter and IMU fusion!\n");
154
+
U_LOG_E(
155
+
"Got excessive angular velocity when filtering "
156
+
"IMU - resetting filter and IMU fusion!");
157
157
reset_filter_and_imu();
158
158
}
159
159
}
···
183
183
184
184
if (resid > residual_limit) {
185
185
// Residual arbitrarily "too large"
186
-
fprintf(
187
-
stderr,
188
-
"Warning - measurement residual is %f, resetting "
189
-
"filter state\n",
186
+
U_LOG_W(
187
+
"measurement residual is %f, resetting "
188
+
"filter state",
190
189
resid);
191
190
reset_filter();
192
191
return;
···
196
195
position_state.valid = true;
197
196
position_state.tracked = true;
198
197
} else {
199
-
fprintf(stderr,
200
-
"Got non-finite something when filtering "
201
-
"tracker - resetting filter!\n");
198
+
U_LOG_W(
199
+
"Got non-finite something when filtering "
200
+
"tracker - resetting filter!");
202
201
reset_filter();
203
202
}
204
203
}
+8
-8
src/xrt/auxiliary/tracking/t_tracker_psvr.cpp
+8
-8
src/xrt/auxiliary/tracking/t_tracker_psvr.cpp
···
585
585
sqrt((error_x * error_x) + (error_y * error_y) +
586
586
(error_z * error_z));
587
587
588
-
// printf("ERROR: %f %f %f %f %f %f\n", temp_points[i].p.x,
588
+
// U_LOG_D("%f %f %f %f %f %f", temp_points[i].p.x,
589
589
// temp_points[i].p.y, temp_points[i].p.z, error_x,
590
590
// error_y, error_z);
591
591
if (rms_error < outlier_thresh) {
···
993
993
solve_for_measurement(&t, measured_points, solved);
994
994
float diff = last_diff(t, solved, &t.last_vertices);
995
995
if (diff < PSVR_HOLD_THRESH) {
996
-
// printf("diff from last: %f\n", diff);
996
+
// U_LOG_D("diff from last: %f", diff);
997
997
998
998
return res;
999
999
}
···
1105
1105
// 'bottom
1106
1106
1107
1107
if (has_bl && has_tl && bl_pos.y() > tl_pos.y()) {
1108
-
// printf("IGNORING BL > TL %f %f\n",
1108
+
// U_LOG_D("IGNORING BL > TL %f %f",
1109
1109
// bl_pos.y(),
1110
1110
// br_pos.y());
1111
1111
// ignore = true;
1112
1112
}
1113
1113
if (has_br && has_tr && br_pos.y() > tr_pos.y()) {
1114
-
// printf("IGNORING TL > TR %f %f\n",
1114
+
// U_LOG_D("IGNORING TL > TR %f %f",
1115
1115
// tl_pos.y(),
1116
1116
// tr_pos.y());
1117
1117
// ignore = true;
···
1127
1127
}
1128
1128
1129
1129
// useful for debugging
1130
-
// printf(
1130
+
// U_LOG_D(
1131
1131
// "match %d dist to last: %f dist to imu: %f "
1132
-
// "rmsError: %f squaredSum:%f %d\n",
1132
+
// "rmsError: %f squaredSum:%f %d",
1133
1133
// i, prev_diff, imu_diff, avg_error, error_sum,
1134
1134
// ignore);
1135
1135
}
···
1143
1143
}
1144
1144
}
1145
1145
1146
-
// printf("lowest_error %f\n", lowest_error);
1146
+
// U_LOG_D("lowest_error %f", lowest_error);
1147
1147
if (best_model == -1) {
1148
1148
PSVR_INFO("COULD NOT MATCH MODEL!");
1149
1149
return Eigen::Matrix4f().Identity();
···
1698
1698
cv::KeyPoint rkp = t.view[1].keypoints.at(r_index);
1699
1699
t.l_blobs.push_back(lkp);
1700
1700
t.r_blobs.push_back(rkp);
1701
-
// printf("2D coords: LX %f LY %f RX %f RY %f\n",
1701
+
// U_LOG_D("2D coords: LX %f LY %f RX %f RY %f",
1702
1702
// lkp.pt.x,
1703
1703
// lkp.pt.y, rkp.pt.x, rkp.pt.y);
1704
1704
}