diff --git a/src/xrt/auxiliary/tracking/t_calibration_opencv.h b/src/xrt/auxiliary/tracking/t_calibration_opencv.h index d51fdd530..4cd59bd8f 100644 --- a/src/xrt/auxiliary/tracking/t_calibration_opencv.h +++ b/src/xrt/auxiliary/tracking/t_calibration_opencv.h @@ -32,7 +32,7 @@ t_file_save_raw_data_hack(struct t_settings_stereo_raw *raw_data); struct CalibrationData : t_settings_stereo { public: - cv::Mat disparity_to_depth = {}; + cv::Mat disparity_to_depth_mat = {}; cv::Mat l_undistort_map_x = {}; cv::Mat l_undistort_map_y = {}; @@ -43,6 +43,21 @@ public: cv::Mat r_undistort_map_y = {}; cv::Mat r_rectify_map_x = {}; cv::Mat r_rectify_map_y = {}; + + +public: + CalibrationData() + { + // clang-format off + disparity_to_depth_mat = cv::Mat(4, 4, CV_64F, &disparity_to_depth[0][0]); + // clang-format on + } + + bool + isDataStorageValid() + { + return disparity_to_depth_mat.size() == cv::Size(4, 4); + } }; /*! @@ -80,6 +95,8 @@ public: camera_essential_mat = cv::Mat(3, 3, CV_64F, &camera_essential[0][0]); camera_fundamental_mat = cv::Mat(3, 3, CV_64F, &camera_fundamental[0][0]); + disparity_to_depth_mat = cv::Mat(4, 4, CV_64F, &disparity_to_depth[0][0]); + l_intrinsics_mat = cv::Mat(3, 3, CV_64F, &l_intrinsics[0][0]); l_distortion_mat = cv::Mat(1, 5, CV_64F, &l_distortion[0]); l_distortion_fisheye_mat = cv::Mat(1, 4, CV_64F, &l_distortion_fisheye[0]); @@ -101,6 +118,7 @@ public: camera_translation_mat.size() == cv::Size(1, 3) && camera_essential_mat.size() == cv::Size(3, 3) && camera_fundamental_mat.size() == cv::Size(3, 3) && + disparity_to_depth_mat.size() == cv::Size(4, 4) && l_intrinsics_mat.size() == cv::Size(3, 3) && l_distortion_mat.size() == cv::Size(5, 1) && l_distortion_fisheye_mat.size() == cv::Size(4, 1) && @@ -136,5 +154,5 @@ calibration_get_stereo(t_settings_stereo *data_c, *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; + *disparity_to_depth = data->disparity_to_depth_mat; } diff --git a/src/xrt/auxiliary/tracking/t_file.cpp b/src/xrt/auxiliary/tracking/t_file.cpp index 06c157b97..36720be54 100644 --- a/src/xrt/auxiliary/tracking/t_file.cpp +++ b/src/xrt/auxiliary/tracking/t_file.cpp @@ -140,11 +140,12 @@ t_settings_stereo_load_v1(FILE *calib_file, } assert(raw.isDataStorageValid()); + assert(data.isDataStorageValid()); // No processing needed. data.image_size_pixels = raw.image_size_pixels; data.new_image_size_pixels = raw.new_image_size_pixels; - data.disparity_to_depth = raw.disparity_to_depth_mat.clone(); + raw.disparity_to_depth_mat.copyTo(data.disparity_to_depth_mat); //! @todo Scale Our intrinsics if the frame size we request // calibration for does not match what was saved diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 602456794..e255f82cd 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -84,6 +84,9 @@ struct t_settings_stereo { struct xrt_size image_size_pixels; struct xrt_size new_image_size_pixels; + + //! Disparity and position to camera world coordinates. + double disparity_to_depth[4][4]; }; /*! @@ -103,6 +106,9 @@ struct t_settings_stereo_raw //! Fundamental matrix. double camera_fundamental[3][3]; + //! Disparity and position to camera world coordinates. + double disparity_to_depth[4][4]; + //! Left camera intrinsics double l_intrinsics[3][3]; //! Left camera distortion