mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-21 06:01:43 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

t/file: Save more calibration data

Browse source
This commit is contained in:
Jakob Bornecrantz 2019-10-12 21:43:26 +01:00
parent 9ec1205f37
commit 43c366ad41
4 changed files with 195 additions and 135 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

89
src/xrt/auxiliary/tracking/t_calibration.cpp
View file

@ -279,20 +279,14 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
cv::Size image_size(cols, rows); cv::Size image_size(cols, rows);
cv::Size new_image_size(cols, rows); cv::Size new_image_size(cols, rows);
// we don't serialise these CalibrationRawData raw = {};
cv::Mat camera_rotation; assert(raw.isDataStorageValid());
cv::Mat camera_translation;
cv::Mat camera_essential;
cv::Mat camera_fundamental;
CalibrationRawData raw;
raw.image_size_pixels.w = image_size.width; raw.image_size_pixels.w = image_size.width;
raw.image_size_pixels.h = image_size.height; raw.image_size_pixels.h = image_size.height;
raw.new_image_size_pixels.w = new_image_size.width; raw.new_image_size_pixels.w = new_image_size.width;
raw.new_image_size_pixels.h = new_image_size.height; raw.new_image_size_pixels.h = new_image_size.height;
cv::Mat zero_distortion = cv::Mat(5, 1, CV_32F, cv::Scalar(0.0f));
// TODO: handle both fisheye and normal cameras -right // TODO: handle both fisheye and normal cameras -right
// now I only have the normal, for the PS4 camera // now I only have the normal, for the PS4 camera
#if 0 #if 0
@ -306,49 +300,56 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
// non-fisheye version // non-fisheye version
float rp_error = float rp_error =
cv::stereoCalibrate(c.state.chessboards_model, // objectPoints cv::stereoCalibrate(c.state.chessboards_model, // objectPoints
c.state.view[0].measured, // inagePoints1 c.state.view[0].measured, // inagePoints1
c.state.view[1].measured, // imagePoints2, c.state.view[1].measured, // imagePoints2,
raw.l_intrinsics, // cameraMatrix1 raw.l_intrinsics_mat, // cameraMatrix1
raw.l_distortion, // distCoeffs1 raw.l_distortion_mat, // distCoeffs1
raw.r_intrinsics, // cameraMatrix2 raw.r_intrinsics_mat, // cameraMatrix2
raw.r_distortion, // distCoeffs2 raw.r_distortion_mat, // distCoeffs2
image_size, // imageSize image_size, // imageSize
camera_rotation, // R raw.camera_rotation_mat, // R
camera_translation, // T raw.camera_translation_mat, // T
camera_essential, // E raw.camera_essential_mat, // E
camera_fundamental, // F raw.camera_fundamental_mat, // F
0); // flags 0); // flags
raw.translation.x = camera_translation.at<float>(0, 0); assert(raw.camera_rotation_mat.size() == cv::Size(3, 3));
raw.translation.y = camera_translation.at<float>(0, 1); assert(raw.camera_translation_mat.size() == cv::Size(1, 3));
raw.translation.z = camera_translation.at<float>(0, 2); assert(raw.camera_essential_mat.size() == cv::Size(3, 3));
assert(raw.camera_fundamental_mat.size() == cv::Size(3, 3));
// We currently don't change the image size or remove invalid pixels. // We currently don't change the image size or remove invalid pixels.
cv::stereoRectify(raw.l_intrinsics, // cameraMatrix1 cv::stereoRectify(raw.l_intrinsics_mat, // cameraMatrix1
zero_distortion, // distCoeffs1 cv::noArray(), // distCoeffs1
raw.r_intrinsics, // cameraMatrix2 raw.r_intrinsics_mat, // cameraMatrix2
zero_distortion, // distCoeffs2 cv::noArray(), // distCoeffs2
image_size, // imageSize image_size, // imageSize
camera_rotation, // R raw.camera_rotation_mat, // R
camera_translation, // T raw.camera_translation_mat, // T
raw.l_rotation, // R1 raw.l_rotation_mat, // R1
raw.r_rotation, // R2 raw.r_rotation_mat, // R2
raw.l_projection, // P1 raw.l_projection_mat, // P1
raw.r_projection, // P2 raw.r_projection_mat, // P2
raw.disparity_to_depth, // Q raw.disparity_to_depth_mat, // Q
cv::CALIB_ZERO_DISPARITY, // flags cv::CALIB_ZERO_DISPARITY, // flags
-1, // alpha -1, // alpha
new_image_size, // newImageSize new_image_size, // newImageSize
NULL, // validPixROI1 NULL, // validPixROI1
NULL); // validPixROI2 NULL); // validPixROI2
// Validate that nothing has been re-allocated.
assert(raw.isDataStorageValid());
P("CALIBRATION DONE RP ERROR %f", rp_error); P("CALIBRATION DONE RP ERROR %f", rp_error);
// clang-format off
std::cout << "calibration rp_error: " << rp_error << "\n"; std::cout << "calibration rp_error: " << rp_error << "\n";
std::cout << "calibration camera_translation:\n" std::cout << "camera_rotation:\n" << raw.camera_rotation_mat << "\n";
<< camera_translation << "\n"; std::cout << "camera_translation:\n" << raw.camera_translation_mat << "\n";
std::cout << "camera_essential:\n" << raw.camera_essential_mat << "\n";
std::cout << "camera_fundamental:\n" << raw.camera_fundamental_mat << "\n";
// clang-format on
t_file_save_raw_data_hack(&raw); t_file_save_raw_data_hack(&raw);
} }

