diff --git a/src/xrt/auxiliary/tracking/t_calibration.cpp b/src/xrt/auxiliary/tracking/t_calibration.cpp index d3d1f8148..c89f5360a 100644 --- a/src/xrt/auxiliary/tracking/t_calibration.cpp +++ b/src/xrt/auxiliary/tracking/t_calibration.cpp @@ -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(0, 0); - raw.translation.y = camera_translation.at(0, 1); - raw.translation.z = camera_translation.at(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); } diff --git a/src/xrt/auxiliary/tracking/t_calibration_opencv.h b/src/xrt/auxiliary/tracking/t_calibration_opencv.h index 13cae7a9a..7612c8b70 100644 --- a/src/xrt/auxiliary/tracking/t_calibration_opencv.h +++ b/src/xrt/auxiliary/tracking/t_calibration_opencv.h @@ -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 diff --git a/src/xrt/auxiliary/tracking/t_file.cpp b/src/xrt/auxiliary/tracking/t_file.cpp index aeddfb6ed..73459ad6b 100644 --- a/src/xrt/auxiliary/tracking/t_file.cpp +++ b/src/xrt/auxiliary/tracking/t_file.cpp @@ -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(0, 0); - raw.translation.y = translation.at(0, 1); - raw.translation.z = translation.at(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(0, 0); + raw.camera_translation[1] = + raw.camera_translation_mat.at(0, 1); + raw.camera_translation[2] = + raw.camera_translation_mat.at(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(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(0, 0) = raw.translation.x; - mat_translation.at(0, 1) = raw.translation.y; - mat_translation.at(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; } diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 62c632b80..f44ce6a7e 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -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]; }; /*!