mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-04 06:06:17 +00:00
t/file: Save more calibration data
This commit is contained in:
Jakob Bornecrantz
parent
9ec1205f37
commit
43c366ad41
|
|
@ -279,20 +279,14 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
cv::Size image_size(cols, rows);
|
||||
cv::Size new_image_size(cols, rows);
|
||||
|
||||
// we don't serialise these
|
||||
cv::Mat camera_rotation;
|
||||
cv::Mat camera_translation;
|
||||
cv::Mat camera_essential;
|
||||
cv::Mat camera_fundamental;
|
||||
CalibrationRawData raw = {};
|
||||
assert(raw.isDataStorageValid());
|
||||
|
||||
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));
|
||||
|
||||
// TODO: handle both fisheye and normal cameras -right
|
||||
// now I only have the normal, for the PS4 camera
|
||||
#if 0
|
||||
|
|
@ -306,49 +300,56 @@ process_stereo_samples(class Calibration &c, int cols, int rows)
|
|||
|
||||
// non-fisheye version
|
||||
float rp_error =
|
||||
cv::stereoCalibrate(c.state.chessboards_model, // objectPoints
|
||||
c.state.view[0].measured, // inagePoints1
|
||||
c.state.view[1].measured, // imagePoints2,
|
||||
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
|
||||
camera_essential, // E
|
||||
camera_fundamental, // F
|
||||
0); // flags
|
||||
cv::stereoCalibrate(c.state.chessboards_model, // objectPoints
|
||||
c.state.view[0].measured, // inagePoints1
|
||||
c.state.view[1].measured, // imagePoints2,
|
||||
raw.l_intrinsics_mat, // cameraMatrix1
|
||||
raw.l_distortion_mat, // distCoeffs1
|
||||
raw.r_intrinsics_mat, // cameraMatrix2
|
||||
raw.r_distortion_mat, // distCoeffs2
|
||||
image_size, // imageSize
|
||||
raw.camera_rotation_mat, // R
|
||||
raw.camera_translation_mat, // T
|
||||
raw.camera_essential_mat, // E
|
||||
raw.camera_fundamental_mat, // F
|
||||
0); // flags
|
||||
|
||||
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);
|
||||
assert(raw.camera_rotation_mat.size() == cv::Size(3, 3));
|
||||
assert(raw.camera_translation_mat.size() == cv::Size(1, 3));
|
||||
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.
|
||||
cv::stereoRectify(raw.l_intrinsics, // cameraMatrix1
|
||||
zero_distortion, // distCoeffs1
|
||||
raw.r_intrinsics, // cameraMatrix2
|
||||
zero_distortion, // distCoeffs2
|
||||
image_size, // imageSize
|
||||
camera_rotation, // R
|
||||
camera_translation, // T
|
||||
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
|
||||
new_image_size, // newImageSize
|
||||
NULL, // validPixROI1
|
||||
NULL); // validPixROI2
|
||||
cv::stereoRectify(raw.l_intrinsics_mat, // cameraMatrix1
|
||||
cv::noArray(), // distCoeffs1
|
||||
raw.r_intrinsics_mat, // cameraMatrix2
|
||||
cv::noArray(), // distCoeffs2
|
||||
image_size, // imageSize
|
||||
raw.camera_rotation_mat, // R
|
||||
raw.camera_translation_mat, // T
|
||||
raw.l_rotation_mat, // R1
|
||||
raw.r_rotation_mat, // R2
|
||||
raw.l_projection_mat, // P1
|
||||
raw.r_projection_mat, // P2
|
||||
raw.disparity_to_depth_mat, // Q
|
||||
cv::CALIB_ZERO_DISPARITY, // flags
|
||||
-1, // alpha
|
||||
new_image_size, // newImageSize
|
||||
NULL, // validPixROI1
|
||||
NULL); // validPixROI2
|
||||
|
||||
// Validate that nothing has been re-allocated.
|
||||
assert(raw.isDataStorageValid());
|
||||
|
||||
P("CALIBRATION DONE RP ERROR %f", rp_error);
|
||||
|
||||
// clang-format off
|
||||
std::cout << "calibration rp_error: " << rp_error << "\n";
|
||||
std::cout << "calibration camera_translation:\n"
|
||||
<< camera_translation << "\n";
|
||||
|
||||
std::cout << "camera_rotation:\n" << raw.camera_rotation_mat << "\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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,19 +24,47 @@ extern "C" {
|
|||
*/
|
||||
struct CalibrationRawData : t_calibration_raw_data
|
||||
{
|
||||
cv::Mat l_intrinsics = {};
|
||||
cv::Mat l_distortion = {};
|
||||
cv::Mat l_distortion_fisheye = {};
|
||||
cv::Mat l_translation = {};
|
||||
cv::Mat l_rotation = {};
|
||||
cv::Mat l_projection = {};
|
||||
cv::Mat r_intrinsics = {};
|
||||
cv::Mat r_distortion = {};
|
||||
cv::Mat r_distortion_fisheye = {};
|
||||
cv::Mat r_translation = {};
|
||||
cv::Mat r_rotation = {};
|
||||
cv::Mat r_projection = {};
|
||||
cv::Mat disparity_to_depth = {};
|
||||
public:
|
||||
cv::Mat camera_rotation_mat = {};
|
||||
cv::Mat camera_translation_mat = {};
|
||||
cv::Mat camera_essential_mat = {};
|
||||
cv::Mat camera_fundamental_mat = {};
|
||||
|
||||
cv::Mat disparity_to_depth_mat = {};
|
||||
|
||||
cv::Mat l_intrinsics_mat = {};
|
||||
cv::Mat l_distortion_mat = {};
|
||||
cv::Mat l_distortion_fisheye_mat = {};
|
||||
cv::Mat l_translation_mat = {};
|
||||
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
|
||||
|
|
|
|||
|
|
@ -59,24 +59,26 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
|
|||
CalibrationRawData &raw = *(new CalibrationRawData());
|
||||
CalibrationData &data = *(new CalibrationData());
|
||||
|
||||
assert(raw.isDataStorageValid());
|
||||
|
||||
//! @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");
|
||||
read_cv_mat(calib_file, &raw.l_intrinsics_mat, "l_intrinsics");
|
||||
read_cv_mat(calib_file, &raw.r_intrinsics_mat, "r_intrinsics");
|
||||
read_cv_mat(calib_file, &raw.l_distortion_mat, "l_distortion");
|
||||
read_cv_mat(calib_file, &raw.r_distortion_mat, "r_distortion");
|
||||
read_cv_mat(calib_file, &raw.l_distortion_fisheye_mat, "l_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &raw.r_distortion_fisheye_mat, "r_distortion_fisheye");
|
||||
read_cv_mat(calib_file, &raw.l_rotation_mat, "l_rotation");
|
||||
read_cv_mat(calib_file, &raw.r_rotation_mat, "r_rotation");
|
||||
read_cv_mat(calib_file, &raw.l_translation_mat, "l_translation");
|
||||
read_cv_mat(calib_file, &raw.r_translation_mat, "r_translation");
|
||||
read_cv_mat(calib_file, &raw.l_projection_mat, "l_projection");
|
||||
read_cv_mat(calib_file, &raw.r_projection_mat, "r_projection");
|
||||
read_cv_mat(calib_file, &raw.disparity_to_depth_mat, "disparity_to_depth");
|
||||
cv::Mat 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;
|
||||
}
|
||||
|
||||
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);
|
||||
if (!read_cv_mat(calib_file, &raw.camera_translation_mat, "translation")) {
|
||||
fprintf(stderr, "\tRe-run calibration!\n");
|
||||
}
|
||||
if (!read_cv_mat(calib_file, &raw.camera_rotation_mat, "rotation")) {
|
||||
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
|
||||
|
||||
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.
|
||||
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
|
||||
// calibration for does not match what was saved
|
||||
|
|
@ -112,40 +134,42 @@ t_file_load_stereo_calibration_v1(FILE *calib_file,
|
|||
|
||||
if (use_fisheye) {
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
raw.l_intrinsics, // cameraMatrix
|
||||
raw.l_distortion_fisheye, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.l_intrinsics, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.l_undistort_map_x, // map1
|
||||
data.l_undistort_map_y); // map2
|
||||
raw.l_intrinsics_mat, // cameraMatrix
|
||||
raw.l_distortion_fisheye_mat, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.l_intrinsics_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.l_undistort_map_x, // map1
|
||||
data.l_undistort_map_y); // map2
|
||||
cv::fisheye::initUndistortRectifyMap(
|
||||
raw.r_intrinsics, // cameraMatrix
|
||||
raw.r_distortion_fisheye, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.r_intrinsics, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.r_undistort_map_x, // map1
|
||||
data.r_undistort_map_y); // map2
|
||||
raw.r_intrinsics_mat, // cameraMatrix
|
||||
raw.r_distortion_fisheye_mat, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.r_intrinsics_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.r_undistort_map_x, // map1
|
||||
data.r_undistort_map_y); // map2
|
||||
} else {
|
||||
cv::initUndistortRectifyMap(raw.l_intrinsics, // cameraMatrix
|
||||
raw.l_distortion, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.l_intrinsics, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.l_undistort_map_x, // map1
|
||||
data.l_undistort_map_y); // map2
|
||||
cv::initUndistortRectifyMap(raw.r_intrinsics, // cameraMatrix
|
||||
raw.r_distortion, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.r_intrinsics, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.r_undistort_map_x, // map1
|
||||
data.r_undistort_map_y); // map2
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.l_intrinsics_mat, // cameraMatrix
|
||||
raw.l_distortion_mat, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.l_intrinsics_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.l_undistort_map_x, // map1
|
||||
data.l_undistort_map_y); // map2
|
||||
cv::initUndistortRectifyMap(
|
||||
raw.r_intrinsics_mat, // cameraMatrix
|
||||
raw.r_distortion_mat, // distCoeffs
|
||||
cv::noArray(), // R
|
||||
raw.r_intrinsics_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
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.
|
||||
*/
|
||||
|
||||
cv::initUndistortRectifyMap(raw.l_intrinsics, // cameraMatrix
|
||||
cv::initUndistortRectifyMap(raw.l_intrinsics_mat, // cameraMatrix
|
||||
cv::noArray(), // distCoeffs
|
||||
raw.l_rotation, // R
|
||||
raw.l_projection, // newCameraMatrix
|
||||
raw.l_rotation_mat, // R
|
||||
raw.l_projection_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
data.l_rectify_map_x, // map1
|
||||
data.l_rectify_map_y); // map2
|
||||
cv::initUndistortRectifyMap(raw.r_intrinsics, // cameraMatrix
|
||||
cv::initUndistortRectifyMap(raw.r_intrinsics_mat, // cameraMatrix
|
||||
cv::noArray(), // distCoeffs
|
||||
raw.r_rotation, // R
|
||||
raw.r_projection, // newCameraMatrix
|
||||
raw.r_rotation_mat, // R
|
||||
raw.r_projection_mat, // newCameraMatrix
|
||||
image_size, // size
|
||||
CV_32FC1, // m1type
|
||||
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;
|
||||
|
||||
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);
|
||||
write_cv_mat(calib_file, &raw.l_intrinsics_mat);
|
||||
write_cv_mat(calib_file, &raw.r_intrinsics_mat);
|
||||
write_cv_mat(calib_file, &raw.l_distortion_mat);
|
||||
write_cv_mat(calib_file, &raw.r_distortion_mat);
|
||||
write_cv_mat(calib_file, &raw.l_distortion_fisheye_mat);
|
||||
write_cv_mat(calib_file, &raw.r_distortion_fisheye_mat);
|
||||
write_cv_mat(calib_file, &raw.l_rotation_mat);
|
||||
write_cv_mat(calib_file, &raw.r_rotation_mat);
|
||||
write_cv_mat(calib_file, &raw.l_translation_mat);
|
||||
write_cv_mat(calib_file, &raw.r_translation_mat);
|
||||
write_cv_mat(calib_file, &raw.l_projection_mat);
|
||||
write_cv_mat(calib_file, &raw.r_projection_mat);
|
||||
write_cv_mat(calib_file, &raw.disparity_to_depth_mat);
|
||||
|
||||
cv::Mat mat_image_size;
|
||||
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;
|
||||
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);
|
||||
write_cv_mat(calib_file, &raw.camera_translation_mat);
|
||||
write_cv_mat(calib_file, &raw.camera_rotation_mat);
|
||||
write_cv_mat(calib_file, &raw.camera_essential_mat);
|
||||
write_cv_mat(calib_file, &raw.camera_fundamental_mat);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -236,8 +236,17 @@ 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;
|
||||
//! Translation between thw two cameras, in the stereo pair.
|
||||
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];
|
||||
};
|
||||
|
||||
/*!
|
||||
|
|
|
|||
Loading…
Reference in a new issue