54
src/xrt/auxiliary/tracking/t_calibration_opencv.h
View file

@ -24,19 +24,47 @@ extern "C" {
*/ */
struct CalibrationRawData : t_calibration_raw_data struct CalibrationRawData : t_calibration_raw_data
{ {
cv::Mat l_intrinsics = {}; public:
cv::Mat l_distortion = {}; cv::Mat camera_rotation_mat = {};
cv::Mat l_distortion_fisheye = {}; cv::Mat camera_translation_mat = {};
cv::Mat l_translation = {}; cv::Mat camera_essential_mat = {};
cv::Mat l_rotation = {}; cv::Mat camera_fundamental_mat = {};
cv::Mat l_projection = {};
cv::Mat r_intrinsics = {}; cv::Mat disparity_to_depth_mat = {};
cv::Mat r_distortion = {};
cv::Mat r_distortion_fisheye = {}; cv::Mat l_intrinsics_mat = {};
cv::Mat r_translation = {}; cv::Mat l_distortion_mat = {};
cv::Mat r_rotation = {}; cv::Mat l_distortion_fisheye_mat = {};
cv::Mat r_projection = {}; cv::Mat l_translation_mat = {};
cv::Mat disparity_to_depth = {}; cv::Mat l_rotation_mat = {};
cv::Mat l_projection_mat = {};
cv::Mat r_intrinsics_mat = {};
cv::Mat r_distortion_mat = {};
cv::Mat r_distortion_fisheye_mat = {};
cv::Mat r_translation_mat = {};
cv::Mat r_rotation_mat = {};
cv::Mat r_projection_mat = {};
public:
CalibrationRawData()
{
// clang-format off
camera_translation_mat = cv::Mat(3, 1, CV_64F, &camera_translation[0]);
camera_rotation_mat = cv::Mat(3, 3, CV_64F, &camera_rotation[0]);
camera_essential_mat = cv::Mat(3, 3, CV_64F, &camera_essential[0]);
camera_fundamental_mat = cv::Mat(3, 3, CV_64F, &camera_fundamental[0]);
// clang-format on
}
bool
isDataStorageValid()
{
return camera_rotation_mat.size() == cv::Size(3, 3) &&
camera_translation_mat.size() == cv::Size(1, 3) &&
camera_essential_mat.size() == cv::Size(3, 3) &&
camera_fundamental_mat.size() == cv::Size(3, 3);
}
}; };
struct CalibrationData : t_calibration_data struct CalibrationData : t_calibration_data

174
src/xrt/auxiliary/tracking/t_file.cpp
View file

@ -59,24 +59,26 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
CalibrationRawData &raw = *(new CalibrationRawData()); CalibrationRawData &raw = *(new CalibrationRawData());
CalibrationData &data = *(new CalibrationData()); CalibrationData &data = *(new CalibrationData());
assert(raw.isDataStorageValid());
//! @todo Load from file. //! @todo Load from file.
bool use_fisheye = false; bool use_fisheye = false;
// Read our calibration from this file // Read our calibration from this file
// clang-format off // clang-format off
read_cv_mat(calib_file, &raw.l_intrinsics, "l_intrinsics"); read_cv_mat(calib_file, &raw.l_intrinsics_mat, "l_intrinsics");
read_cv_mat(calib_file, &raw.r_intrinsics, "r_intrinsics"); read_cv_mat(calib_file, &raw.r_intrinsics_mat, "r_intrinsics");
read_cv_mat(calib_file, &raw.l_distortion, "l_distortion"); read_cv_mat(calib_file, &raw.l_distortion_mat, "l_distortion");
read_cv_mat(calib_file, &raw.r_distortion, "r_distortion"); read_cv_mat(calib_file, &raw.r_distortion_mat, "r_distortion");
read_cv_mat(calib_file, &raw.l_distortion_fisheye, "l_distortion_fisheye"); read_cv_mat(calib_file, &raw.l_distortion_fisheye_mat, "l_distortion_fisheye");
read_cv_mat(calib_file, &raw.r_distortion_fisheye, "r_distortion_fisheye"); read_cv_mat(calib_file, &raw.r_distortion_fisheye_mat, "r_distortion_fisheye");
read_cv_mat(calib_file, &raw.l_rotation, "l_rotation"); read_cv_mat(calib_file, &raw.l_rotation_mat, "l_rotation");
read_cv_mat(calib_file, &raw.r_rotation, "r_rotation"); read_cv_mat(calib_file, &raw.r_rotation_mat, "r_rotation");
read_cv_mat(calib_file, &raw.l_translation, "l_translation"); read_cv_mat(calib_file, &raw.l_translation_mat, "l_translation");
read_cv_mat(calib_file, &raw.r_translation, "r_translation"); read_cv_mat(calib_file, &raw.r_translation_mat, "r_translation");
read_cv_mat(calib_file, &raw.l_projection, "l_projection"); read_cv_mat(calib_file, &raw.l_projection_mat, "l_projection");
read_cv_mat(calib_file, &raw.r_projection, "r_projection"); read_cv_mat(calib_file, &raw.r_projection_mat, "r_projection");
read_cv_mat(calib_file, &raw.disparity_to_depth, "disparity_to_depth"); read_cv_mat(calib_file, &raw.disparity_to_depth_mat, "disparity_to_depth");
cv::Mat mat_image_size = {}; cv::Mat mat_image_size = {};
read_cv_mat(calib_file, &mat_image_size, "mat_image_size"); read_cv_mat(calib_file, &mat_image_size, "mat_image_size");
@ -93,17 +95,37 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
raw.new_image_size_pixels.h = raw.image_size_pixels.h; raw.new_image_size_pixels.h = raw.image_size_pixels.h;
} }
cv::Mat translation = {}; if (!read_cv_mat(calib_file, &raw.camera_translation_mat, "translation")) {
if (read_cv_mat(calib_file, &translation, "translation")) { fprintf(stderr, "\tRe-run calibration!\n");
raw.translation.x = translation.at<float>(0, 0); }
raw.translation.y = translation.at<float>(0, 1); if (!read_cv_mat(calib_file, &raw.camera_rotation_mat, "rotation")) {
raw.translation.z = translation.at<float>(0, 2); fprintf(stderr, "\tRe-run calibration!\n");
}
if (!read_cv_mat(calib_file, &raw.camera_essential_mat, "essential")) {
fprintf(stderr, "\tRe-run calibration!\n");
}
if (!read_cv_mat(calib_file, &raw.camera_fundamental_mat, "fundamental")) {
fprintf(stderr, "\tRe-run calibration!\n");
} }
// clang-format on // clang-format on
if (raw.camera_translation_mat.size() == cv::Size(3, 1)) {
fprintf(stderr,
"Radjusting translation, re-run calibration.\n");
raw.camera_translation[0] =
raw.camera_translation_mat.at<double>(0, 0);
raw.camera_translation[1] =
raw.camera_translation_mat.at<double>(0, 1);
raw.camera_translation[2] =
raw.camera_translation_mat.at<double>(0, 2);
raw.camera_translation_mat =
cv::Mat(3, 1, CV_64F, &raw.camera_translation[0]);
}
assert(raw.isDataStorageValid());
// No processing needed. // No processing needed.
data.disparity_to_depth = raw.disparity_to_depth; data.disparity_to_depth = raw.disparity_to_depth_mat.clone();
//! @todo Scale Our intrinsics if the frame size we request //! @todo Scale Our intrinsics if the frame size we request
// calibration for does not match what was saved // calibration for does not match what was saved
@ -112,40 +134,42 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
if (use_fisheye) { if (use_fisheye) {
cv::fisheye::initUndistortRectifyMap( cv::fisheye::initUndistortRectifyMap(
raw.l_intrinsics, // cameraMatrix raw.l_intrinsics_mat, // cameraMatrix
raw.l_distortion_fisheye, // distCoeffs raw.l_distortion_fisheye_mat, // distCoeffs
cv::noArray(), // R cv::noArray(), // R
raw.l_intrinsics, // newCameraMatrix raw.l_intrinsics_mat, // newCameraMatrix
image_size, // size image_size, // size
CV_32FC1, // m1type CV_32FC1, // m1type
data.l_undistort_map_x, // map1 data.l_undistort_map_x, // map1
data.l_undistort_map_y); // map2 data.l_undistort_map_y); // map2
cv::fisheye::initUndistortRectifyMap( cv::fisheye::initUndistortRectifyMap(
raw.r_intrinsics, // cameraMatrix raw.r_intrinsics_mat, // cameraMatrix
raw.r_distortion_fisheye, // distCoeffs raw.r_distortion_fisheye_mat, // distCoeffs
cv::noArray(), // R cv::noArray(), // R
raw.r_intrinsics, // newCameraMatrix raw.r_intrinsics_mat, // newCameraMatrix
image_size, // size image_size, // size
CV_32FC1, // m1type CV_32FC1, // m1type
data.r_undistort_map_x, // map1 data.r_undistort_map_x, // map1
data.r_undistort_map_y); // map2 data.r_undistort_map_y); // map2
} else { } else {
cv::initUndistortRectifyMap(raw.l_intrinsics, // cameraMatrix cv::initUndistortRectifyMap(
raw.l_distortion, // distCoeffs raw.l_intrinsics_mat, // cameraMatrix
cv::noArray(), // R raw.l_distortion_mat, // distCoeffs
raw.l_intrinsics, // newCameraMatrix cv::noArray(), // R
image_size, // size raw.l_intrinsics_mat, // newCameraMatrix
CV_32FC1, // m1type image_size, // size
data.l_undistort_map_x, // map1 CV_32FC1, // m1type
data.l_undistort_map_y); // map2 data.l_undistort_map_x, // map1
cv::initUndistortRectifyMap(raw.r_intrinsics, // cameraMatrix data.l_undistort_map_y); // map2
raw.r_distortion, // distCoeffs cv::initUndistortRectifyMap(
cv::noArray(), // R raw.r_intrinsics_mat, // cameraMatrix
raw.r_intrinsics, // newCameraMatrix raw.r_distortion_mat, // distCoeffs
image_size, // size cv::noArray(), // R
CV_32FC1, // m1type raw.r_intrinsics_mat, // newCameraMatrix
data.r_undistort_map_x, // map1 image_size, // size
data.r_undistort_map_y); // map2 CV_32FC1, // m1type
data.r_undistort_map_x, // map1
data.r_undistort_map_y); // map2
} }
/* /*
@ -154,18 +178,18 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
* Here cv::noArray() means zero distortion. * Here cv::noArray() means zero distortion.
*/ */
cv::initUndistortRectifyMap(raw.l_intrinsics, // cameraMatrix cv::initUndistortRectifyMap(raw.l_intrinsics_mat, // cameraMatrix
cv::noArray(), // distCoeffs cv::noArray(), // distCoeffs
raw.l_rotation, // R raw.l_rotation_mat, // R
raw.l_projection, // newCameraMatrix raw.l_projection_mat, // newCameraMatrix
image_size, // size image_size, // size
CV_32FC1, // m1type CV_32FC1, // m1type
data.l_rectify_map_x, // map1 data.l_rectify_map_x, // map1
data.l_rectify_map_y); // map2 data.l_rectify_map_y); // map2
cv::initUndistortRectifyMap(raw.r_intrinsics, // cameraMatrix cv::initUndistortRectifyMap(raw.r_intrinsics_mat, // cameraMatrix
cv::noArray(), // distCoeffs cv::noArray(), // distCoeffs
raw.r_rotation, // R raw.r_rotation_mat, // R
raw.r_projection, // newCameraMatrix raw.r_projection_mat, // newCameraMatrix
image_size, // size image_size, // size
CV_32FC1, // m1type CV_32FC1, // m1type
data.r_rectify_map_x, // map1 data.r_rectify_map_x, // map1
@ -189,19 +213,19 @@ t_file_save_raw_data(FILE *calib_file, struct t_calibration_raw_data *raw_data)
{ {
CalibrationRawData &raw = *(CalibrationRawData *)raw_data; CalibrationRawData &raw = *(CalibrationRawData *)raw_data;
write_cv_mat(calib_file, &raw.l_intrinsics); write_cv_mat(calib_file, &raw.l_intrinsics_mat);
write_cv_mat(calib_file, &raw.r_intrinsics); write_cv_mat(calib_file, &raw.r_intrinsics_mat);
write_cv_mat(calib_file, &raw.l_distortion); write_cv_mat(calib_file, &raw.l_distortion_mat);
write_cv_mat(calib_file, &raw.r_distortion); write_cv_mat(calib_file, &raw.r_distortion_mat);
write_cv_mat(calib_file, &raw.l_distortion_fisheye); write_cv_mat(calib_file, &raw.l_distortion_fisheye_mat);
write_cv_mat(calib_file, &raw.r_distortion_fisheye); write_cv_mat(calib_file, &raw.r_distortion_fisheye_mat);
write_cv_mat(calib_file, &raw.l_rotation); write_cv_mat(calib_file, &raw.l_rotation_mat);
write_cv_mat(calib_file, &raw.r_rotation); write_cv_mat(calib_file, &raw.r_rotation_mat);
write_cv_mat(calib_file, &raw.l_translation); write_cv_mat(calib_file, &raw.l_translation_mat);
write_cv_mat(calib_file, &raw.r_translation); write_cv_mat(calib_file, &raw.r_translation_mat);
write_cv_mat(calib_file, &raw.l_projection); write_cv_mat(calib_file, &raw.l_projection_mat);
write_cv_mat(calib_file, &raw.r_projection); write_cv_mat(calib_file, &raw.r_projection_mat);
write_cv_mat(calib_file, &raw.disparity_to_depth); write_cv_mat(calib_file, &raw.disparity_to_depth_mat);
cv::Mat mat_image_size; cv::Mat mat_image_size;
mat_image_size.create(1, 2, CV_32F); mat_image_size.create(1, 2, CV_32F);
@ -215,12 +239,10 @@ t_file_save_raw_data(FILE *calib_file, struct t_calibration_raw_data *raw_data)
mat_new_image_size.at<float>(0, 1) = raw.new_image_size_pixels.h; mat_new_image_size.at<float>(0, 1) = raw.new_image_size_pixels.h;
write_cv_mat(calib_file, &mat_new_image_size); write_cv_mat(calib_file, &mat_new_image_size);
cv::Mat mat_translation; write_cv_mat(calib_file, &raw.camera_translation_mat);
mat_translation.create(1, 3, CV_32F); write_cv_mat(calib_file, &raw.camera_rotation_mat);
mat_translation.at<float>(0, 0) = raw.translation.x; write_cv_mat(calib_file, &raw.camera_essential_mat);
mat_translation.at<float>(0, 1) = raw.translation.y; write_cv_mat(calib_file, &raw.camera_fundamental_mat);
mat_translation.at<float>(0, 2) = raw.translation.z;
write_cv_mat(calib_file, &mat_translation);
return true; return true;
} }

13
src/xrt/auxiliary/tracking/t_tracking.h
View file

@ -236,8 +236,17 @@ struct t_calibration_raw_data
struct xrt_size image_size_pixels; struct xrt_size image_size_pixels;
struct xrt_size new_image_size_pixels; struct xrt_size new_image_size_pixels;
//! Translation between stereo cameras. //! Translation between thw two cameras, in the stereo pair.
struct xrt_vec3 translation; double camera_translation[3];
//! Rotation matrix between the two cameras, in the stereo pair.
double camera_rotation[3][3];
//! Essential matrix.
double camera_essential[3][3];
//! Fundamental matrix.
double camera_fundamental[3][3];
}; };
/*! /*!