mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-03-03 21:26:36 +00:00
aux/tracking: Refactor calibration data handling
This commit is contained in:
Jakob Bornecrantz
parent
b37eb0f55b
commit
51f3fac192
src/xrt/auxiliary
|
|
@ -24,6 +24,7 @@ set(TRACKING_SOURCE_FILES
|
|||
tracking/t_debug_hsv_filter.cpp
|
||||
tracking/t_debug_hsv_picker.cpp
|
||||
tracking/t_debug_hsv_viewer.cpp
|
||||
tracking/t_file.cpp
|
||||
tracking/t_hsv_filter.c
|
||||
tracking/t_tracker_psmv.cpp
|
||||
tracking/t_tracker_psvr.cpp
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ tracking_srcs = [
|
|||
'tracking/t_debug_hsv_filter.cpp',
|
||||
'tracking/t_debug_hsv_picker.cpp',
|
||||
'tracking/t_debug_hsv_viewer.cpp',
|
||||
'tracking/t_file.cpp',
|
||||
'tracking/t_hsv_filter.c',
|
||||
'tracking/t_tracker_psmv.cpp',
|
||||
'tracking/t_tracker_psvr.cpp',
|
||||
|
|
|
|||
|
|
@ -276,6 +276,7 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
c.state.calibrated = true;
|
||||
|
||||
cv::Size image_size(cols, rows);
|
||||
cv::Size new_image_size(cols, rows);
|
||||
|
||||
// we don't serialise these
|
||||
cv::Mat camera_rotation;
|
||||
|
|
@ -283,7 +284,11 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
cv::Mat camera_essential;
|
||||
cv::Mat camera_fundamental;
|
||||
|
||||
struct opencv_calibration_params cp;
|
||||
CalibrationRawData raw;
|
||||
raw.image_size_pixels.w = image_size.width;
|
||||
raw.image_size_pixels.h = image_size.height;
|
||||
raw.new_image_size_pixels.w = new_image_size.width;
|
||||
raw.new_image_size_pixels.h = new_image_size.height;
|
||||
|
||||
cv::Mat zero_distortion = cv::Mat(5, 1, CV_32F, cv::Scalar(0.0f));
|
||||
|
||||
|
|
@ -303,10 +308,10 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
cv::stereoCalibrate(c.state.chessboards_model, // objectPoints
|
||||
c.state.view[0].measured, // inagePoints1
|
||||
c.state.view[1].measured, // imagePoints2,
|
||||
cp.l_intrinsics, // cameraMatrix1
|
||||
cp.l_distortion, // distCoeffs1
|
||||
cp.r_intrinsics, // cameraMatrix2
|
||||
cp.r_distortion, // distCoeffs2
|
||||
raw.l_intrinsics, // cameraMatrix1
|
||||
raw.l_distortion, // distCoeffs1
|
||||
raw.r_intrinsics, // cameraMatrix2
|
||||
raw.r_distortion, // distCoeffs2
|
||||
image_size, // imageSize
|
||||
camera_rotation, // R
|
||||
camera_translation, // T
|
||||
|
|
@ -314,73 +319,37 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
camera_fundamental, // F
|
||||
0); // flags
|
||||
|
||||
std::cout << "calibration rp_error: " << rp_error << "\n";
|
||||
std::cout << "calibration camera_translation:\n"
|
||||
<< camera_translation << "\n";
|
||||
raw.translation.x = camera_translation.at<float>(0, 0);
|
||||
raw.translation.y = camera_translation.at<float>(0, 1);
|
||||
raw.translation.z = camera_translation.at<float>(0, 2);
|
||||
|
||||
// We currently don't change the image size or remove invalid pixels.
|
||||
cv::stereoRectify(cp.l_intrinsics, // cameraMatrix1
|
||||
cv::stereoRectify(raw.l_intrinsics, // cameraMatrix1
|
||||
zero_distortion, // distCoeffs1
|
||||
cp.r_intrinsics, // cameraMatrix2
|
||||
raw.r_intrinsics, // cameraMatrix2
|
||||
zero_distortion, // distCoeffs2
|
||||
image_size, // imageSize
|
||||
camera_rotation, // R
|
||||
camera_translation, // T
|
||||
cp.l_rotation, // R1
|
||||
cp.r_rotation, // R2
|
||||
cp.l_projection, // P1
|
||||
cp.r_projection, // P2
|
||||
cp.disparity_to_depth, // Q
|
||||
raw.l_rotation, // R1
|
||||
raw.r_rotation, // R2
|
||||
raw.l_projection, // P1
|
||||
raw.r_projection, // P2
|
||||
raw.disparity_to_depth, // Q
|
||||
cv::CALIB_ZERO_DISPARITY, // flags
|
||||
-1, // alpha
|
||||
image_size, // newImageSize
|
||||
new_image_size, // newImageSize
|
||||
NULL, // validPixROI1
|
||||
NULL); // validPixROI2
|
||||
|
||||
P("CALIBRATION DONE RP ERROR %f", rp_error);
|
||||
|
||||
char path_string[PATH_MAX];
|
||||
char file_string[PATH_MAX];
|
||||
// TODO: centralise this - use multiple env vars?
|
||||
char *config_path = secure_getenv("HOME");
|
||||
snprintf(path_string, PATH_MAX, "%s/.config/monado", config_path);
|
||||
snprintf(file_string, PATH_MAX, "%s/.config/monado/%s.calibration",
|
||||
config_path, "PS4_EYE");
|
||||
FILE *calib_file = fopen(file_string, "wb");
|
||||
if (!calib_file) {
|
||||
// try creating it
|
||||
mkpath(path_string);
|
||||
}
|
||||
calib_file = fopen(file_string, "wb");
|
||||
if (!calib_file) {
|
||||
printf(
|
||||
"ERROR. could not create calibration file "
|
||||
"%s\n",
|
||||
file_string);
|
||||
return;
|
||||
}
|
||||
std::cout << "calibration rp_error: " << rp_error << "\n";
|
||||
std::cout << "calibration camera_translation:\n"
|
||||
<< camera_translation << "\n";
|
||||
|
||||
write_cv_mat(calib_file, &cp.l_intrinsics);
|
||||
write_cv_mat(calib_file, &cp.r_intrinsics);
|
||||
write_cv_mat(calib_file, &cp.l_distortion);
|
||||
write_cv_mat(calib_file, &cp.r_distortion);
|
||||
write_cv_mat(calib_file, &cp.l_distortion_fisheye);
|
||||
write_cv_mat(calib_file, &cp.r_distortion_fisheye);
|
||||
write_cv_mat(calib_file, &cp.l_rotation);
|
||||
write_cv_mat(calib_file, &cp.r_rotation);
|
||||
write_cv_mat(calib_file, &cp.l_translation);
|
||||
write_cv_mat(calib_file, &cp.r_translation);
|
||||
write_cv_mat(calib_file, &cp.l_projection);
|
||||
write_cv_mat(calib_file, &cp.r_projection);
|
||||
write_cv_mat(calib_file, &cp.disparity_to_depth);
|
||||
|
||||
cv::Mat mat_image_size;
|
||||
mat_image_size.create(1, 2, CV_32F);
|
||||
mat_image_size.at<float>(0, 0) = image_size.width;
|
||||
mat_image_size.at<float>(0, 1) = image_size.height;
|
||||
write_cv_mat(calib_file, &mat_image_size);
|
||||
|
||||
fclose(calib_file);
|
||||
t_file_save_raw_data_hack(&raw);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "tracking/t_tracking.h"
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <sys/stat.h>
|
||||
|
||||
|
|
@ -16,7 +18,10 @@ extern "C" {
|
|||
#endif
|
||||
|
||||
|
||||
struct opencv_calibration_params
|
||||
/*!
|
||||
* What calibration data that is saved down to file.
|
||||
*/
|
||||
struct CalibrationRawData : t_calibration_raw_data
|
||||
{
|
||||
cv::Mat l_intrinsics = {};
|
||||
cv::Mat l_distortion = {};
|
||||
|
|
@ -31,52 +36,26 @@ struct opencv_calibration_params
|
|||
cv::Mat r_rotation = {};
|
||||
cv::Mat r_projection = {};
|
||||
cv::Mat disparity_to_depth = {};
|
||||
cv::Mat mat_image_size = {};
|
||||
};
|
||||
|
||||
XRT_MAYBE_UNUSED static bool
|
||||
write_cv_mat(FILE *f, cv::Mat *m)
|
||||
struct CalibrationData : t_calibration_data
|
||||
{
|
||||
uint32_t header[3];
|
||||
header[0] = static_cast<uint32_t>(m->elemSize());
|
||||
header[1] = static_cast<uint32_t>(m->rows);
|
||||
header[2] = static_cast<uint32_t>(m->cols);
|
||||
fwrite(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
fwrite(static_cast<void *>(m->data), header[0], header[1] * header[2],
|
||||
f);
|
||||
return true;
|
||||
}
|
||||
cv::Mat l_undistort_map_x = {};
|
||||
cv::Mat l_undistort_map_y = {};
|
||||
cv::Mat l_rectify_map_x = {};
|
||||
cv::Mat l_rectify_map_y = {};
|
||||
cv::Mat r_undistort_map_x = {};
|
||||
cv::Mat r_undistort_map_y = {};
|
||||
cv::Mat r_rectify_map_x = {};
|
||||
cv::Mat r_rectify_map_y = {};
|
||||
cv::Mat disparity_to_depth = {};
|
||||
};
|
||||
|
||||
XRT_MAYBE_UNUSED static bool
|
||||
read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
||||
{
|
||||
uint32_t header[3] = {};
|
||||
size_t read = 0;
|
||||
extern "C" bool
|
||||
t_file_load_stereo_calibration_v1_hack(struct t_calibration_data **out_data);
|
||||
|
||||
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
if (read != 3) {
|
||||
printf("Failed to read mat header: '%i' '%s'\n", (int)read,
|
||||
name);
|
||||
return false;
|
||||
}
|
||||
|
||||
//! @todo We may have written things other than CV_32F and CV_64F.
|
||||
if (header[0] == 4) {
|
||||
m->create(static_cast<int>(header[1]),
|
||||
static_cast<int>(header[2]), CV_32F);
|
||||
} else {
|
||||
m->create(static_cast<int>(header[1]),
|
||||
static_cast<int>(header[2]), CV_64F);
|
||||
}
|
||||
read = fread(static_cast<void *>(m->data), header[0],
|
||||
header[1] * header[2], f);
|
||||
if (read != (header[1] * header[2])) {
|
||||
printf("Failed to read mat body: '%i' '%s'\n", (int)read, name);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
extern "C" bool
|
||||
t_file_save_raw_data_hack(struct t_calibration_raw_data *raw_data);
|
||||
|
||||
XRT_MAYBE_UNUSED static bool
|
||||
calibration_get_stereo(const char *configuration_filename,
|
||||
|
|
@ -93,77 +72,26 @@ calibration_get_stereo(const char *configuration_filename,
|
|||
cv::Mat *r_rectify_map_y,
|
||||
cv::Mat *disparity_to_depth)
|
||||
{
|
||||
struct opencv_calibration_params cp;
|
||||
cv::Mat zero_distortion = cv::Mat(5, 1, CV_32F, cv::Scalar(0.0f));
|
||||
t_calibration_data *data_c;
|
||||
bool ok = t_file_load_stereo_calibration_v1_hack(&data_c);
|
||||
|
||||
char path_string[256]; //! @todo 256 maybe not enough
|
||||
//! @todo Use multiple env vars?
|
||||
char *config_path = secure_getenv("HOME");
|
||||
snprintf(path_string, 256, "%s/.config/monado/%s.calibration",
|
||||
config_path, configuration_filename); //! @todo Hardcoded 256
|
||||
|
||||
FILE *calib_file = fopen(path_string, "rb");
|
||||
if (calib_file == NULL) {
|
||||
if (!ok) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Read our calibration from this file
|
||||
// clang-format off
|
||||
read_cv_mat(calib_file, &cp.l_intrinsics, "l_intrinsics");
|
||||
read_cv_mat(calib_file, &cp.r_intrinsics, "r_intrinsics");
|
||||
read_cv_mat(calib_file, &cp.l_distortion, "l_distortion");
|
||||
read_cv_mat(calib_file, &cp.r_distortion, "r_distortion");
|
||||
read_cv_mat(calib_file, &cp.l_distortion_fisheye, "l_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &cp.r_distortion_fisheye, "r_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &cp.l_rotation, "l_rotation");
|
||||
read_cv_mat(calib_file, &cp.r_rotation, "r_rotation");
|
||||
read_cv_mat(calib_file, &cp.l_translation, "l_translation");
|
||||
read_cv_mat(calib_file, &cp.r_translation, "r_translation");
|
||||
read_cv_mat(calib_file, &cp.l_projection, "l_projection");
|
||||
read_cv_mat(calib_file, &cp.r_projection, "r_projection");
|
||||
read_cv_mat(calib_file, &cp.disparity_to_depth, "disparity_to_depth");
|
||||
read_cv_mat(calib_file, &cp.mat_image_size, "mat_image_size");
|
||||
// clang-format on
|
||||
CalibrationData *data = (CalibrationData *)data_c;
|
||||
|
||||
// provided by caller
|
||||
*disparity_to_depth = cp.disparity_to_depth;
|
||||
*l_undistort_map_x = data->l_undistort_map_x;
|
||||
*l_undistort_map_y = data->l_undistort_map_y;
|
||||
*l_rectify_map_x = data->l_rectify_map_x;
|
||||
*l_rectify_map_y = data->l_rectify_map_y;
|
||||
*r_undistort_map_x = data->r_undistort_map_x;
|
||||
*r_undistort_map_y = data->r_undistort_map_y;
|
||||
*r_rectify_map_x = data->r_rectify_map_x;
|
||||
*r_rectify_map_y = data->r_rectify_map_y;
|
||||
*disparity_to_depth = data->disparity_to_depth;
|
||||
|
||||
//! @todo Scale Our intrinsics if the frame size we request
|
||||
// calibration for does not match what was saved
|
||||
|
||||
cv::Size image_size(int(cp.mat_image_size.at<float>(0, 0)),
|
||||
int(cp.mat_image_size.at<float>(0, 1)));
|
||||
|
||||
// Generate undistortion maps - handle fisheye or rectilinear sources
|
||||
|
||||
if (use_fisheye) {
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
cp.l_intrinsics, cp.l_distortion_fisheye, cv::noArray(),
|
||||
cp.l_intrinsics, image_size, CV_32FC1, *l_undistort_map_x,
|
||||
*l_undistort_map_y);
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
cp.r_intrinsics, cp.r_distortion_fisheye, cv::noArray(),
|
||||
cp.r_intrinsics, image_size, CV_32FC1, *r_undistort_map_x,
|
||||
*r_undistort_map_y);
|
||||
} else {
|
||||
cv::initUndistortRectifyMap(
|
||||
cp.l_intrinsics, cp.l_distortion, cv::noArray(),
|
||||
cp.l_intrinsics, image_size, CV_32FC1, *l_undistort_map_x,
|
||||
*l_undistort_map_y);
|
||||
cv::initUndistortRectifyMap(
|
||||
cp.r_intrinsics, cp.r_distortion, cv::noArray(),
|
||||
cp.r_intrinsics, image_size, CV_32FC1, *r_undistort_map_x,
|
||||
*r_undistort_map_y);
|
||||
}
|
||||
|
||||
// Generate our rectification maps
|
||||
|
||||
cv::initUndistortRectifyMap(
|
||||
cp.l_intrinsics, zero_distortion, cp.l_rotation, cp.l_projection,
|
||||
image_size, CV_32FC1, *l_rectify_map_x, *l_rectify_map_y);
|
||||
cv::initUndistortRectifyMap(
|
||||
cp.r_intrinsics, zero_distortion, cp.r_rotation, cp.r_projection,
|
||||
image_size, CV_32FC1, *r_rectify_map_x, *r_rectify_map_y);
|
||||
t_calibration_data_free(data_c);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
311
src/xrt/auxiliary/tracking/t_file.cpp
Normal file
311
src/xrt/auxiliary/tracking/t_file.cpp
Normal file
|
|
@ -0,0 +1,311 @@
|
|||
// Copyright 2019, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Handling of files and calibration data.
|
||||
* @author Pete Black <pblack@collabora.com>
|
||||
* @author Jakob Bornecrantz <jakob@collabora.com>
|
||||
*/
|
||||
|
||||
#include "t_calibration_opencv.h"
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Pre-declar functions.
|
||||
*
|
||||
*/
|
||||
|
||||
static bool
|
||||
read_cv_mat(FILE *f, cv::Mat *m, const char *name);
|
||||
|
||||
static bool
|
||||
write_cv_mat(FILE *f, cv::Mat *m);
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Free functions.
|
||||
*
|
||||
*/
|
||||
|
||||
extern "C" void
|
||||
t_calibration_data_free(struct t_calibration_data *data)
|
||||
{
|
||||
CalibrationData *d_ptr = (CalibrationData *)data;
|
||||
delete d_ptr;
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
t_calibration_raw_data_free(struct t_calibration_raw_data *raw_data)
|
||||
{
|
||||
CalibrationRawData *rd_ptr = (CalibrationRawData *)raw_data;
|
||||
delete rd_ptr;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Load functions.
|
||||
*
|
||||
*/
|
||||
|
||||
extern "C" bool
|
||||
t_file_load_stereo_calibration_v1(FILE *calib_file,
|
||||
struct t_calibration_data **out_data,
|
||||
struct t_calibration_raw_data **out_raw_data)
|
||||
{
|
||||
CalibrationRawData &raw = *(new CalibrationRawData());
|
||||
CalibrationData &data = *(new CalibrationData());
|
||||
|
||||
cv::Mat zero_distortion = cv::Mat(5, 1, CV_32F, cv::Scalar(0.0f));
|
||||
|
||||
//! @todo Load from file.
|
||||
bool use_fisheye = false;
|
||||
|
||||
// Read our calibration from this file
|
||||
// clang-format off
|
||||
read_cv_mat(calib_file, &raw.l_intrinsics, "l_intrinsics");
|
||||
read_cv_mat(calib_file, &raw.r_intrinsics, "r_intrinsics");
|
||||
read_cv_mat(calib_file, &raw.l_distortion, "l_distortion");
|
||||
read_cv_mat(calib_file, &raw.r_distortion, "r_distortion");
|
||||
read_cv_mat(calib_file, &raw.l_distortion_fisheye, "l_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &raw.r_distortion_fisheye, "r_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &raw.l_rotation, "l_rotation");
|
||||
read_cv_mat(calib_file, &raw.r_rotation, "r_rotation");
|
||||
read_cv_mat(calib_file, &raw.l_translation, "l_translation");
|
||||
read_cv_mat(calib_file, &raw.r_translation, "r_translation");
|
||||
read_cv_mat(calib_file, &raw.l_projection, "l_projection");
|
||||
read_cv_mat(calib_file, &raw.r_projection, "r_projection");
|
||||
read_cv_mat(calib_file, &raw.disparity_to_depth, "disparity_to_depth");
|
||||
cv::Mat mat_image_size = {};
|
||||
read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
|
||||
|
||||
raw.image_size_pixels.w = uint32_t(mat_image_size.at<float>(0, 0));
|
||||
raw.image_size_pixels.h = uint32_t(mat_image_size.at<float>(0, 1));
|
||||
cv::Size image_size(raw.image_size_pixels.w, raw.image_size_pixels.h);
|
||||
|
||||
cv::Mat mat_new_image_size = {};
|
||||
if (read_cv_mat(calib_file, &mat_new_image_size, "mat_new_image_size")) {
|
||||
raw.new_image_size_pixels.w = uint32_t(mat_new_image_size.at<float>(0, 0));
|
||||
raw.new_image_size_pixels.h = uint32_t(mat_new_image_size.at<float>(0, 1));
|
||||
} else {
|
||||
raw.new_image_size_pixels.w = raw.image_size_pixels.w;
|
||||
raw.new_image_size_pixels.h = raw.image_size_pixels.h;
|
||||
}
|
||||
|
||||
cv::Mat translation = {};
|
||||
if (read_cv_mat(calib_file, &translation, "translation")) {
|
||||
raw.translation.x = translation.at<float>(0, 0);
|
||||
raw.translation.y = translation.at<float>(0, 1);
|
||||
raw.translation.z = translation.at<float>(0, 2);
|
||||
}
|
||||
// clang-format on
|
||||
|
||||
|
||||
// No processing needed.
|
||||
data.disparity_to_depth = raw.disparity_to_depth;
|
||||
|
||||
//! @todo Scale Our intrinsics if the frame size we request
|
||||
// calibration for does not match what was saved
|
||||
|
||||
// Generate undistortion maps - handle fisheye or rectilinear sources
|
||||
|
||||
if (use_fisheye) {
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
raw.l_intrinsics, raw.l_distortion_fisheye, cv::noArray(),
|
||||
raw.l_intrinsics, image_size, CV_32FC1,
|
||||
data.l_undistort_map_x, data.l_undistort_map_y);
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
raw.r_intrinsics, raw.r_distortion_fisheye, cv::noArray(),
|
||||
raw.r_intrinsics, image_size, CV_32FC1,
|
||||
data.r_undistort_map_x, data.r_undistort_map_y);
|
||||
} else {
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.l_intrinsics, raw.l_distortion, cv::noArray(),
|
||||
raw.l_intrinsics, image_size, CV_32FC1,
|
||||
data.l_undistort_map_x, data.l_undistort_map_y);
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.r_intrinsics, raw.r_distortion, cv::noArray(),
|
||||
raw.r_intrinsics, image_size, CV_32FC1,
|
||||
data.r_undistort_map_x, data.r_undistort_map_y);
|
||||
}
|
||||
|
||||
// Generate our rectification maps
|
||||
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.l_intrinsics, zero_distortion, raw.l_rotation, raw.l_projection,
|
||||
image_size, CV_32FC1, data.l_rectify_map_x, data.l_rectify_map_y);
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.r_intrinsics, zero_distortion, raw.r_rotation, raw.r_projection,
|
||||
image_size, CV_32FC1, data.r_rectify_map_x, data.r_rectify_map_y);
|
||||
|
||||
*out_data = &data;
|
||||
*out_raw_data = &raw;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Save functions.
|
||||
*
|
||||
*/
|
||||
|
||||
extern "C" bool
|
||||
t_file_save_raw_data(FILE *calib_file, struct t_calibration_raw_data *raw_data)
|
||||
{
|
||||
CalibrationRawData &raw = *(CalibrationRawData *)raw_data;
|
||||
|
||||
write_cv_mat(calib_file, &raw.l_intrinsics);
|
||||
write_cv_mat(calib_file, &raw.r_intrinsics);
|
||||
write_cv_mat(calib_file, &raw.l_distortion);
|
||||
write_cv_mat(calib_file, &raw.r_distortion);
|
||||
write_cv_mat(calib_file, &raw.l_distortion_fisheye);
|
||||
write_cv_mat(calib_file, &raw.r_distortion_fisheye);
|
||||
write_cv_mat(calib_file, &raw.l_rotation);
|
||||
write_cv_mat(calib_file, &raw.r_rotation);
|
||||
write_cv_mat(calib_file, &raw.l_translation);
|
||||
write_cv_mat(calib_file, &raw.r_translation);
|
||||
write_cv_mat(calib_file, &raw.l_projection);
|
||||
write_cv_mat(calib_file, &raw.r_projection);
|
||||
write_cv_mat(calib_file, &raw.disparity_to_depth);
|
||||
|
||||
cv::Mat mat_image_size;
|
||||
mat_image_size.create(1, 2, CV_32F);
|
||||
mat_image_size.at<float>(0, 0) = raw.image_size_pixels.w;
|
||||
mat_image_size.at<float>(0, 1) = raw.image_size_pixels.h;
|
||||
write_cv_mat(calib_file, &mat_image_size);
|
||||
|
||||
cv::Mat mat_new_image_size;
|
||||
mat_new_image_size.create(1, 2, CV_32F);
|
||||
mat_new_image_size.at<float>(0, 0) = raw.new_image_size_pixels.w;
|
||||
mat_new_image_size.at<float>(0, 1) = raw.new_image_size_pixels.h;
|
||||
write_cv_mat(calib_file, &mat_new_image_size);
|
||||
|
||||
cv::Mat mat_translation;
|
||||
mat_translation.create(1, 3, CV_32F);
|
||||
mat_translation.at<float>(0, 0) = raw.translation.x;
|
||||
mat_translation.at<float>(0, 1) = raw.translation.y;
|
||||
mat_translation.at<float>(0, 2) = raw.translation.z;
|
||||
write_cv_mat(calib_file, &mat_translation);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Hack functions.
|
||||
*
|
||||
*/
|
||||
|
||||
extern "C" bool
|
||||
t_file_load_stereo_calibration_v1_hack(struct t_calibration_data **out_data)
|
||||
{
|
||||
const char *configuration_filename = "PS4_EYE";
|
||||
|
||||
char path_string[256]; //! @todo 256 maybe not enough
|
||||
//! @todo Use multiple env vars?
|
||||
char *config_path = secure_getenv("HOME");
|
||||
snprintf(path_string, 256, "%s/.config/monado/%s.calibration",
|
||||
config_path, configuration_filename); //! @todo Hardcoded 256
|
||||
|
||||
FILE *calib_file = fopen(path_string, "rb");
|
||||
if (calib_file == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
t_calibration_raw_data *raw_data;
|
||||
bool ret =
|
||||
t_file_load_stereo_calibration_v1(calib_file, out_data, &raw_data);
|
||||
|
||||
t_calibration_raw_data_free(raw_data);
|
||||
|
||||
fclose(calib_file);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
extern "C" bool
|
||||
t_file_save_raw_data_hack(struct t_calibration_raw_data *raw_data)
|
||||
{
|
||||
char path_string[PATH_MAX];
|
||||
char file_string[PATH_MAX];
|
||||
// TODO: centralise this - use multiple env vars?
|
||||
char *config_path = secure_getenv("HOME");
|
||||
snprintf(path_string, PATH_MAX, "%s/.config/monado", config_path);
|
||||
snprintf(file_string, PATH_MAX, "%s/.config/monado/%s.calibration",
|
||||
config_path, "PS4_EYE");
|
||||
FILE *calib_file = fopen(file_string, "wb");
|
||||
if (!calib_file) {
|
||||
// try creating it
|
||||
mkpath(path_string);
|
||||
}
|
||||
calib_file = fopen(file_string, "wb");
|
||||
if (!calib_file) {
|
||||
printf(
|
||||
"ERROR. could not create calibration file "
|
||||
"%s\n",
|
||||
file_string);
|
||||
return false;
|
||||
}
|
||||
|
||||
t_file_save_raw_data(calib_file, raw_data);
|
||||
|
||||
fclose(calib_file);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* Helpers
|
||||
*
|
||||
*/
|
||||
|
||||
static bool
|
||||
write_cv_mat(FILE *f, cv::Mat *m)
|
||||
{
|
||||
uint32_t header[3];
|
||||
header[0] = static_cast<uint32_t>(m->elemSize());
|
||||
header[1] = static_cast<uint32_t>(m->rows);
|
||||
header[2] = static_cast<uint32_t>(m->cols);
|
||||
fwrite(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
fwrite(static_cast<void *>(m->data), header[0], header[1] * header[2],
|
||||
f);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
read_cv_mat(FILE *f, cv::Mat *m, const char *name)
|
||||
{
|
||||
uint32_t header[3] = {};
|
||||
size_t read = 0;
|
||||
|
||||
read = fread(static_cast<void *>(header), sizeof(uint32_t), 3, f);
|
||||
if (read != 3) {
|
||||
printf("Failed to read mat header: '%i' '%s'\n", (int)read,
|
||||
name);
|
||||
return false;
|
||||
}
|
||||
|
||||
//! @todo We may have written things other than CV_32F and CV_64F.
|
||||
if (header[0] == 4) {
|
||||
m->create(static_cast<int>(header[1]),
|
||||
static_cast<int>(header[2]), CV_32F);
|
||||
} else {
|
||||
m->create(static_cast<int>(header[1]),
|
||||
static_cast<int>(header[2]), CV_64F);
|
||||
}
|
||||
read = fread(static_cast<void *>(m->data), header[0],
|
||||
header[1] * header[2], f);
|
||||
if (read != (header[1] * header[2])) {
|
||||
printf("Failed to read mat body: '%i' '%s'\n", (int)read, name);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -11,6 +11,9 @@
|
|||
|
||||
#include "xrt/xrt_frame.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
|
@ -180,6 +183,43 @@ t_psvr_create(struct xrt_frame_context *xfctx,
|
|||
9, 7, 0.025f, true, 5, \
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* The calibration data that C only code can see.
|
||||
*/
|
||||
struct t_calibration_raw_data
|
||||
{
|
||||
struct xrt_size image_size_pixels;
|
||||
struct xrt_size new_image_size_pixels;
|
||||
|
||||
//! Translation between stereo cameras.
|
||||
struct xrt_vec3 translation;
|
||||
};
|
||||
|
||||
/*!
|
||||
* Free raw calibration data.
|
||||
*/
|
||||
void
|
||||
t_calibration_raw_data_free(struct t_calibration_raw_data *raw_data);
|
||||
|
||||
/*!
|
||||
* Refined calibration data to be given to trackers.
|
||||
*/
|
||||
struct t_calibration_data
|
||||
{
|
||||
struct xrt_size image_size_pixels;
|
||||
struct xrt_size new_image_size_pixels;
|
||||
};
|
||||
|
||||
void
|
||||
t_calibration_data_free(struct t_calibration_data *data);
|
||||
|
||||
bool
|
||||
t_file_load_stereo_calibration_v1(FILE *calib_file,
|
||||
struct t_calibration_data **out_data,
|
||||
struct t_calibration_raw_data **out_raw_data);
|
||||
|
||||
struct t_calibration_params
|
||||
{
|
||||
int checker_cols_num;
|
||||
|
|
|
|||
Loading…
Reference in a new issue