matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2020, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Handling of files and calibration data.
6 * @author Pete Black <pblack@collabora.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
9 * @ingroup aux_tracking
10 */
11
12#include "tracking/t_calibration_opencv.hpp"
13#include "tracking/t_tracking.h"
14#include "util/u_misc.h"
15#include "util/u_logging.h"
16#include "util/u_json.hpp"
17#include "os/os_time.h"
18
19
20DEBUG_GET_ONCE_LOG_OPTION(calib_log, "CALIB_LOG", U_LOGGING_INFO)
21
22#define CALIB_TRACE(...) U_LOG_IFL_T(debug_get_log_option_calib_log(), __VA_ARGS__)
23#define CALIB_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_calib_log(), __VA_ARGS__)
24#define CALIB_INFO(...) U_LOG_IFL_I(debug_get_log_option_calib_log(), __VA_ARGS__)
25#define CALIB_WARN(...) U_LOG_IFL_W(debug_get_log_option_calib_log(), __VA_ARGS__)
26#define CALIB_ERROR(...) U_LOG_IFL_E(debug_get_log_option_calib_log(), __VA_ARGS__)
27#define CALIB_ASSERT(predicate, ...) \
28 do { \
29 bool p = predicate; \
30 if (!p) { \
31 U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
32 assert(false && "CALIB_ASSERT failed: " #predicate); \
33 exit(EXIT_FAILURE); \
34 } \
35 } while (false);
36#define CALIB_ASSERT_(predicate) CALIB_ASSERT(predicate, "Assertion failed " #predicate)
37
38// Return assert
39#define CALIB_ASSERTR(predicate, ...) \
40 if (!(predicate)) { \
41 U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
42 return false; \
43 }
44
45#define COPY(TO, FROM) \
46 do { \
47 CALIB_ASSERT(FROM.size() == TO.size(), "Sizes doesn't match for " #FROM); \
48 FROM.copyTo(TO); \
49 } while (false)
50
51
52/*
53 *
54 * Pre-declar functions.
55 *
56 */
57
58static bool
59read_cv_mat(FILE *f, cv::Mat *m, const char *name);
60
61static bool
62write_cv_mat(FILE *f, cv::Mat *m);
63
64
65/*
66 *
67 * Refine and create functions.
68 *
69 */
70namespace xrt::auxiliary::tracking {
71RemapPair
72calibration_get_undistort_map(t_camera_calibration &calib,
73 cv::InputArray rectify_transform_optional,
74 cv::Mat new_camera_matrix_optional)
75{
76 RemapPair ret;
77 CameraCalibrationWrapper wrap(calib);
78 if (new_camera_matrix_optional.empty()) {
79 new_camera_matrix_optional = wrap.intrinsics_mat;
80 }
81
82 //! @todo Scale Our intrinsics if the frame size we request
83 // calibration for does not match what was saved
84 cv::Size image_size(calib.image_size_pixels.w, calib.image_size_pixels.h);
85
86 if (t_camera_distortion_model_is_opencv_fisheye(wrap.distortion_model)) {
87 cv::fisheye::initUndistortRectifyMap(wrap.intrinsics_mat, // cameraMatrix
88 wrap.distortion_mat, // distCoeffs
89 rectify_transform_optional, // R
90 new_camera_matrix_optional, // newCameraMatrix
91 image_size, // size
92 CV_32FC1, // m1type
93 ret.remap_x, // map1
94 ret.remap_y); // map2
95 } else if (t_camera_distortion_model_is_opencv_non_fisheye(wrap.distortion_model)) {
96 cv::initUndistortRectifyMap(wrap.intrinsics_mat, // cameraMatrix
97 wrap.distortion_mat, // distCoeffs
98 rectify_transform_optional, // R
99 new_camera_matrix_optional, // newCameraMatrix
100 image_size, // size
101 CV_32FC1, // m1type
102 ret.remap_x, // map1
103 ret.remap_y); // map2
104 } else {
105 assert(!"Unsupported distortion model");
106 }
107
108 return ret;
109}
110
111StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *data)
112{
113 CALIB_ASSERT_(data != NULL);
114 CALIB_ASSERT_(data->view[0].image_size_pixels.w == data->view[1].image_size_pixels.w);
115 CALIB_ASSERT_(data->view[0].image_size_pixels.h == data->view[1].image_size_pixels.h);
116
117 CALIB_ASSERT_(data->view[0].distortion_model == data->view[1].distortion_model);
118
119 cv::Size image_size(data->view[0].image_size_pixels.w, data->view[0].image_size_pixels.h);
120 StereoCameraCalibrationWrapper wrapped(data);
121
122 t_camera_distortion_model distortion_model = data->view[0].distortion_model;
123
124 /*
125 * Generate our rectification maps
126 *
127 * Here cv::noArray() means zero distortion.
128 */
129 if (t_camera_distortion_model_is_opencv_fisheye(distortion_model)) {
130#if 0
131 //! @todo for some reason this looks weird?
132 // Alpha of 1.0 kinda works, not really.
133 int flags = cv::CALIB_ZERO_DISPARITY;
134 double balance = 0.0; // also known as alpha.
135 double fov_scale = 1.0;
136
137 cv::fisheye::stereoRectify(
138 wrapped.view[0].intrinsics_mat, // K1
139 wrapped.view[0].distortion_mat, // D1
140 wrapped.view[1].intrinsics_mat, // K2
141 wrapped.view[1].distortion_mat, // D2
142 image_size, // imageSize
143 wrapped.camera_rotation_mat, // R
144 wrapped.camera_translation_mat, // tvec
145 view[0].rotation_mat, // R1
146 view[1].rotation_mat, // R2
147 view[0].projection_mat, // P1
148 view[1].projection_mat, // P2
149 disparity_to_depth_mat, // Q
150 flags, // flags
151 cv::Size(), // newImageSize
152 balance, // balance
153 fov_scale); // fov_scale
154#else
155 // Regular stereoRectify function instead, without distortion.
156 int flags = cv::CALIB_ZERO_DISPARITY;
157 // The function performs the default scaling.
158 float alpha = -1.0f;
159
160 cv::stereoRectify(wrapped.view[0].intrinsics_mat, // cameraMatrix1
161 cv::noArray(), // distCoeffs1
162 wrapped.view[1].intrinsics_mat, // cameraMatrix2
163 cv::noArray(), // distCoeffs2
164 image_size, // imageSize
165 wrapped.camera_rotation_mat, // R
166 wrapped.camera_translation_mat, // T
167 view[0].rotation_mat, // R1
168 view[1].rotation_mat, // R2
169 view[0].projection_mat, // P1
170 view[1].projection_mat, // P2
171 disparity_to_depth_mat, // Q
172 flags, // flags
173 alpha, // alpha
174 cv::Size(), // newImageSize
175 NULL, // validPixROI1
176 NULL); // validPixROI2
177#endif
178 } else if (t_camera_distortion_model_is_opencv_non_fisheye(distortion_model)) {
179 // Have the same principal point on both.
180 int flags = cv::CALIB_ZERO_DISPARITY;
181 // Get all of the pixels from the camera.
182 float alpha = 1.0f;
183
184 cv::stereoRectify(wrapped.view[0].intrinsics_mat, // cameraMatrix1
185 /* cv::noArray(), */ // distCoeffs1
186 wrapped.view[0].distortion_mat, // distCoeffs1
187 wrapped.view[1].intrinsics_mat, // cameraMatrix2
188 /* cv::noArray(), */ // distCoeffs2
189 wrapped.view[1].distortion_mat, // distCoeffs2
190 image_size, // imageSize
191 wrapped.camera_rotation_mat, // R
192 wrapped.camera_translation_mat, // T
193 view[0].rotation_mat, // R1
194 view[1].rotation_mat, // R2
195 view[0].projection_mat, // P1
196 view[1].projection_mat, // P2
197 disparity_to_depth_mat, // Q
198 flags, // flags
199 alpha, // alpha
200 cv::Size(), // newImageSize
201 NULL, // validPixROI1
202 NULL); // validPixROI2
203 } else {
204 assert(!"Unsupported distortion model");
205 }
206
207 view[0].rectify = calibration_get_undistort_map(data->view[0], view[0].rotation_mat, view[0].projection_mat);
208 view[1].rectify = calibration_get_undistort_map(data->view[1], view[1].rotation_mat, view[1].projection_mat);
209}
210} // namespace xrt::auxiliary::tracking
211
212using std::array;
213using std::string;
214using std::vector;
215using xrt::auxiliary::tracking::CameraCalibrationWrapper;
216using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper;
217using xrt::auxiliary::util::json::JSONBuilder;
218using xrt::auxiliary::util::json::JSONNode;
219
220/*
221 *
222 * Load functions.
223 *
224 */
225
226extern "C" bool
227t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data)
228{
229 // Scratch-space temporary matrix
230 cv::Mat scratch;
231
232 // Temp load matrices
233 cv::Mat_<double> l_intrinsics(3, 3);
234 cv::Mat_<double> r_intrinsics(3, 3);
235 cv::Mat_<double> l_distortion(5, 1);
236 cv::Mat_<double> r_distortion(5, 1);
237 cv::Mat_<double> l_distortion_fisheye(4, 1);
238 cv::Mat_<double> r_distortion_fisheye(4, 1);
239 cv::Mat_<double> translation(3, 1);
240 cv::Mat_<double> rotation(3, 3);
241 cv::Mat_<double> essential(3, 3);
242 cv::Mat_<double> fundamental(3, 3);
243 cv::Mat_<float> mat_use_fisheye(1, 1, {0.0f}); // Ensure is initialised.
244 cv::Mat_<float> mat_image_size(1, 2);
245 cv::Mat_<float> mat_new_image_size(1, 2);
246
247 // Read our calibration from this file
248 bool result = read_cv_mat(calib_file, &l_intrinsics, "l_intrinsics"); // 3 x 3
249 result = result && read_cv_mat(calib_file, &r_intrinsics, "r_intrinsics"); // 3 x 3
250 result = result && read_cv_mat(calib_file, &l_distortion, "l_distortion"); // 5 x 1
251 result = result && read_cv_mat(calib_file, &r_distortion, "r_distortion"); // 5 x 1
252 result = result && read_cv_mat(calib_file, &l_distortion_fisheye, "l_distortion_fisheye"); // 4 x 1
253 result = result && read_cv_mat(calib_file, &r_distortion_fisheye, "r_distortion_fisheye"); // 4 x 1
254 result = result && read_cv_mat(calib_file, &scratch, "l_rotation"); // 3 x 3
255 result = result && read_cv_mat(calib_file, &scratch, "r_rotation"); // 3 x 3
256 result = result && read_cv_mat(calib_file, &scratch, "l_translation"); // empty
257 result = result && read_cv_mat(calib_file, &scratch, "r_translation"); // empty
258 result = result && read_cv_mat(calib_file, &scratch, "l_projection"); // 3 x 4
259 result = result && read_cv_mat(calib_file, &scratch, "r_projection"); // 3 x 4
260 result = result && read_cv_mat(calib_file, &scratch, "disparity_to_depth"); // 4 x 4
261 result = result && read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
262
263 if (!result) {
264 CALIB_WARN("Re-run calibration!");
265 return false;
266 }
267
268 if (read_cv_mat(calib_file, &mat_new_image_size, "mat_new_image_size")) {
269 // do nothing particular here.
270 }
271 if (!read_cv_mat(calib_file, &translation, "translation")) { // 3 x 1
272 CALIB_WARN("Re-run calibration!");
273 }
274 if (!read_cv_mat(calib_file, &rotation, "rotation")) { // 3 x 3
275 CALIB_WARN("Re-run calibration!");
276 }
277 if (!read_cv_mat(calib_file, &essential, "essential")) { // 3 x 3
278 CALIB_WARN("Re-run calibration!");
279 }
280 if (!read_cv_mat(calib_file, &fundamental, "fundamental")) { // 3 x 3
281 CALIB_WARN("Re-run calibration!");
282 }
283 if (!read_cv_mat(calib_file, &mat_use_fisheye, "use_fisheye")) {
284 CALIB_WARN("Re-run calibration! (Assuming not fisheye)");
285 }
286
287
288 /*
289 * Extract some data.
290 */
291
292 bool is_fisheye = mat_use_fisheye(0, 0) != 0.0f;
293 uint32_t size_w = uint32_t(mat_image_size(0, 0));
294 uint32_t size_h = uint32_t(mat_image_size(0, 1));
295 t_camera_distortion_model model = is_fisheye ? T_DISTORTION_FISHEYE_KB4 : T_DISTORTION_OPENCV_RADTAN_5;
296
297
298 /*
299 * Copy to calibration struct.
300 */
301
302 t_stereo_camera_calibration *data_ptr = NULL;
303 t_stereo_camera_calibration_alloc(&data_ptr, model);
304 StereoCameraCalibrationWrapper wrapped(data_ptr);
305
306 COPY(wrapped.view[0].intrinsics_mat, l_intrinsics);
307 COPY(wrapped.view[1].intrinsics_mat, r_intrinsics);
308 if (is_fisheye) {
309 COPY(wrapped.view[0].distortion_mat, l_distortion_fisheye);
310 COPY(wrapped.view[1].distortion_mat, r_distortion_fisheye);
311 } else {
312 COPY(wrapped.view[0].distortion_mat, l_distortion);
313 COPY(wrapped.view[1].distortion_mat, r_distortion);
314 }
315 COPY(wrapped.camera_translation_mat, translation);
316 COPY(wrapped.camera_rotation_mat, rotation);
317 COPY(wrapped.camera_essential_mat, essential);
318 COPY(wrapped.camera_fundamental_mat, fundamental);
319 wrapped.view[0].image_size_pixels.w = wrapped.view[1].image_size_pixels.w = size_w;
320 wrapped.view[0].image_size_pixels.h = wrapped.view[1].image_size_pixels.h = size_h;
321
322 CALIB_ASSERT_(wrapped.isDataStorageValid());
323
324 t_stereo_camera_calibration_reference(out_data, data_ptr);
325 t_stereo_camera_calibration_reference(&data_ptr, NULL);
326
327 return true;
328}
329
330static bool
331t_stereo_camera_calibration_load_path_v1(const char *calib_path, struct t_stereo_camera_calibration **out_data)
332{
333 CALIB_WARN("Deprecated function %s", __func__);
334
335 FILE *calib_file = fopen(calib_path, "rb");
336 if (calib_file == nullptr) {
337 CALIB_ERROR("Unable to open calibration file: '%s'", calib_path);
338 return false;
339 }
340
341 bool success = t_stereo_camera_calibration_load_v1(calib_file, out_data);
342 fclose(calib_file);
343
344 return success;
345}
346
347//!@todo merge these with t_tracking.h
348#define PINHOLE_RADTAN5 "pinhole_radtan5"
349#define FISHEYE_EQUIDISTANT4 "fisheye_equidistant4"
350
351//! Fills @p out_mat from a json array stored in @p jn. Returns true if @p jn is
352//! a valid @p rows * @p cols matrix, false otherwise.
353static bool
354load_mat_field(const JSONNode &jn, int rows, int cols, cv::Mat_<double> &out_mat)
355{
356 vector<JSONNode> data = jn.asArray();
357 bool valid = jn.isArray() && data.size() == static_cast<size_t>(rows * cols);
358
359 if (valid) {
360 out_mat.create(rows, cols);
361 for (int i = 0; i < rows * cols; i++) {
362 out_mat(i) = data[i].asDouble();
363 }
364 } else {
365 CALIB_WARN("Invalid '%s' matrix field", jn.getName().c_str());
366 }
367
368 return valid;
369}
370
371/*!
372 * Overload of @ref load_mat_field that saves the result into a 2D C-array.
373 */
374template <int rows, int cols>
375XRT_MAYBE_UNUSED static bool
376load_mat_field(const JSONNode &jn, double (&out_arr)[rows][cols])
377{
378 cv::Mat_<double> cvmat{rows, cols, &out_arr[0][0]}; // Wraps out_arr address
379 return load_mat_field(jn, rows, cols, cvmat);
380}
381
382/*!
383 * Overload of @ref load_mat_field that saves the result into a 1D C-array.
384 */
385template <int dim>
386XRT_MAYBE_UNUSED static bool
387load_mat_field(const JSONNode &jn, double (&out_arr)[dim])
388{
389 cv::Mat_<double> cvmat{dim, 1, &out_arr[0]}; // Wraps out_arr address
390 return load_mat_field(jn, dim, 1, cvmat);
391}
392
393static bool
394t_camera_calibration_load_v2(cJSON *cjson_cam, t_camera_calibration *cc)
395{
396 JSONNode jc{cjson_cam};
397
398 string model = jc["model"].asString();
399 memset(&cc->intrinsics, 0, sizeof(cc->intrinsics));
400 cc->intrinsics[0][0] = jc["intrinsics"]["fx"].asDouble();
401 cc->intrinsics[1][1] = jc["intrinsics"]["fy"].asDouble();
402 cc->intrinsics[0][2] = jc["intrinsics"]["cx"].asDouble();
403 cc->intrinsics[1][2] = jc["intrinsics"]["cy"].asDouble();
404 cc->intrinsics[2][2] = 1;
405
406 size_t n = jc["distortion"].asObject().size();
407 if (model == PINHOLE_RADTAN5) {
408 cc->distortion_model = T_DISTORTION_OPENCV_RADTAN_5;
409 CALIB_ASSERTR(n == 5, "%zu != 5 distortion params", n);
410
411 cc->rt5.k1 = jc["distortion"]["k1"].asDouble();
412 cc->rt5.k2 = jc["distortion"]["k2"].asDouble();
413 cc->rt5.p1 = jc["distortion"]["p1"].asDouble();
414 cc->rt5.p2 = jc["distortion"]["p2"].asDouble();
415 cc->rt5.k3 = jc["distortion"]["k3"].asDouble();
416 } else if (model == FISHEYE_EQUIDISTANT4) {
417 cc->distortion_model = T_DISTORTION_FISHEYE_KB4;
418 CALIB_ASSERTR(n == 4, "%zu != 4 distortion params", n);
419
420 cc->kb4.k1 = jc["distortion"]["k1"].asDouble();
421 cc->kb4.k2 = jc["distortion"]["k2"].asDouble();
422 cc->kb4.k3 = jc["distortion"]["k3"].asDouble();
423 cc->kb4.k4 = jc["distortion"]["k4"].asDouble();
424 } else {
425 CALIB_ASSERTR(false, "Invalid camera model: '%s'", model.c_str());
426 return false;
427 }
428
429 cc->image_size_pixels.w = jc["resolution"]["width"].asInt();
430 cc->image_size_pixels.h = jc["resolution"]["height"].asInt();
431 return true;
432}
433
434extern "C" bool
435t_stereo_camera_calibration_from_json_v2(cJSON *cjson, struct t_stereo_camera_calibration **out_stereo)
436{
437 JSONNode json{cjson};
438
439 // Load file metadata
440 const int supported_version = 2;
441 int version = json["metadata"]["version"].asInt(supported_version);
442 if (json["metadata"]["version"].isInvalid()) {
443 CALIB_WARN("'metadata.version' not found, will assume version=%d", supported_version);
444 }
445 CALIB_ASSERTR(version == supported_version, "Calibration json version (%d) != %d", version, supported_version);
446
447 // Temporary camera calibration structs so we can infer the distortion model easily
448 t_camera_calibration tmp_calibs[2];
449
450 // Load cameras
451 vector<JSONNode> cameras = json["cameras"].asArray();
452 bool okmats = true;
453 CALIB_ASSERTR(cameras.size() == 2, "Two cameras must be specified, %zu given", cameras.size());
454 for (size_t i = 0; i < cameras.size(); i++) {
455 JSONNode jc = cameras[i];
456 bool loaded = t_camera_calibration_load_v2(jc.getCJSON(), &tmp_calibs[i]);
457 CALIB_ASSERTR(loaded, "Unable to load camera calibration: %s", jc.toString(false).c_str());
458 }
459
460 t_camera_distortion_model model = tmp_calibs[0].distortion_model;
461
462 //!@todo At some point it'll make sense to support different distortion models per-camera, but right now we
463 //! don't have any cameras like that and the way t_stereo_camera_calib_alloc and
464 //!(Stereo)CameraCalibrationWrapper work makes it pretty annoying.
465
466 CALIB_ASSERT_(tmp_calibs[0].distortion_model == tmp_calibs[1].distortion_model);
467
468 StereoCameraCalibrationWrapper stereo{model};
469
470 stereo.view[0].base = tmp_calibs[0];
471 stereo.view[1].base = tmp_calibs[1];
472
473
474 JSONNode rel = json["opencv_stereo_calibrate"];
475 okmats &= load_mat_field(rel["rotation"], 3, 3, stereo.camera_rotation_mat);
476 okmats &= load_mat_field(rel["translation"], 3, 1, stereo.camera_translation_mat);
477 okmats &= load_mat_field(rel["essential"], 3, 3, stereo.camera_essential_mat);
478 okmats &= load_mat_field(rel["fundamental"], 3, 3, stereo.camera_fundamental_mat);
479
480 CALIB_ASSERTR(okmats, "One or more calibration matrices couldn't be loaded");
481 CALIB_ASSERT_(stereo.isDataStorageValid());
482
483 t_stereo_camera_calibration_reference(out_stereo, stereo.base);
484
485 return true;
486}
487
488static bool
489t_stereo_camera_calibration_load_path_v2(const char *calib_path, struct t_stereo_camera_calibration **out_stereo)
490{
491 JSONNode json = JSONNode::loadFromFile(calib_path);
492 if (json.isInvalid()) {
493 CALIB_ERROR("Unable to open calibration file: '%s'", calib_path);
494 return false;
495 }
496 return t_stereo_camera_calibration_from_json_v2(json.getCJSON(), out_stereo);
497}
498
499
500/*
501 *
502 * Save functions.
503 *
504 */
505
506extern "C" bool
507t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_calibration *data)
508{
509 CALIB_WARN("Deprecated function: %s", __func__);
510
511 StereoCameraCalibrationWrapper wrapped(data);
512
513 bool is_fisheye = false;
514
515 switch (data->view[0].distortion_model) {
516 case T_DISTORTION_OPENCV_RADTAN_5: is_fisheye = false; break;
517 case T_DISTORTION_FISHEYE_KB4: is_fisheye = true; break;
518 default:
519 CALIB_ERROR("Can't save distortion model %s in a v1 calib file!",
520 t_stringify_camera_distortion_model(data->view[0].distortion_model));
521 return false;
522 }
523
524 if (data->view[0].distortion_model != data->view[1].distortion_model) {
525 CALIB_ERROR("v1 calibrations can't deal with differing distortion models!");
526 return false;
527 }
528
529 // Scratch-space temporary matrix
530 cv::Mat scratch;
531
532 write_cv_mat(calib_file, &wrapped.view[0].intrinsics_mat);
533 write_cv_mat(calib_file, &wrapped.view[1].intrinsics_mat);
534 if (is_fisheye) {
535 cv::Mat_<double> distortion(5, 1, {0.0});
536 write_cv_mat(calib_file, &distortion); // l_distortion
537 write_cv_mat(calib_file, &distortion); // r_distortion
538 write_cv_mat(calib_file, &wrapped.view[0].distortion_mat);
539 write_cv_mat(calib_file, &wrapped.view[1].distortion_mat);
540 } else {
541 cv::Mat_<double> distortion_fisheye(4, 1, {0.0});
542 write_cv_mat(calib_file, &wrapped.view[0].distortion_mat);
543 write_cv_mat(calib_file, &wrapped.view[1].distortion_mat);
544 write_cv_mat(calib_file, &distortion_fisheye); // l_distortion_fisheye
545 write_cv_mat(calib_file, &distortion_fisheye); // r_distortion_fisheye
546 }
547
548 write_cv_mat(calib_file, &scratch); // view[0].rotation_mat
549 write_cv_mat(calib_file, &scratch); // view[1].rotation_mat
550 write_cv_mat(calib_file, &scratch); // l_translation
551 write_cv_mat(calib_file, &scratch); // r_translation
552 write_cv_mat(calib_file, &scratch); // view[0].projection_mat
553 write_cv_mat(calib_file, &scratch); // view[1].projection_mat
554 write_cv_mat(calib_file, &scratch); // disparity_to_depth_mat
555
556 cv::Mat mat_image_size;
557 mat_image_size.create(1, 2, CV_32F);
558 mat_image_size.at<float>(0, 0) = wrapped.view[0].image_size_pixels.w;
559 mat_image_size.at<float>(0, 1) = wrapped.view[0].image_size_pixels.h;
560 write_cv_mat(calib_file, &mat_image_size);
561
562 // "new" image size - we actually leave up to the caller now
563 write_cv_mat(calib_file, &mat_image_size);
564
565 write_cv_mat(calib_file, &wrapped.camera_translation_mat);
566 write_cv_mat(calib_file, &wrapped.camera_rotation_mat);
567 write_cv_mat(calib_file, &wrapped.camera_essential_mat);
568 write_cv_mat(calib_file, &wrapped.camera_fundamental_mat);
569
570 cv::Mat mat_use_fisheye;
571 mat_use_fisheye.create(1, 1, CV_32F);
572 mat_use_fisheye.at<float>(0, 0) = is_fisheye;
573 write_cv_mat(calib_file, &mat_use_fisheye);
574
575 return true;
576}
577
578static bool
579t_stereo_camera_calibration_save_path_v1(const char *calib_path, struct t_stereo_camera_calibration *data)
580{
581 FILE *calib_file = fopen(calib_path, "wb");
582 if (calib_file == nullptr) {
583 CALIB_ERROR("Unable to open calibration file: '%s'", calib_path);
584 return false;
585 }
586
587 bool success = t_stereo_camera_calibration_save_v1(calib_file, data);
588 fclose(calib_file);
589
590 return success;
591}
592
593//! Writes @p mat data into a @p jb as a json array.
594static JSONBuilder &
595operator<<(JSONBuilder &jb, const cv::Mat_<double> &mat)
596{
597 jb << "[";
598 for (int i = 0; i < mat.rows * mat.cols; i++) {
599 jb << mat.at<double>(i);
600 }
601 jb << "]";
602 return jb;
603}
604
605extern "C" bool
606t_stereo_camera_calibration_to_json_v2(cJSON **out_cjson, struct t_stereo_camera_calibration *data)
607{
608 if (data->view[0].distortion_model != data->view[1].distortion_model) {
609 CALIB_ASSERTR(false,
610 "Can't deal with a stereo camera calibration with different distortion models per-view!");
611 }
612
613 if (data->view[0].distortion_model != T_DISTORTION_FISHEYE_KB4 &&
614 data->view[0].distortion_model != T_DISTORTION_OPENCV_RADTAN_5) {
615 CALIB_ASSERTR(false, "Can only deal with fisheye or radtan5 distortion models!");
616 }
617
618 StereoCameraCalibrationWrapper wrapped(data);
619 JSONBuilder jb{};
620
621 jb << "{";
622 jb << "$schema"
623 << "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json";
624 jb << "metadata";
625 jb << "{";
626 jb << "version" << 2;
627 jb << "}";
628
629 jb << "cameras";
630 jb << "[";
631
632 // Cameras
633 for (size_t i = 0; i < 2; i++) {
634 const auto &view = wrapped.view[i];
635 bool fisheye = view.distortion_model == T_DISTORTION_FISHEYE_KB4;
636 jb << "{";
637 jb << "model" << (fisheye ? FISHEYE_EQUIDISTANT4 : PINHOLE_RADTAN5);
638
639 jb << "intrinsics";
640 jb << "{";
641 jb << "fx" << view.intrinsics_mat(0, 0);
642 jb << "fy" << view.intrinsics_mat(1, 1);
643 jb << "cx" << view.intrinsics_mat(0, 2);
644 jb << "cy" << view.intrinsics_mat(1, 2);
645 jb << "}";
646
647 jb << "distortion";
648 jb << "{";
649 if (fisheye) {
650 int n = view.distortion_mat.size().area(); // Number of distortion parameters
651 CALIB_ASSERT_(n == 4);
652
653 constexpr array names{"k1", "k2", "k3", "k4"};
654 for (int i = 0; i < n; i++) {
655 jb << names[i] << view.distortion_mat(i);
656 }
657 } else {
658 int n = view.distortion_mat.size().area(); // Number of distortion parameters
659 CALIB_ASSERT_(n == 5);
660
661 constexpr array names{"k1", "k2", "p1", "p2", "k3"};
662 for (int i = 0; i < n; i++) {
663 jb << names[i] << view.distortion_mat(i);
664 }
665 }
666 jb << "}";
667
668 jb << "resolution";
669 jb << "{";
670 jb << "width" << view.image_size_pixels.w;
671 jb << "height" << view.image_size_pixels.h;
672 jb << "}";
673
674 jb << "}";
675 }
676
677 jb << "]";
678
679 // cv::stereoCalibrate data
680 jb << "opencv_stereo_calibrate"
681 << "{";
682 jb << "rotation" << wrapped.camera_rotation_mat;
683 jb << "translation" << wrapped.camera_translation_mat;
684 jb << "essential" << wrapped.camera_essential_mat;
685 jb << "fundamental" << wrapped.camera_fundamental_mat;
686 jb << "}";
687
688 jb << "}";
689
690 cJSON *cjson = jb.getBuiltNode()->getCJSON();
691 *out_cjson = cJSON_Duplicate(cjson, true);
692 return true;
693}
694
695static bool
696t_stereo_camera_calibration_save_path_v2(const char *calib_path, struct t_stereo_camera_calibration *data)
697{
698 cJSON *cjson = NULL;
699 bool success = t_stereo_camera_calibration_to_json_v2(&cjson, data);
700 if (!success) {
701 return false;
702 }
703
704 JSONNode json{cjson, true, nullptr}; // is_owner=true so it will free cjson object when leaving scope
705 CALIB_INFO("Saving calibration file: %s", json.toString(false).c_str());
706 return json.saveToFile(calib_path);
707}
708
709
710/*
711 *
712 * Helpers
713 *
714 */
715
716static bool
717write_cv_mat(FILE *f, cv::Mat *m)
718{
719 uint32_t header[3];
720 header[0] = static_cast<uint32_t>(m->elemSize());
721 header[1] = static_cast<uint32_t>(m->rows);
722 header[2] = static_cast<uint32_t>(m->cols);
723 fwrite(static_cast<void *>(header), sizeof(uint32_t), 3, f);
724 fwrite(static_cast<void *>(m->data), header[0], header[1] * header[2], f);
725 return true;
726}
727
728static bool
729read_cv_mat(FILE *f, cv::Mat *m, const char *name)
730{
731 uint32_t header[3] = {};
732 size_t read = 0;
733
734 cv::Mat temp;
735 read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
736 if (read != 3) {
737 CALIB_ERROR("Failed to read mat header: '%i' '%s'", (int)read, name);
738 return false;
739 }
740
741 if (header[1] == 0 && header[2] == 0) {
742 return true;
743 }
744
745 if (header[0] >= 32 || header[1] >= 32) {
746 CALIB_ERROR("Matrix dimensions for '%s' is too large: '%ux%u'", name, header[0], header[1]);
747 return false;
748 }
749
750 //! @todo We may have written things other than CV_32F and CV_64F.
751 if (header[0] == 4) {
752 temp.create(static_cast<int>(header[1]), static_cast<int>(header[2]), CV_32F);
753 } else {
754 temp.create(static_cast<int>(header[1]), static_cast<int>(header[2]), CV_64F);
755 }
756 read = fread(static_cast<void *>(temp.data), header[0], header[1] * header[2], f);
757 if (read != (header[1] * header[2])) {
758 CALIB_ERROR("Failed to read mat body: '%i' '%s'", (int)read, name);
759 return false;
760 }
761 if (m->empty()) {
762 m->create(header[1], header[2], temp.type());
763 }
764 if (temp.type() != m->type()) {
765 CALIB_ERROR("Mat body type does not match: %i vs %i for '%s'", (int)temp.type(), (int)m->type(), name);
766 return false;
767 }
768 if (temp.total() != m->total()) {
769 CALIB_ERROR("Mat total size does not match: %i vs %i for '%s'", (int)temp.total(), (int)m->total(),
770 name);
771 return false;
772 }
773 if (temp.size() == m->size()) {
774 // Exact match
775 temp.copyTo(*m);
776 return true;
777 }
778 if (temp.size().width == m->size().height && temp.size().height == m->size().width) {
779 CALIB_WARN("Mat transposing on load: '%s'", name);
780 // needs transpose
781 cv::transpose(temp, *m);
782 return true;
783 }
784 // highly unlikely so minimally-helpful error message.
785 CALIB_ERROR("Mat dimension unknown mismatch: '%s'", name);
786 return false;
787}
788
789static bool
790has_json_extension(const char *filename)
791{
792 const char extension[] = ".json";
793 size_t name_len = strlen(filename);
794 size_t ext_len = strlen(extension);
795
796 if (name_len > ext_len) {
797 return strcmp(&filename[name_len - ext_len], extension) == 0;
798 }
799
800 return false;
801}
802
803
804/*
805 *
806 * Exported functions
807 *
808 */
809
810extern "C" bool
811t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data)
812{
813 return has_json_extension(calib_path) ? t_stereo_camera_calibration_load_path_v2(calib_path, out_data)
814 : t_stereo_camera_calibration_load_path_v1(calib_path, out_data);
815}
816
817extern "C" bool
818t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data)
819{
820 return has_json_extension(calib_path) ? t_stereo_camera_calibration_save_path_v2(calib_path, data)
821 : t_stereo_camera_calibration_save_path_v1(calib_path, data);
822}