diff --git a/src/xrt/auxiliary/tracking/t_calibration_opencv.h b/src/xrt/auxiliary/tracking/t_calibration_opencv.h index 5c8dc859a..2ea186b5e 100644 --- a/src/xrt/auxiliary/tracking/t_calibration_opencv.h +++ b/src/xrt/auxiliary/tracking/t_calibration_opencv.h @@ -100,17 +100,11 @@ struct CalibrationData : t_calibration_data cv::Mat disparity_to_depth = {}; }; -extern "C" bool -t_file_load_stereo_calibration_v1_hack(struct t_calibration_data **out_data); - 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, - uint32_t frame_w, - uint32_t frame_h, - bool use_fisheye, +XRT_MAYBE_UNUSED static void +calibration_get_stereo(t_calibration_data *data_c, cv::Mat *l_undistort_map_x, cv::Mat *l_undistort_map_y, cv::Mat *l_rectify_map_x, @@ -121,13 +115,6 @@ calibration_get_stereo(const char *configuration_filename, cv::Mat *r_rectify_map_y, cv::Mat *disparity_to_depth) { - t_calibration_data *data_c; - bool ok = t_file_load_stereo_calibration_v1_hack(&data_c); - - if (!ok) { - return false; - } - CalibrationData *data = (CalibrationData *)data_c; *l_undistort_map_x = data->l_undistort_map_x; @@ -139,10 +126,6 @@ calibration_get_stereo(const char *configuration_filename, *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; - - t_calibration_data_free(&data_c); - - return true; } //! @todo Move this as it is a generic helper diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp index 9c070481c..2d18f025c 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp @@ -253,28 +253,7 @@ process(TrackerPSMV &t, struct xrt_frame *xf) } if (!t.calibrated) { - bool ok = calibration_get_stereo( - "PS4_EYE", // name - xf->width, // width - xf->height, // height - false, // use_fisheye - &t.view[0].undistort_map_x, // l_undistort_map_x - &t.view[0].undistort_map_y, // l_undistort_map_y - &t.view[0].rectify_map_x, // l_rectify_map_x - &t.view[0].rectify_map_y, // l_rectify_map_y - &t.view[1].undistort_map_x, // r_undistort_map_x - &t.view[1].undistort_map_y, // r_undistort_map_y - &t.view[1].rectify_map_x, // r_rectify_map_x - &t.view[1].rectify_map_y, // r_rectify_map_y - &t.disparity_to_depth); // disparity_to_depth - - if (ok) { - printf("loaded calibration for camera!\n"); - t.calibrated = true; - } else { - xrt_frame_reference(&xf, NULL); - return; - } + return; } // Create the debug frame if needed. @@ -574,6 +553,7 @@ t_psmv_start(struct xrt_tracked_psmv *xtmv) extern "C" int t_psmv_create(struct xrt_frame_context *xfctx, struct xrt_colour_rgb_f32 *rgb, + struct t_calibration_data *data, struct xrt_tracked_psmv **out_xtmv, struct xrt_frame_sink **out_sink) { @@ -620,6 +600,18 @@ t_psmv_create(struct xrt_frame_context *xfctx, break; } + calibration_get_stereo(data, + &t.view[0].undistort_map_x, // l_undistort_map_x + &t.view[0].undistort_map_y, // l_undistort_map_y + &t.view[0].rectify_map_x, // l_rectify_map_x + &t.view[0].rectify_map_y, // l_rectify_map_y + &t.view[1].undistort_map_x, // r_undistort_map_x + &t.view[1].undistort_map_y, // r_undistort_map_y + &t.view[1].rectify_map_x, // r_rectify_map_x + &t.view[1].rectify_map_y, // r_rectify_map_y + &t.disparity_to_depth); // disparity_to_depth + t.calibrated = true; + // clang-format off cv::SimpleBlobDetector::Params blob_params; blob_params.filterByArea = false; diff --git a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp index 81d4cb8b7..44c3ded34 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp @@ -262,6 +262,7 @@ t_psvr_start(struct xrt_tracked_psvr *xtvr) extern "C" int t_psvr_create(struct xrt_frame_context *xfctx, + struct t_calibration_data *data, struct xrt_tracked_psvr **out_xtvr, struct xrt_frame_sink **out_sink) { diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index a00322c5a..f10ce5015 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -69,6 +69,7 @@ extern "C" { struct xrt_tracked_psmv; struct xrt_tracked_psvr; +struct t_calibration_data; /* @@ -203,6 +204,7 @@ t_psmv_start(struct xrt_tracked_psmv *xtmv); int t_psmv_create(struct xrt_frame_context *xfctx, struct xrt_colour_rgb_f32 *rgb, + struct t_calibration_data *data, struct xrt_tracked_psmv **out_xtmv, struct xrt_frame_sink **out_sink); @@ -211,6 +213,7 @@ t_psvr_start(struct xrt_tracked_psvr *xtvr); int t_psvr_create(struct xrt_frame_context *xfctx, + struct t_calibration_data *data, struct xrt_tracked_psvr **out_xtvr, struct xrt_frame_sink **out_sink); @@ -291,6 +294,12 @@ t_file_load_stereo_calibration_v1(FILE *calib_file, struct t_calibration_data **out_data, struct t_calibration_raw_data **out_raw_data); +/*! + * Load a stereo calibration struct from a hardcoded place. + */ +bool +t_file_load_stereo_calibration_v1_hack(struct t_calibration_data **out_data); + struct t_calibration_params { int checker_cols_num; diff --git a/src/xrt/state_trackers/prober/p_tracking.c b/src/xrt/state_trackers/prober/p_tracking.c index 759ab6e27..ac7bcbe11 100644 --- a/src/xrt/state_trackers/prober/p_tracking.c +++ b/src/xrt/state_trackers/prober/p_tracking.c @@ -46,6 +46,9 @@ struct p_factory struct xrt_frame_context xfctx; #ifdef XRT_HAVE_OPENCV + //! Data to be given to the trackers. + struct t_calibration_data *data; + //! Keep track of how many psmv trackers that has been handed out. size_t num_xtmv; @@ -114,14 +117,21 @@ p_factory_ensure_frameserver(struct p_factory *fact) return; } + // Now load the calibration data. + if (!t_file_load_stereo_calibration_v1_hack(&fact->data)) { + return; + } + struct xrt_frame_sink *xsink = NULL; struct xrt_frame_sink *xsinks[4] = {0}; struct xrt_colour_rgb_f32 rgb[2] = {{1.f, 0.f, 0.f}, {1.f, 0.f, 1.f}}; // We create the two psmv trackers up front, but don't start them. - t_psmv_create(&fact->xfctx, &rgb[0], &fact->xtmv[0], &xsinks[0]); - t_psmv_create(&fact->xfctx, &rgb[1], &fact->xtmv[1], &xsinks[1]); - t_psvr_create(&fact->xfctx, &fact->xtvr, &xsinks[2]); + // clang-format off + t_psmv_create(&fact->xfctx, &rgb[0], fact->data, &fact->xtmv[0], &xsinks[0]); + t_psmv_create(&fact->xfctx, &rgb[1], fact->data, &fact->xtmv[1], &xsinks[1]); + t_psvr_create(&fact->xfctx, fact->data, &fact->xtvr, &xsinks[2]); + // clang-format on // Setup origin to the common one. fact->xtvr->origin = &fact->origin; @@ -256,11 +266,21 @@ p_tracking_teardown(struct prober *p) #ifdef XRT_HAVE_OPENCV fact->xtmv[0] = NULL; fact->xtmv[1] = NULL; + fact->xtvr = NULL; #endif // Take down the node graph. xrt_frame_context_destroy_nodes(&fact->xfctx); +#ifdef XRT_HAVE_OPENCV + /* + * Needs to be done after the trackers has been destroyed. + * + * Does null checking and sets to null. + */ + t_calibration_data_free(&fact->data); +#endif + free(fact); p->base.tracking = NULL; }