mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-03 12:28:07 +00:00
t/file: Implement json save/load for v2 calibration
This commit is contained in:
parent
31f71251e0
commit
1483ec32c5
src/xrt/auxiliary
|
@ -105,7 +105,7 @@ add_library(
|
|||
target_link_libraries(
|
||||
aux_tracking
|
||||
PUBLIC aux-includes
|
||||
PRIVATE aux_math aux_util xrt-external-flexkalman xrt-external-hungarian
|
||||
PRIVATE aux_math aux_util xrt-external-flexkalman xrt-external-hungarian xrt-external-cjson
|
||||
)
|
||||
target_include_directories(aux_tracking SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
|
||||
if(XRT_HAVE_OPENCV)
|
||||
|
|
|
@ -244,6 +244,7 @@ lib_aux_tracking = static_library(
|
|||
xrt_include,
|
||||
flexkalman_include,
|
||||
hungarian_include,
|
||||
cjson_include,
|
||||
],
|
||||
dependencies: tracking_deps
|
||||
)
|
||||
|
|
|
@ -12,8 +12,35 @@
|
|||
#include "tracking/t_calibration_opencv.hpp"
|
||||
#include "util/u_misc.h"
|
||||
#include "util/u_logging.h"
|
||||
#include "util/u_json.hpp"
|
||||
#include "os/os_time.h"
|
||||
|
||||
|
||||
DEBUG_GET_ONCE_LOG_OPTION(calib_log, "CALIB_LOG", U_LOGGING_WARN)
|
||||
|
||||
#define CALIB_TRACE(...) U_LOG_IFL_T(debug_get_log_option_calib_log(), __VA_ARGS__)
|
||||
#define CALIB_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_calib_log(), __VA_ARGS__)
|
||||
#define CALIB_INFO(...) U_LOG_IFL_I(debug_get_log_option_calib_log(), __VA_ARGS__)
|
||||
#define CALIB_WARN(...) U_LOG_IFL_W(debug_get_log_option_calib_log(), __VA_ARGS__)
|
||||
#define CALIB_ERROR(...) U_LOG_IFL_E(debug_get_log_option_calib_log(), __VA_ARGS__)
|
||||
#define CALIB_ASSERT(predicate, ...) \
|
||||
do { \
|
||||
bool p = predicate; \
|
||||
if (!p) { \
|
||||
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
|
||||
assert(false && "CALIB_ASSERT failed: " #predicate); \
|
||||
exit(EXIT_FAILURE); \
|
||||
} \
|
||||
} while (false);
|
||||
#define CALIB_ASSERT_(predicate) CALIB_ASSERT(predicate, "Assertion failed " #predicate)
|
||||
|
||||
// Return assert
|
||||
#define CALIB_ASSERTR(predicate, ...) \
|
||||
if (!(predicate)) { \
|
||||
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
|
||||
return false; \
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Pre-declar functions.
|
||||
|
@ -73,11 +100,11 @@ calibration_get_undistort_map(t_camera_calibration &calib,
|
|||
|
||||
StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
assert(data->view[0].image_size_pixels.w == data->view[1].image_size_pixels.w);
|
||||
assert(data->view[0].image_size_pixels.h == data->view[1].image_size_pixels.h);
|
||||
CALIB_ASSERT_(data != NULL);
|
||||
CALIB_ASSERT_(data->view[0].image_size_pixels.w == data->view[1].image_size_pixels.w);
|
||||
CALIB_ASSERT_(data->view[0].image_size_pixels.h == data->view[1].image_size_pixels.h);
|
||||
|
||||
assert(data->view[0].use_fisheye == data->view[1].use_fisheye);
|
||||
CALIB_ASSERT_(data->view[0].use_fisheye == data->view[1].use_fisheye);
|
||||
|
||||
cv::Size image_size(data->view[0].image_size_pixels.w, data->view[0].image_size_pixels.h);
|
||||
StereoCameraCalibrationWrapper wrapped(data);
|
||||
|
@ -168,6 +195,14 @@ StereoRectificationMaps::StereoRectificationMaps(t_stereo_camera_calibration *da
|
|||
}
|
||||
} // namespace xrt::auxiliary::tracking
|
||||
|
||||
using std::array;
|
||||
using std::string;
|
||||
using std::vector;
|
||||
using xrt::auxiliary::tracking::CameraCalibrationWrapper;
|
||||
using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper;
|
||||
using xrt::auxiliary::util::json::JSONBuilder;
|
||||
using xrt::auxiliary::util::json::JSONNode;
|
||||
|
||||
/*
|
||||
*
|
||||
* Load functions.
|
||||
|
@ -190,8 +225,8 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
cv::Mat_<float> mat_image_size(2, 1);
|
||||
bool result = read_cv_mat(calib_file, &wrapped.view[0].intrinsics_mat, "l_intrinsics"); // 3 x 3
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[1].intrinsics_mat, "r_intrinsics"); // 3 x 3
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_mat, "l_distortion"); // 1 x 5
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_mat, "r_distortion"); // 1 x 5
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_mat, "l_distortion"); // 5 x 1
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_mat, "r_distortion"); // 5 x 1
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[0].distortion_fisheye_mat, "l_distortion_fisheye"); // 4 x 1
|
||||
result = result && read_cv_mat(calib_file, &wrapped.view[1].distortion_fisheye_mat, "r_distortion_fisheye"); // 4 x 1
|
||||
result = result && read_cv_mat(calib_file, &dummy, "l_rotation"); // 3 x 3
|
||||
|
@ -204,7 +239,7 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
result = result && read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
|
||||
|
||||
if (!result) {
|
||||
U_LOG_W("Re-run calibration!");
|
||||
CALIB_WARN("Re-run calibration!");
|
||||
return false;
|
||||
}
|
||||
wrapped.view[0].image_size_pixels.w = uint32_t(mat_image_size(0, 0));
|
||||
|
@ -217,22 +252,22 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
}
|
||||
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_translation_mat, "translation")) {
|
||||
U_LOG_W("Re-run calibration!");
|
||||
CALIB_WARN("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_rotation_mat, "rotation")) {
|
||||
U_LOG_W("Re-run calibration!");
|
||||
CALIB_WARN("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_essential_mat, "essential")) {
|
||||
U_LOG_W("Re-run calibration!");
|
||||
CALIB_WARN("Re-run calibration!");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &wrapped.camera_fundamental_mat, "fundamental")) {
|
||||
U_LOG_W("Re-run calibration!");
|
||||
CALIB_WARN("Re-run calibration!");
|
||||
}
|
||||
|
||||
cv::Mat_<float> mat_use_fisheye(1, 1);
|
||||
if (!read_cv_mat(calib_file, &mat_use_fisheye, "use_fisheye")) {
|
||||
wrapped.view[0].use_fisheye = false;
|
||||
U_LOG_W("Re-run calibration! (Assuming not fisheye)");
|
||||
CALIB_WARN("Re-run calibration! (Assuming not fisheye)");
|
||||
} else {
|
||||
wrapped.view[0].use_fisheye = mat_use_fisheye(0, 0) != 0.0f;
|
||||
}
|
||||
|
@ -240,7 +275,7 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
// clang-format on
|
||||
|
||||
|
||||
assert(wrapped.isDataStorageValid());
|
||||
CALIB_ASSERT_(wrapped.isDataStorageValid());
|
||||
|
||||
t_stereo_camera_calibration_reference(out_data, data_ptr);
|
||||
t_stereo_camera_calibration_reference(&data_ptr, NULL);
|
||||
|
@ -248,6 +283,135 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
return true;
|
||||
}
|
||||
|
||||
#define PINHOLE_RADTAN5 "pinhole_radtan5"
|
||||
#define FISHEYE_EQUIDISTANT4 "fisheye_equidistant4"
|
||||
|
||||
//! Fills @p out_mat from a json array stored in @p jn. Returns true if @p jn is
|
||||
//! a valid @p rows * @p cols matrix, false otherwise.
|
||||
static bool
|
||||
load_mat_field(const JSONNode &jn, int rows, int cols, cv::Mat_<double> &out_mat)
|
||||
{
|
||||
vector<JSONNode> data = jn.asArray();
|
||||
bool valid = jn.isArray() && data.size() == static_cast<size_t>(rows * cols);
|
||||
|
||||
if (valid) {
|
||||
out_mat.create(rows, cols);
|
||||
for (int i = 0; i < rows * cols; i++) {
|
||||
out_mat(i) = data[i].asDouble();
|
||||
}
|
||||
} else {
|
||||
CALIB_WARN("Invalid '%s' matrix field", jn.getName().c_str());
|
||||
}
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Overload of @ref load_mat_field that saves the result into a 2D C-array.
|
||||
*/
|
||||
template <int rows, int cols>
|
||||
XRT_MAYBE_UNUSED static bool
|
||||
load_mat_field(const JSONNode &jn, double (&out_arr)[rows][cols])
|
||||
{
|
||||
cv::Mat_<double> cvmat{rows, cols, &out_arr[0][0]}; // Wraps out_arr address
|
||||
return load_mat_field(jn, rows, cols, cvmat);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Overload of @ref load_mat_field that saves the result into a 1D C-array.
|
||||
*/
|
||||
template <int dim>
|
||||
XRT_MAYBE_UNUSED static bool
|
||||
load_mat_field(const JSONNode &jn, double (&out_arr)[dim])
|
||||
{
|
||||
cv::Mat_<double> cvmat{dim, 1, &out_arr[0]}; // Wraps out_arr address
|
||||
return load_mat_field(jn, dim, 1, cvmat);
|
||||
}
|
||||
|
||||
static bool
|
||||
t_camera_calibration_load_v2(cJSON *cjson_cam, t_camera_calibration *cc)
|
||||
{
|
||||
JSONNode jc{cjson_cam};
|
||||
|
||||
string model = jc["model"].asString();
|
||||
memset(&cc->intrinsics, 0, sizeof(cc->intrinsics));
|
||||
cc->intrinsics[0][0] = jc["intrinsics"]["fx"].asDouble();
|
||||
cc->intrinsics[1][1] = jc["intrinsics"]["fy"].asDouble();
|
||||
cc->intrinsics[0][2] = jc["intrinsics"]["cx"].asDouble();
|
||||
cc->intrinsics[1][2] = jc["intrinsics"]["cy"].asDouble();
|
||||
cc->intrinsics[2][2] = 1;
|
||||
|
||||
size_t n = jc["distortion"].asObject().size();
|
||||
if (model == PINHOLE_RADTAN5) {
|
||||
cc->use_fisheye = false;
|
||||
CALIB_ASSERTR(n == 5, "%zu != 5 distortion params", n);
|
||||
|
||||
constexpr array names{"k1", "k2", "p1", "p2", "k3"};
|
||||
for (size_t i = 0; i < n; i++) {
|
||||
cc->distortion[i] = jc["distortion"][names[i]].asDouble();
|
||||
}
|
||||
} else if (model == FISHEYE_EQUIDISTANT4) {
|
||||
cc->use_fisheye = true;
|
||||
CALIB_ASSERTR(n == 4, "%zu != 4 distortion params", n);
|
||||
|
||||
constexpr array names{"k1", "k2", "k3", "k4"};
|
||||
for (size_t i = 0; i < n; i++) {
|
||||
cc->distortion_fisheye[i] = jc["distortion"][names[i]].asDouble();
|
||||
}
|
||||
} else {
|
||||
CALIB_ASSERTR(false, "Invalid camera model: '%s'", model.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
cc->image_size_pixels.w = jc["resolution"]["width"].asInt();
|
||||
cc->image_size_pixels.h = jc["resolution"]["height"].asInt();
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
t_stereo_camera_calibration_load_v2(const char *calib_path, struct t_stereo_camera_calibration **out_stereo)
|
||||
{
|
||||
JSONNode json = JSONNode::loadFromFile(calib_path);
|
||||
if (json.isInvalid()) {
|
||||
CALIB_ERROR("Unable to open calibration file: '%s'", calib_path);
|
||||
return false;
|
||||
}
|
||||
|
||||
StereoCameraCalibrationWrapper stereo{5}; // Hardcoded to 5 distortion parameters.
|
||||
|
||||
// Load file metadata
|
||||
const int supported_version = 2;
|
||||
int version = json["file"]["version"].asInt(supported_version);
|
||||
if (json["file"]["version"].isInvalid()) {
|
||||
CALIB_WARN("'file.version' not found in %s, will assume v2", calib_path);
|
||||
}
|
||||
CALIB_ASSERTR(version == supported_version, "Calibration file version (%d) != %d", version, supported_version);
|
||||
|
||||
// Load cameras
|
||||
vector<JSONNode> cameras = json["cameras"].asArray();
|
||||
bool okmats = true;
|
||||
CALIB_ASSERTR(cameras.size() == 2, "Two cameras must be specified, %zu given", cameras.size());
|
||||
for (size_t i = 0; i < cameras.size(); i++) {
|
||||
JSONNode jc = cameras[i];
|
||||
CameraCalibrationWrapper &cc = stereo.view[i];
|
||||
bool loaded = t_camera_calibration_load_v2(jc.getCJSON(), &cc.base);
|
||||
CALIB_ASSERTR(loaded, "Unable to load camera calibration: %s", jc.toString(false).c_str());
|
||||
}
|
||||
|
||||
JSONNode rel = json["opencv_stereo_calibrate"];
|
||||
okmats &= load_mat_field(rel["rotation"], 3, 3, stereo.camera_rotation_mat);
|
||||
okmats &= load_mat_field(rel["translation"], 3, 1, stereo.camera_translation_mat);
|
||||
okmats &= load_mat_field(rel["essential"], 3, 3, stereo.camera_essential_mat);
|
||||
okmats &= load_mat_field(rel["fundamental"], 3, 3, stereo.camera_fundamental_mat);
|
||||
|
||||
CALIB_ASSERTR(okmats, "One or more calibration matrices couldn't be loaded");
|
||||
CALIB_ASSERT_(stereo.isDataStorageValid());
|
||||
|
||||
t_stereo_camera_calibration_reference(out_stereo, stereo.base);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -300,6 +464,98 @@ t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_cal
|
|||
return true;
|
||||
}
|
||||
|
||||
//! Writes @p mat data into a @p jb as a json array.
|
||||
static JSONBuilder &
|
||||
operator<<(JSONBuilder &jb, const cv::Mat_<double> &mat)
|
||||
{
|
||||
jb << "[";
|
||||
for (int i = 0; i < mat.rows * mat.cols; i++) {
|
||||
jb << mat.at<double>(i);
|
||||
}
|
||||
jb << "]";
|
||||
return jb;
|
||||
}
|
||||
|
||||
static bool
|
||||
t_stereo_camera_calibration_save_v2(const char *calib_path, struct t_stereo_camera_calibration *data)
|
||||
{
|
||||
StereoCameraCalibrationWrapper wrapped(data);
|
||||
JSONBuilder jb{};
|
||||
|
||||
jb << "{";
|
||||
jb << "$schema"
|
||||
<< "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json";
|
||||
jb << "file";
|
||||
jb << "{";
|
||||
jb << "version" << 2;
|
||||
char datetime[OS_ISO_STR_SIZE];
|
||||
to_iso_string(os_realtime_get_ns(), datetime);
|
||||
jb << "created_on" << datetime;
|
||||
jb << "}";
|
||||
|
||||
jb << "cameras";
|
||||
jb << "[";
|
||||
|
||||
// Cameras
|
||||
for (size_t i = 0; i < 2; i++) {
|
||||
const auto &view = wrapped.view[i];
|
||||
jb << "{";
|
||||
jb << "model" << (view.use_fisheye ? FISHEYE_EQUIDISTANT4 : PINHOLE_RADTAN5);
|
||||
|
||||
jb << "intrinsics";
|
||||
jb << "{";
|
||||
jb << "fx" << view.intrinsics_mat(0, 0);
|
||||
jb << "fy" << view.intrinsics_mat(1, 1);
|
||||
jb << "cx" << view.intrinsics_mat(0, 2);
|
||||
jb << "cy" << view.intrinsics_mat(1, 2);
|
||||
jb << "}";
|
||||
|
||||
jb << "distortion";
|
||||
jb << "{";
|
||||
if (view.use_fisheye) {
|
||||
int n = view.distortion_mat.size().area(); // Number of distortion parameters
|
||||
CALIB_ASSERT_(n == 4);
|
||||
|
||||
constexpr array names{"k1", "k2", "k3", "k4"};
|
||||
for (int i = 0; i < n; i++) {
|
||||
jb << names[i] << view.distortion_fisheye_mat(i);
|
||||
}
|
||||
} else {
|
||||
int n = view.distortion_mat.size().area(); // Number of distortion parameters
|
||||
CALIB_ASSERT_(n == 5);
|
||||
|
||||
constexpr array names{"k1", "k2", "p1", "p2", "k3"};
|
||||
for (int i = 0; i < n; i++) {
|
||||
jb << names[i] << view.distortion_mat(i);
|
||||
}
|
||||
}
|
||||
jb << "}";
|
||||
|
||||
jb << "resolution";
|
||||
jb << "{";
|
||||
jb << "width" << view.image_size_pixels.w;
|
||||
jb << "height" << view.image_size_pixels.h;
|
||||
jb << "}";
|
||||
|
||||
jb << "}";
|
||||
}
|
||||
|
||||
jb << "]";
|
||||
|
||||
// cv::stereoCalibrate data
|
||||
jb << "opencv_stereo_calibrate"
|
||||
<< "{";
|
||||
jb << "rotation" << wrapped.camera_rotation_mat;
|
||||
jb << "translation" << wrapped.camera_translation_mat;
|
||||
jb << "essential" << wrapped.camera_essential_mat;
|
||||
jb << "fundamental" << wrapped.camera_fundamental_mat;
|
||||
jb << "}";
|
||||
|
||||
jb << "}";
|
||||
|
||||
CALIB_INFO("Saving calibration file: %s", jb.getBuiltNode()->toString().c_str());
|
||||
return jb.getBuiltNode()->saveToFile(calib_path);
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
|
@ -328,7 +584,7 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
cv::Mat temp;
|
||||
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
if (read != 3) {
|
||||
U_LOG_E("Failed to read mat header: '%i' '%s'", (int)read, name);
|
||||
CALIB_ERROR("Failed to read mat header: '%i' '%s'", (int)read, name);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -344,18 +600,19 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
}
|
||||
read = fread(static_cast<void *>(temp.data), header[0], header[1] * header[2], f);
|
||||
if (read != (header[1] * header[2])) {
|
||||
U_LOG_E("Failed to read mat body: '%i' '%s'", (int)read, name);
|
||||
CALIB_ERROR("Failed to read mat body: '%i' '%s'", (int)read, name);
|
||||
return false;
|
||||
}
|
||||
if (m->empty()) {
|
||||
m->create(header[1], header[2], temp.type());
|
||||
}
|
||||
if (temp.type() != m->type()) {
|
||||
U_LOG_E("Mat body type does not match: %i vs %i for '%s'", (int)temp.type(), (int)m->type(), name);
|
||||
CALIB_ERROR("Mat body type does not match: %i vs %i for '%s'", (int)temp.type(), (int)m->type(), name);
|
||||
return false;
|
||||
}
|
||||
if (temp.total() != m->total()) {
|
||||
U_LOG_E("Mat total size does not match: %i vs %i for '%s'", (int)temp.total(), (int)m->total(), name);
|
||||
CALIB_ERROR("Mat total size does not match: %i vs %i for '%s'", (int)temp.total(), (int)m->total(),
|
||||
name);
|
||||
return false;
|
||||
}
|
||||
if (temp.size() == m->size()) {
|
||||
|
@ -364,12 +621,12 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
|||
return true;
|
||||
}
|
||||
if (temp.size().width == m->size().height && temp.size().height == m->size().width) {
|
||||
U_LOG_W("Mat transposing on load: '%s'", name);
|
||||
CALIB_WARN("Mat transposing on load: '%s'", name);
|
||||
// needs transpose
|
||||
cv::transpose(temp, *m);
|
||||
return true;
|
||||
}
|
||||
// highly unlikely so minimally-helpful error message.
|
||||
U_LOG_E("Mat dimension unknown mismatch: '%s'", name);
|
||||
CALIB_ERROR("Mat dimension unknown mismatch: '%s'", name);
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -94,6 +94,9 @@ public:
|
|||
: cjson(cjson), is_owner(is_owner), parent(parent)
|
||||
{}
|
||||
|
||||
//! Wrap cJSON object for easy manipulation, does not take ownership
|
||||
JSONNode(cJSON *cjson) : JSONNode(cjson, false, nullptr) {}
|
||||
|
||||
//! Makes a null object; `isInvalid()` on it returns true.
|
||||
JSONNode() {}
|
||||
|
||||
|
@ -321,6 +324,12 @@ public:
|
|||
{
|
||||
return string(cjson->string ? cjson->string : "");
|
||||
}
|
||||
|
||||
cJSON *
|
||||
getCJSON()
|
||||
{
|
||||
return cjson;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
Loading…
Reference in a new issue