The open source OpenXR runtime
t/file: Implement json save/load for v2 calibration
+287
-20
+1
-1
src/xrt/auxiliary/CMakeLists.txt
+1
-1
src/xrt/auxiliary/CMakeLists.txt
···
105
105
target_link_libraries(
106
106
aux_tracking
107
107
PUBLIC aux-includes
108
-
PRIVATE aux_math aux_util xrt-external-flexkalman xrt-external-hungarian
108
+
PRIVATE aux_math aux_util xrt-external-flexkalman xrt-external-hungarian xrt-external-cjson
109
109
)
110
110
target_include_directories(aux_tracking SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
111
111
if(XRT_HAVE_OPENCV)
+1
src/xrt/auxiliary/meson.build
+1
src/xrt/auxiliary/meson.build
+276
-19
src/xrt/auxiliary/tracking/t_file.cpp
+276
-19
src/xrt/auxiliary/tracking/t_file.cpp
···
12
12
#include "tracking/t_calibration_opencv.hpp"
13
13
#include "util/u_misc.h"
14
14
#include "util/u_logging.h"
15
+
#include "util/u_json.hpp"
16
+
#include "os/os_time.h"
15
17
16
18
19
+
DEBUG_GET_ONCE_LOG_OPTION(calib_log, "CALIB_LOG", U_LOGGING_WARN)
20
+
21
+
#define CALIB_TRACE(...) U_LOG_IFL_T(debug_get_log_option_calib_log(), __VA_ARGS__)
22
+
#define CALIB_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_calib_log(), __VA_ARGS__)
23
+
#define CALIB_INFO(...) U_LOG_IFL_I(debug_get_log_option_calib_log(), __VA_ARGS__)
24
+
#define CALIB_WARN(...) U_LOG_IFL_W(debug_get_log_option_calib_log(), __VA_ARGS__)
25
+
#define CALIB_ERROR(...) U_LOG_IFL_E(debug_get_log_option_calib_log(), __VA_ARGS__)
26
+
#define CALIB_ASSERT(predicate, ...) \
27
+
do { \
28
+
bool p = predicate; \
29
+
if (!p) { \
30
+
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
31
+
assert(false && "CALIB_ASSERT failed: " #predicate); \
32
+
exit(EXIT_FAILURE); \
33
+
} \
34
+
} while (false);
35
+
#define CALIB_ASSERT_(predicate) CALIB_ASSERT(predicate, "Assertion failed " #predicate)
36
+
37
+
// Return assert
38
+
#define CALIB_ASSERTR(predicate, ...) \
39
+
if (!(predicate)) { \
40
+
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
41
+
return false; \
42
+
}
43
+
17
44
/*
18
45
*
19
46
* Pre-declar functions.
···
73
100
74
101
StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *data)
75
102
{
76
-
assert(data != NULL);
77
-
assert(data->view[0].image_size_pixels.w == data->view[1].image_size_pixels.w);
78
-
assert(data->view[0].image_size_pixels.h == data->view[1].image_size_pixels.h);
103
+
CALIB_ASSERT_(data != NULL);
104
+
CALIB_ASSERT_(data->view[0].image_size_pixels.w == data->view[1].image_size_pixels.w);
105
+
CALIB_ASSERT_(data->view[0].image_size_pixels.h == data->view[1].image_size_pixels.h);
79
106
80
-
assert(data->view[0].use_fisheye == data->view[1].use_fisheye);
107
+
CALIB_ASSERT_(data->view[0].use_fisheye == data->view[1].use_fisheye);
81
108
82
109
cv::Size image_size(data->view[0].image_size_pixels.w, data->view[0].image_size_pixels.h);
83
110
StereoCameraCalibrationWrapper wrapped(data);
···
168
195
}
169
196
} // namespace xrt::auxiliary::tracking
170
197
198
+
using std::array;
199
+
using std::string;
200
+
using std::vector;
201
+
using xrt::auxiliary::tracking::CameraCalibrationWrapper;
202
+
using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper;
203
+
using xrt::auxiliary::util::json::JSONBuilder;
204
+
using xrt::auxiliary::util::json::JSONNode;
205
+
171
206
/*
172
207
*
173
208
* Load functions.
···
190
225
cv::Mat_<float> mat_image_size(2, 1);
191
226
bool result = read_cv_mat(calib_file, &wrapped.view[0].intrinsics_mat, "l_intrinsics"); // 3 x 3
192
227
result = result && read_cv_mat(calib_file, &wrapped.view[1].intrinsics_mat, "r_intrinsics"); // 3 x 3
193
-
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_mat, "l_distortion"); // 1 x 5
194
-
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_mat, "r_distortion"); // 1 x 5
228
+
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_mat, "l_distortion"); // 5 x 1
229
+
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_mat, "r_distortion"); // 5 x 1
195
230
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_fisheye_mat, "l_distortion_fisheye"); // 4 x 1
196
231
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_fisheye_mat, "r_distortion_fisheye"); // 4 x 1
197
232
result = result && read_cv_mat(calib_file, &dummy, "l_rotation"); // 3 x 3
···
204
239
result = result && read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
205
240
206
241
if (!result) {
207
-
U_LOG_W("Re-run calibration!");
242
+
CALIB_WARN("Re-run calibration!");
208
243
return false;
209
244
}
210
245
wrapped.view[0].image_size_pixels.w = uint32_t(mat_image_size(0, 0));
···
217
252
}
218
253
219
254
if (!read_cv_mat(calib_file, &wrapped.camera_translation_mat, "translation")) {
220
-
U_LOG_W("Re-run calibration!");
255
+
CALIB_WARN("Re-run calibration!");
221
256
}
222
257
if (!read_cv_mat(calib_file, &wrapped.camera_rotation_mat, "rotation")) {
223
-
U_LOG_W("Re-run calibration!");
258
+
CALIB_WARN("Re-run calibration!");
224
259
}
225
260
if (!read_cv_mat(calib_file, &wrapped.camera_essential_mat, "essential")) {
226
-
U_LOG_W("Re-run calibration!");
261
+
CALIB_WARN("Re-run calibration!");
227
262
}
228
263
if (!read_cv_mat(calib_file, &wrapped.camera_fundamental_mat, "fundamental")) {
229
-
U_LOG_W("Re-run calibration!");
264
+
CALIB_WARN("Re-run calibration!");
230
265
}
231
266
232
267
cv::Mat_<float> mat_use_fisheye(1, 1);
233
268
if (!read_cv_mat(calib_file, &mat_use_fisheye, "use_fisheye")) {
234
269
wrapped.view[0].use_fisheye = false;
235
-
U_LOG_W("Re-run calibration! (Assuming not fisheye)");
270
+
CALIB_WARN("Re-run calibration! (Assuming not fisheye)");
236
271
} else {
237
272
wrapped.view[0].use_fisheye = mat_use_fisheye(0, 0) != 0.0f;
238
273
}
···
240
275
// clang-format on
241
276
242
277
243
-
assert(wrapped.isDataStorageValid());
278
+
CALIB_ASSERT_(wrapped.isDataStorageValid());
244
279
245
280
t_stereo_camera_calibration_reference(out_data, data_ptr);
246
281
t_stereo_camera_calibration_reference(&data_ptr, NULL);
···
248
283
return true;
249
284
}
250
285
286
+
#define PINHOLE_RADTAN5 "pinhole_radtan5"
287
+
#define FISHEYE_EQUIDISTANT4 "fisheye_equidistant4"
288
+
289
+
//! Fills @p out_mat from a json array stored in @p jn. Returns true if @p jn is
290
+
//! a valid @p rows * @p cols matrix, false otherwise.
291
+
static bool
292
+
load_mat_field(const JSONNode &jn, int rows, int cols, cv::Mat_<double> &out_mat)
293
+
{
294
+
vector<JSONNode> data = jn.asArray();
295
+
bool valid = jn.isArray() && data.size() == static_cast<size_t>(rows * cols);
296
+
297
+
if (valid) {
298
+
out_mat.create(rows, cols);
299
+
for (int i = 0; i < rows * cols; i++) {
300
+
out_mat(i) = data[i].asDouble();
301
+
}
302
+
} else {
303
+
CALIB_WARN("Invalid '%s' matrix field", jn.getName().c_str());
304
+
}
305
+
306
+
return valid;
307
+
}
308
+
309
+
/*!
310
+
* Overload of @ref load_mat_field that saves the result into a 2D C-array.
311
+
*/
312
+
template <int rows, int cols>
313
+
XRT_MAYBE_UNUSED static bool
314
+
load_mat_field(const JSONNode &jn, double (&out_arr)[rows][cols])
315
+
{
316
+
cv::Mat_<double> cvmat{rows, cols, &out_arr[0][0]}; // Wraps out_arr address
317
+
return load_mat_field(jn, rows, cols, cvmat);
318
+
}
319
+
320
+
/*!
321
+
* Overload of @ref load_mat_field that saves the result into a 1D C-array.
322
+
*/
323
+
template <int dim>
324
+
XRT_MAYBE_UNUSED static bool
325
+
load_mat_field(const JSONNode &jn, double (&out_arr)[dim])
326
+
{
327
+
cv::Mat_<double> cvmat{dim, 1, &out_arr[0]}; // Wraps out_arr address
328
+
return load_mat_field(jn, dim, 1, cvmat);
329
+
}
330
+
331
+
static bool
332
+
t_camera_calibration_load_v2(cJSON *cjson_cam, t_camera_calibration *cc)
333
+
{
334
+
JSONNode jc{cjson_cam};
335
+
336
+
string model = jc["model"].asString();
337
+
memset(&cc->intrinsics, 0, sizeof(cc->intrinsics));
338
+
cc->intrinsics[0][0] = jc["intrinsics"]["fx"].asDouble();
339
+
cc->intrinsics[1][1] = jc["intrinsics"]["fy"].asDouble();
340
+
cc->intrinsics[0][2] = jc["intrinsics"]["cx"].asDouble();
341
+
cc->intrinsics[1][2] = jc["intrinsics"]["cy"].asDouble();
342
+
cc->intrinsics[2][2] = 1;
343
+
344
+
size_t n = jc["distortion"].asObject().size();
345
+
if (model == PINHOLE_RADTAN5) {
346
+
cc->use_fisheye = false;
347
+
CALIB_ASSERTR(n == 5, "%zu != 5 distortion params", n);
348
+
349
+
constexpr array names{"k1", "k2", "p1", "p2", "k3"};
350
+
for (size_t i = 0; i < n; i++) {
351
+
cc->distortion[i] = jc["distortion"][names[i]].asDouble();
352
+
}
353
+
} else if (model == FISHEYE_EQUIDISTANT4) {
354
+
cc->use_fisheye = true;
355
+
CALIB_ASSERTR(n == 4, "%zu != 4 distortion params", n);
356
+
357
+
constexpr array names{"k1", "k2", "k3", "k4"};
358
+
for (size_t i = 0; i < n; i++) {
359
+
cc->distortion_fisheye[i] = jc["distortion"][names[i]].asDouble();
360
+
}
361
+
} else {
362
+
CALIB_ASSERTR(false, "Invalid camera model: '%s'", model.c_str());
363
+
return false;
364
+
}
365
+
366
+
cc->image_size_pixels.w = jc["resolution"]["width"].asInt();
367
+
cc->image_size_pixels.h = jc["resolution"]["height"].asInt();
368
+
return true;
369
+
}
370
+
371
+
static bool
372
+
t_stereo_camera_calibration_load_v2(const char *calib_path, struct t_stereo_camera_calibration **out_stereo)
373
+
{
374
+
JSONNode json = JSONNode::loadFromFile(calib_path);
375
+
if (json.isInvalid()) {
376
+
CALIB_ERROR("Unable to open calibration file: '%s'", calib_path);
377
+
return false;
378
+
}
379
+
380
+
StereoCameraCalibrationWrapper stereo{5}; // Hardcoded to 5 distortion parameters.
381
+
382
+
// Load file metadata
383
+
const int supported_version = 2;
384
+
int version = json["file"]["version"].asInt(supported_version);
385
+
if (json["file"]["version"].isInvalid()) {
386
+
CALIB_WARN("'file.version' not found in %s, will assume v2", calib_path);
387
+
}
388
+
CALIB_ASSERTR(version == supported_version, "Calibration file version (%d) != %d", version, supported_version);
389
+
390
+
// Load cameras
391
+
vector<JSONNode> cameras = json["cameras"].asArray();
392
+
bool okmats = true;
393
+
CALIB_ASSERTR(cameras.size() == 2, "Two cameras must be specified, %zu given", cameras.size());
394
+
for (size_t i = 0; i < cameras.size(); i++) {
395
+
JSONNode jc = cameras[i];
396
+
CameraCalibrationWrapper &cc = stereo.view[i];
397
+
bool loaded = t_camera_calibration_load_v2(jc.getCJSON(), &cc.base);
398
+
CALIB_ASSERTR(loaded, "Unable to load camera calibration: %s", jc.toString(false).c_str());
399
+
}
400
+
401
+
JSONNode rel = json["opencv_stereo_calibrate"];
402
+
okmats &= load_mat_field(rel["rotation"], 3, 3, stereo.camera_rotation_mat);
403
+
okmats &= load_mat_field(rel["translation"], 3, 1, stereo.camera_translation_mat);
404
+
okmats &= load_mat_field(rel["essential"], 3, 3, stereo.camera_essential_mat);
405
+
okmats &= load_mat_field(rel["fundamental"], 3, 3, stereo.camera_fundamental_mat);
406
+
407
+
CALIB_ASSERTR(okmats, "One or more calibration matrices couldn't be loaded");
408
+
CALIB_ASSERT_(stereo.isDataStorageValid());
409
+
410
+
t_stereo_camera_calibration_reference(out_stereo, stereo.base);
411
+
412
+
return true;
413
+
}
414
+
251
415
252
416
/*
253
417
*
···
300
464
return true;
301
465
}
302
466
467
+
//! Writes @p mat data into a @p jb as a json array.
468
+
static JSONBuilder &
469
+
operator<<(JSONBuilder &jb, const cv::Mat_<double> &mat)
470
+
{
471
+
jb << "[";
472
+
for (int i = 0; i < mat.rows * mat.cols; i++) {
473
+
jb << mat.at<double>(i);
474
+
}
475
+
jb << "]";
476
+
return jb;
477
+
}
478
+
479
+
static bool
480
+
t_stereo_camera_calibration_save_v2(const char *calib_path, struct t_stereo_camera_calibration *data)
481
+
{
482
+
StereoCameraCalibrationWrapper wrapped(data);
483
+
JSONBuilder jb{};
484
+
485
+
jb << "{";
486
+
jb << "$schema"
487
+
<< "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json";
488
+
jb << "file";
489
+
jb << "{";
490
+
jb << "version" << 2;
491
+
char datetime[OS_ISO_STR_SIZE];
492
+
to_iso_string(os_realtime_get_ns(), datetime);
493
+
jb << "created_on" << datetime;
494
+
jb << "}";
495
+
496
+
jb << "cameras";
497
+
jb << "[";
498
+
499
+
// Cameras
500
+
for (size_t i = 0; i < 2; i++) {
501
+
const auto &view = wrapped.view[i];
502
+
jb << "{";
503
+
jb << "model" << (view.use_fisheye ? FISHEYE_EQUIDISTANT4 : PINHOLE_RADTAN5);
504
+
505
+
jb << "intrinsics";
506
+
jb << "{";
507
+
jb << "fx" << view.intrinsics_mat(0, 0);
508
+
jb << "fy" << view.intrinsics_mat(1, 1);
509
+
jb << "cx" << view.intrinsics_mat(0, 2);
510
+
jb << "cy" << view.intrinsics_mat(1, 2);
511
+
jb << "}";
512
+
513
+
jb << "distortion";
514
+
jb << "{";
515
+
if (view.use_fisheye) {
516
+
int n = view.distortion_mat.size().area(); // Number of distortion parameters
517
+
CALIB_ASSERT_(n == 4);
518
+
519
+
constexpr array names{"k1", "k2", "k3", "k4"};
520
+
for (int i = 0; i < n; i++) {
521
+
jb << names[i] << view.distortion_fisheye_mat(i);
522
+
}
523
+
} else {
524
+
int n = view.distortion_mat.size().area(); // Number of distortion parameters
525
+
CALIB_ASSERT_(n == 5);
526
+
527
+
constexpr array names{"k1", "k2", "p1", "p2", "k3"};
528
+
for (int i = 0; i < n; i++) {
529
+
jb << names[i] << view.distortion_mat(i);
530
+
}
531
+
}
532
+
jb << "}";
533
+
534
+
jb << "resolution";
535
+
jb << "{";
536
+
jb << "width" << view.image_size_pixels.w;
537
+
jb << "height" << view.image_size_pixels.h;
538
+
jb << "}";
539
+
540
+
jb << "}";
541
+
}
542
+
543
+
jb << "]";
544
+
545
+
// cv::stereoCalibrate data
546
+
jb << "opencv_stereo_calibrate"
547
+
<< "{";
548
+
jb << "rotation" << wrapped.camera_rotation_mat;
549
+
jb << "translation" << wrapped.camera_translation_mat;
550
+
jb << "essential" << wrapped.camera_essential_mat;
551
+
jb << "fundamental" << wrapped.camera_fundamental_mat;
552
+
jb << "}";
553
+
554
+
jb << "}";
555
+
556
+
CALIB_INFO("Saving calibration file: %s", jb.getBuiltNode()->toString().c_str());
557
+
return jb.getBuiltNode()->saveToFile(calib_path);
558
+
}
303
559
304
560
/*
305
561
*
···
328
584
cv::Mat temp;
329
585
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
330
586
if (read != 3) {
331
-
U_LOG_E("Failed to read mat header: '%i' '%s'", (int)read, name);
587
+
CALIB_ERROR("Failed to read mat header: '%i' '%s'", (int)read, name);
332
588
return false;
333
589
}
334
590
···
344
600
}
345
601
read = fread(static_cast<void *>(temp.data), header[0], header[1] * header[2], f);
346
602
if (read != (header[1] * header[2])) {
347
-
U_LOG_E("Failed to read mat body: '%i' '%s'", (int)read, name);
603
+
CALIB_ERROR("Failed to read mat body: '%i' '%s'", (int)read, name);
348
604
return false;
349
605
}
350
606
if (m->empty()) {
351
607
m->create(header[1], header[2], temp.type());
352
608
}
353
609
if (temp.type() != m->type()) {
354
-
U_LOG_E("Mat body type does not match: %i vs %i for '%s'", (int)temp.type(), (int)m->type(), name);
610
+
CALIB_ERROR("Mat body type does not match: %i vs %i for '%s'", (int)temp.type(), (int)m->type(), name);
355
611
return false;
356
612
}
357
613
if (temp.total() != m->total()) {
358
-
U_LOG_E("Mat total size does not match: %i vs %i for '%s'", (int)temp.total(), (int)m->total(), name);
614
+
CALIB_ERROR("Mat total size does not match: %i vs %i for '%s'", (int)temp.total(), (int)m->total(),
615
+
name);
359
616
return false;
360
617
}
361
618
if (temp.size() == m->size()) {
···
364
621
return true;
365
622
}
366
623
if (temp.size().width == m->size().height && temp.size().height == m->size().width) {
367
-
U_LOG_W("Mat transposing on load: '%s'", name);
624
+
CALIB_WARN("Mat transposing on load: '%s'", name);
368
625
// needs transpose
369
626
cv::transpose(temp, *m);
370
627
return true;
371
628
}
372
629
// highly unlikely so minimally-helpful error message.
373
-
U_LOG_E("Mat dimension unknown mismatch: '%s'", name);
630
+
CALIB_ERROR("Mat dimension unknown mismatch: '%s'", name);
374
631
return false;
375
632
}
+9
src/xrt/auxiliary/util/u_json.hpp
+9
src/xrt/auxiliary/util/u_json.hpp
···
94
94
: cjson(cjson), is_owner(is_owner), parent(parent)
95
95
{}
96
96
97
+
//! Wrap cJSON object for easy manipulation, does not take ownership
98
+
JSONNode(cJSON *cjson) : JSONNode(cjson, false, nullptr) {}
99
+
97
100
//! Makes a null object; `isInvalid()` on it returns true.
98
101
JSONNode() {}
99
102
···
320
323
getName() const
321
324
{
322
325
return string(cjson->string ? cjson->string : "");
326
+
}
327
+
328
+
cJSON *
329
+
getCJSON()
330
+
{
331
+
return cjson;
323
332
}
324
333
};
325
334