From 85f30f48c6b4992914662a6df734168e20197715 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Mon, 16 May 2022 16:02:40 +0100 Subject: [PATCH] mercury: Correctly handle lower resolution input images And better calibration debug prints --- src/xrt/tracking/hand/mercury/hg_sync.cpp | 66 ++++++++++++++++++++--- src/xrt/tracking/hand/mercury/hg_sync.hpp | 7 ++- 2 files changed, 64 insertions(+), 9 deletions(-) diff --git a/src/xrt/tracking/hand/mercury/hg_sync.cpp b/src/xrt/tracking/hand/mercury/hg_sync.cpp index d2e01f3ae..a88671339 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.cpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.cpp @@ -11,6 +11,7 @@ #include "hg_sync.hpp" #include "hg_model.hpp" +#include namespace xrt::tracking::hand::mercury { // Flags to tell state tracker that these are indeed valid joints @@ -49,12 +50,19 @@ getCalibration(struct HandTracking *htd, t_stereo_camera_calibration *calibratio xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0), (float)wrap.camera_translation_mat(2, 0)}; htd->baseline = m_vec3_len(trans); + HT_DEBUG(htd, "I think the baseline is %f meters!", htd->baseline); // Note, this assumes camera 0 is the left camera and camera 1 is the right camera. // If you find one with the opposite arrangement, you'll need to invert htd->baseline, and look at // hgJointDisparityMath htd->use_fisheye = wrap.view[0].use_fisheye; + if (htd->use_fisheye) { + HT_DEBUG(htd, "I think the cameras are fisheye!"); + } else { + HT_DEBUG(htd, "I think the cameras are not fisheye!"); + } + cv::Matx34d P1; cv::Matx34d P2; @@ -105,10 +113,24 @@ getCalibration(struct HandTracking *htd, t_stereo_camera_calibration *calibratio } else { htd->views[i].distortion = wrap.view[i].distortion_mat; } + + if (htd->log_level <= U_LOGGING_DEBUG) { + HT_DEBUG(htd, "R%d ->", i); + std::cout << htd->views[i].rotate_camera_to_stereo_camera << std::endl; + + HT_DEBUG(htd, "K%d ->", i); + std::cout << htd->views[i].cameraMatrix << std::endl; + + HT_DEBUG(htd, "D%d ->", i); + std::cout << htd->views[i].distortion << std::endl; + } } - htd->one_view_size_px.w = wrap.view[0].image_size_pixels.w; - htd->one_view_size_px.h = wrap.view[0].image_size_pixels.h; + htd->calibration_one_view_size_px.w = wrap.view[0].image_size_pixels.w; + htd->calibration_one_view_size_px.h = wrap.view[0].image_size_pixels.h; + + htd->last_frame_one_view_size_px = htd->calibration_one_view_size_px; + htd->multiply_px_coord_for_undistort = 1.0f; cv::Matx33d rotate_stereo_camera_to_left_camera = htd->views[0].rotate_camera_to_stereo_camera.inv(); @@ -235,6 +257,29 @@ applyJointWidths(struct HandTracking *htd, struct xrt_hand_joint_set *set) .040f * .5f; // Measured my wrist thickness with calipers } +static bool handle_changed_image_size(HandTracking *htd, xrt_size &new_one_view_size) +{ + int gcd_calib = std::gcd(htd->calibration_one_view_size_px.h, htd->calibration_one_view_size_px.w); + int gcd_new = std::gcd(new_one_view_size.h, new_one_view_size.w); + + int lcm_h_calib = htd->calibration_one_view_size_px.h/gcd_calib; + int lcm_w_calib = htd->calibration_one_view_size_px.w/gcd_calib; + + int lcm_h_new = new_one_view_size.h/gcd_new; + int lcm_w_new = new_one_view_size.w/gcd_new; + + bool good = (lcm_h_calib == lcm_h_new) && (lcm_w_calib == lcm_w_new); + + if (!good) { + HT_WARN(htd, "Can't process this frame, wrong aspect ratio."); + return false; + } + + htd->multiply_px_coord_for_undistort = (float)htd->calibration_one_view_size_px.h/(float)new_one_view_size.h; + htd->last_frame_one_view_size_px = new_one_view_size; + return true; +} + /* * * Member functions. @@ -296,12 +341,19 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, const int full_height = left_frame->height; const int full_width = left_frame->width*2; - const int view_width = htd->one_view_size_px.w; - const int view_height = htd->one_view_size_px.h; + if ((left_frame->width != (uint32_t)htd->last_frame_one_view_size_px.w) || (left_frame->height != (uint32_t)htd->last_frame_one_view_size_px.h)) + { + xrt_size new_one_view_size; + new_one_view_size.h = left_frame->height; + new_one_view_size.w = left_frame->width; + // Could be an assert, should never happen. + if (!handle_changed_image_size(htd, new_one_view_size)) { + return; + } + } - assert(full_height == view_height); - - htd->multiply_px_coord_for_undistort = 1.0f; + const int view_width = htd->last_frame_one_view_size_px.w; + const int view_height = htd->last_frame_one_view_size_px.h; const cv::Size full_size = cv::Size(full_width, full_height); const cv::Size view_size = cv::Size(view_width, view_height); diff --git a/src/xrt/tracking/hand/mercury/hg_sync.hpp b/src/xrt/tracking/hand/mercury/hg_sync.hpp index 6819ad2c9..593ea0d8c 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.hpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.hpp @@ -169,7 +169,7 @@ public: struct u_sink_debug debug_sink = {}; - int multiply_px_coord_for_undistort; + float multiply_px_coord_for_undistort; bool use_fisheye; @@ -177,7 +177,10 @@ public: struct t_stereo_camera_calibration *calib; - struct xrt_size one_view_size_px = {}; + struct xrt_size calibration_one_view_size_px = {}; + + // So that we can calibrate cameras at 1280x800 but ship images over USB at 640x400 + struct xrt_size last_frame_one_view_size_px = {}; #ifdef USE_NCNN ncnn_net_t net;