aux/tracking: Refactor calibration data handling

This commit is contained in:
Jakob Bornecrantz 2019-09-29 15:30:11 +01:00
parent b37eb0f55b
commit 51f3fac192
6 changed files with 413 additions and 163 deletions

View file

@ -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

View file

@ -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',

View file

@ -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);
}

View file

@ -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;
}

View 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;
}

View file

@ -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;