diff --git a/src/xrt/tracking/hand/mercury/hg_model.cpp b/src/xrt/tracking/hand/mercury/hg_model.cpp index 050f2d3e2..9ed8868b8 100644 --- a/src/xrt/tracking/hand/mercury/hg_model.cpp +++ b/src/xrt/tracking/hand/mercury/hg_model.cpp @@ -325,7 +325,7 @@ run_hand_detection(void *ptr) size_t plane_size = 80 * 60; for (int hand_idx = 0; hand_idx < 2; hand_idx++) { - const float *this_side_data = out_data + hand_idx * plane_size * 2; + float *this_side_data = out_data + hand_idx * plane_size * 2; int max_idx = argmax(this_side_data, 4800); hand_bounding_box *output = info->outputs[hand_idx]; @@ -364,6 +364,31 @@ run_hand_detection(void *ptr) PINK, 1); } } + + if (hgt->debug_scribble) { + // note: this will multiply the model outputs by 255, don't do anything with them after this. + int top_of_rect_y = 8; // 8 + 128 + 8 + 128 + 8; + int left_of_rect_x = 8 + ((128 + 8) * 4); + int start_y = top_of_rect_y + ((240 + 8) * view->view); + int start_x = left_of_rect_x + 8 + 320 + 8; + cv::Rect p = cv::Rect(left_of_rect_x, start_y, 320, 240); + + _240x320_uint8.copyTo(hgt->visualizers.mat(p)); + + { + cv::Rect p = cv::Rect(start_x + (hand_idx * (80 + 8)), start_y, 80, 60); + cv::Mat start(cv::Size(80, 60), CV_32FC1, this_side_data, 80 * sizeof(float)); + start *= 255.0; + start.copyTo(hgt->visualizers.mat(p)); + } + + { + cv::Rect p = cv::Rect(start_x + (hand_idx * (80 + 8)), start_y + ((60 + 8)), 80, 60); + cv::Mat start(cv::Size(80, 60), CV_32FC1, this_side_data + 4800, 80 * sizeof(float)); + start *= 255.0; + start.copyTo(hgt->visualizers.mat(p)); + } + } } wrap->api->ReleaseValue(output_tensor); @@ -646,6 +671,25 @@ calc_src_tri(cv::Point2f center, } } +void +make_keypoint_heatmap_output(int camera_idx, int hand_idx, int grid_pt_x, int grid_pt_y, float *plane, cv::Mat &out) +{ + int root_x = 8 + ((1 + 2 * hand_idx) * (128 + 8)); + int root_y = 8 + (camera_idx * (128 + 8)); + + int org_x = (root_x) + (grid_pt_x * 25); + int org_y = (root_y) + (grid_pt_y * 25); + cv::Rect p = cv::Rect(org_x, org_y, 22, 22); + + + cv::Mat start(cv::Size(22, 22), CV_32FC1, plane, 22 * sizeof(float)); + // cv::Mat start(cv::Size(40, 42), CV_32FC1, plane, 40 * 42 * sizeof(float)); + start *= 255.0; + + start.copyTo(out(p)); +} + + void run_keypoint_estimation_new(void *ptr) { @@ -682,11 +726,11 @@ run_keypoint_estimation_new(void *ptr) cv::Matx23f go_there = getAffineTransform(src_tri, dst_tri); cv::Matx23f go_back = getAffineTransform(dst_tri, src_tri); + cv::Mat data_128x128_uint8; + { XRT_TRACE_IDENT(transforms); - cv::Mat data_128x128_uint8; - cv::warpAffine(info->view->run_model_on_this, data_128x128_uint8, go_there, cv::Size(128, 128), cv::INTER_LINEAR); @@ -742,21 +786,48 @@ run_keypoint_estimation_new(void *ptr) tan_space.kps[i] = raycoord(info->view, loc); } - if (hgt->debug_scribble && hgt->tuneable_values.scribble_keypoint_model_outputs) { - for (int finger = 0; finger < 5; finger++) { - cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y}; - for (int joint = 0; joint < 4; joint++) { - cv::Point the_new = {(int)keypoints_global[1 + finger * 4 + joint].x, - (int)keypoints_global[1 + finger * 4 + joint].y}; + if (hgt->debug_scribble) { + int data_acc_idx = 0; - cv::line(debug, last, the_new, colors[info->hand_idx]); - last = the_new; + int root_x = 8 + ((2 * info->hand_idx) * (128 + 8)); + int root_y = 8 + (info->view->view * (128 + 8)); + + cv::Rect p = cv::Rect(root_x, root_y, 128, 128); + + data_128x128_uint8.copyTo(hgt->visualizers.mat(p)); + + make_keypoint_heatmap_output(info->view->view, info->hand_idx, 0, 0, + out_data + (data_acc_idx * plane_size), hgt->visualizers.mat); + data_acc_idx++; + + for (int finger = 0; finger < 5; finger++) { + for (int joint = 0; joint < 4; joint++) { + + make_keypoint_heatmap_output(info->view->view, info->hand_idx, 1 + joint, finger, + out_data + (data_acc_idx * plane_size), + hgt->visualizers.mat); + data_acc_idx++; } } - for (int i = 0; i < 21; i++) { - xrt_vec2 loc = keypoints_global[i]; - handDot(debug, loc, 2, (float)(i) / 21.0, 1, 2); + + + if (hgt->tuneable_values.scribble_keypoint_model_outputs) { + for (int finger = 0; finger < 5; finger++) { + cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y}; + for (int joint = 0; joint < 4; joint++) { + cv::Point the_new = {(int)keypoints_global[1 + finger * 4 + joint].x, + (int)keypoints_global[1 + finger * 4 + joint].y}; + + cv::line(debug, last, the_new, colors[info->hand_idx]); + last = the_new; + } + } + + for (int i = 0; i < 21; i++) { + xrt_vec2 loc = keypoints_global[i]; + handDot(debug, loc, 2, (float)(i) / 21.0, 1, 2); + } } } diff --git a/src/xrt/tracking/hand/mercury/hg_sync.cpp b/src/xrt/tracking/hand/mercury/hg_sync.cpp index 58edd0eb9..963f9fc39 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.cpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.cpp @@ -14,6 +14,7 @@ #include "util/u_hand_tracking.h" #include "math/m_vec2.h" #include "util/u_misc.h" +#include "xrt/xrt_frame.h" #include @@ -590,12 +591,16 @@ HandTracking::HandTracking() { this->base.process = &HandTracking::cCallbackProcess; this->base.destroy = &HandTracking::cCallbackDestroy; - u_sink_debug_init(&this->debug_sink); + u_sink_debug_init(&this->debug_sink_ann); + u_sink_debug_init(&this->debug_sink_model); } HandTracking::~HandTracking() { - u_sink_debug_destroy(&this->debug_sink); + u_sink_debug_destroy(&this->debug_sink_ann); + u_sink_debug_destroy(&this->debug_sink_model); + + xrt_frame_reference(&this->visualizers.old_frame, NULL); release_onnx_wrap(&this->views[0].keypoint[0]); release_onnx_wrap(&this->views[0].keypoint[1]); @@ -672,7 +677,8 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, *out_timestamp_ns = hgt->current_frame_timestamp; // No filtering, fine to do this now. Also just a reminder // that this took you 2 HOURS TO DEBUG THAT ONE TIME. - hgt->debug_scribble = u_sink_debug_is_active(&hgt->debug_sink); + hgt->debug_scribble = + u_sink_debug_is_active(&hgt->debug_sink_ann) && u_sink_debug_is_active(&hgt->debug_sink_model); cv::Mat debug_output = {}; xrt_frame *debug_frame = nullptr; @@ -692,6 +698,35 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, hgt->views[0].debug_out_to_this = debug_output(cv::Rect(view_offsets[0], view_size)); hgt->views[1].debug_out_to_this = debug_output(cv::Rect(view_offsets[1], view_size)); scribble_image_boundary(hgt); + + // + + struct xrt_frame *new_model_inputs_and_outputs = NULL; + + // Let's check that the collage size is actually as big as we think it is + static_assert(1064 == (8 + ((128 + 8) * 4) + ((320 + 8)) + ((80 + 8) * 2) + 8)); + static_assert(504 == (240 + 240 + 8 + 8 + 8)); + + const int w = 1064; + const int h = 504; + + u_frame_create_one_off(XRT_FORMAT_L8, w, h, &hgt->visualizers.xrtframe); + hgt->visualizers.xrtframe->timestamp = hgt->current_frame_timestamp; + + cv::Size size = cv::Size(w, h); + + hgt->visualizers.mat = + cv::Mat(size, CV_8U, hgt->visualizers.xrtframe->data, hgt->visualizers.xrtframe->stride); + + if (hgt->visualizers.old_frame == NULL) { + // There wasn't a previous frame so let's setup the background + hgt->visualizers.mat = 255; + } else { + // They had better be the same size. + memcpy(hgt->visualizers.xrtframe->data, hgt->visualizers.old_frame->data, + hgt->visualizers.old_frame->size); + xrt_frame_reference(&hgt->visualizers.old_frame, NULL); + } } check_new_user_event(hgt); @@ -885,8 +920,14 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, // If the debug UI is active, push our debug frame if (hgt->debug_scribble) { - u_sink_debug_push_frame(&hgt->debug_sink, debug_frame); + u_sink_debug_push_frame(&hgt->debug_sink_ann, debug_frame); xrt_frame_reference(&debug_frame, NULL); + + // We don't dereference the model inputs/outputs frame here; we make a copy of it next frame and + // dereference it then. + u_sink_debug_push_frame(&hgt->debug_sink_model, hgt->visualizers.xrtframe); + xrt_frame_reference(&hgt->visualizers.old_frame, hgt->visualizers.xrtframe); + xrt_frame_reference(&hgt->visualizers.xrtframe, NULL); } // done! @@ -1043,7 +1084,8 @@ t_hand_tracking_sync_mercury_create(struct t_stereo_camera_calibration *calib, "Use IK optimizer (may put tracking in unexpected state, use with care)"); - u_var_add_sink_debug(hgt, &hgt->debug_sink, "i"); + u_var_add_sink_debug(hgt, &hgt->debug_sink_ann, "Annotated camera feeds"); + u_var_add_sink_debug(hgt, &hgt->debug_sink_model, "Model inputs and outputs"); HG_DEBUG(hgt, "Hand Tracker initialized!"); diff --git a/src/xrt/tracking/hand/mercury/hg_sync.hpp b/src/xrt/tracking/hand/mercury/hg_sync.hpp index abcbce692..17881f938 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.hpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.hpp @@ -172,6 +172,17 @@ struct hand_size_refinement float hand_size_refinement_schedule_y = 0; }; +struct model_output_visualizers +{ + // After setup, these reference the same piece of memory. + cv::Mat mat; + xrt_frame *xrtframe = NULL; + + // After pushing to the debug UI, we reference the frame here so that we can copy memory out of it for next + // frame. + xrt_frame *old_frame = NULL; +}; + /*! * Main class of Mercury hand tracking. * @@ -183,7 +194,8 @@ public: // Base thing, has to be first. t_hand_tracking_sync base = {}; - struct u_sink_debug debug_sink = {}; + struct u_sink_debug debug_sink_ann = {}; + struct u_sink_debug debug_sink_model = {}; float multiply_px_coord_for_undistort; @@ -203,6 +215,8 @@ public: struct ht_view views[2] = {}; + struct model_output_visualizers visualizers; + u_worker_thread_pool *pool; u_worker_group *group; @@ -213,8 +227,7 @@ public: uint64_t current_frame_timestamp = {}; - // Change this whenever you want - volatile bool debug_scribble = true; + bool debug_scribble = false; char models_folder[1024];