From 62013c142b817528edf39e51e2b869eb8f31218b Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Tue, 14 Jan 2020 18:18:08 +0000 Subject: [PATCH] t/helper: Introduce a small debug frame helper --- src/xrt/auxiliary/CMakeLists.txt | 1 + src/xrt/auxiliary/meson.build | 1 + .../tracking/t_helper_debug_sink.hpp | 127 ++++++++++++++++++ src/xrt/auxiliary/tracking/t_tracker_psmv.cpp | 49 +------ 4 files changed, 136 insertions(+), 42 deletions(-) create mode 100644 src/xrt/auxiliary/tracking/t_helper_debug_sink.hpp diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index c97b68e5b..da034657c 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -42,6 +42,7 @@ if(BUILD_TRACKING) tracking/t_debug_hsv_viewer.cpp tracking/t_file.cpp tracking/t_fusion.hpp + tracking/t_helper_debug_sink.hpp tracking/t_hsv_filter.c tracking/t_kalman.cpp tracking/t_tracker_psmv_fusion.hpp diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index d07a72d8c..56c67644e 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -120,6 +120,7 @@ if build_tracking 'tracking/t_debug_hsv_viewer.cpp', 'tracking/t_file.cpp', 'tracking/t_fusion.hpp', + 'tracking/t_helper_debug_sink.hpp', 'tracking/t_hsv_filter.c', 'tracking/t_kalman.cpp', 'tracking/t_tracker_psmv.cpp', diff --git a/src/xrt/auxiliary/tracking/t_helper_debug_sink.hpp b/src/xrt/auxiliary/tracking/t_helper_debug_sink.hpp new file mode 100644 index 000000000..7c9de7b3c --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_helper_debug_sink.hpp @@ -0,0 +1,127 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Small helper struct that for debugging views. + * @author Jakob Bornecrantz + * @ingroup aux_tracking + */ + +#pragma once + +#ifndef __cplusplus +#error "This header is C++-only." +#endif + +#include +#include "util/u_frame.h" + + +struct HelperDebugSink +{ +public: + enum Kind + { + AllAvailable, + AlwaysSingle, + }; + +public: + Kind kind = AllAvailable; + struct xrt_frame_sink *sink = {}; + struct xrt_frame *frame = {}; + + cv::Mat rgb[2] = {}; + + +public: + HelperDebugSink(Kind kind) + { + this->kind = kind; + } + + HelperDebugSink() = delete; + + ~HelperDebugSink() + { + xrt_frame_reference(&frame, NULL); + } + + void + refresh(struct xrt_frame *xf) + { + if (sink == NULL) { + return; + } + + // But what about second breakfast? + bool second_view = false; + int rows, cols, width, height; + + cols = xf->width; + rows = xf->height; + width = xf->width; + height = xf->height; + enum xrt_stereo_format stereo_format = xf->stereo_format; + + switch (xf->stereo_format) { + case XRT_STEREO_FORMAT_SBS: + cols /= 2; + if (kind == AllAvailable) { + second_view = true; + } else { + stereo_format = XRT_STEREO_FORMAT_NONE; + width /= 2; + second_view = false; + } + break; + case XRT_STEREO_FORMAT_NONE: + // Noop + break; + default: return; + } + + // Create a new frame and also dereferences the old frame. + u_frame_create_one_off(XRT_FORMAT_R8G8B8, width, height, + &frame); + + // Copy needed info. + frame->source_sequence = xf->source_sequence; + frame->stereo_format = stereo_format; + + // Doesn't claim ownership of the frame data, + // points directly at the frame data. + rgb[0] = cv::Mat( // + rows, // rows + cols, // cols + CV_8UC3, // channels + frame->data, // data + frame->stride); // stride + + if (second_view) { + // Doesn't claim ownership of the frame data, + // points directly at the frame data. + rgb[1] = cv::Mat( // + rows, // rows + cols, // cols + CV_8UC3, // channels + frame->data + 3 * cols, // data + frame->stride); // stride + } + } + + void + submit() + { + if (frame != NULL) { + // Make sure that the cv::Mats doesn't use the data. + rgb[0] = cv::Mat(); + rgb[1] = cv::Mat(); + sink->push_frame(sink, frame); + } + + // We unreference the frame here, downstream is either + // done with it or have referenced it themselves. + xrt_frame_reference(&frame, NULL); + } +}; diff --git a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp index f33300b47..68a410f63 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psmv.cpp @@ -14,6 +14,7 @@ #include "tracking/t_tracking.h" #include "tracking/t_calibration_opencv.hpp" #include "tracking/t_tracker_psmv_fusion.hpp" +#include "tracking/t_helper_debug_sink.hpp" #include "util/u_var.h" #include "util/u_misc.h" @@ -65,13 +66,7 @@ struct TrackerPSMV bool tracked = false; - struct - { - struct xrt_frame_sink *sink; - struct xrt_frame *frame; - - cv::Mat rgb[2]; - } debug; + HelperDebugSink debug = {HelperDebugSink::AllAvailable}; //! Have we received a new IMU sample. bool has_imu = false; @@ -95,34 +90,6 @@ struct TrackerPSMV xrt_vec3 tracked_object_position; }; -static void -refresh_gui_frame(TrackerPSMV &t, struct xrt_frame *xf) -{ - if (t.debug.sink == NULL) { - return; - } - - // Also dereferences the old frame. - u_frame_create_one_off(XRT_FORMAT_R8G8B8, xf->width, xf->height, - &t.debug.frame); - t.debug.frame->source_sequence = xf->source_sequence; - - int rows = xf->height; - int cols = xf->width / 2; - - t.debug.rgb[0] = cv::Mat(rows, // rows - cols, // cols - CV_8UC3, // channels - t.debug.frame->data, // data - t.debug.frame->stride); // stride - - t.debug.rgb[1] = cv::Mat(rows, // rows - cols, // cols - CV_8UC3, // channels - t.debug.frame->data + 3 * cols, // data - t.debug.frame->stride); // stride -} - /*! * @brief Perform per-view (two in a stereo camera image) processing on an @@ -255,7 +222,7 @@ process(TrackerPSMV &t, struct xrt_frame *xf) } // Create the debug frame if needed. - refresh_gui_frame(t, xf); + t.debug.refresh(xf); t.view[0].keypoints.clear(); t.view[1].keypoints.clear(); @@ -315,14 +282,12 @@ process(TrackerPSMV &t, struct xrt_frame *xf) t.filter->clear_position_tracked_flag(); } - if (t.debug.frame != NULL) { - t.debug.sink->push_frame(t.debug.sink, t.debug.frame); - t.debug.rgb[0] = cv::Mat(); - t.debug.rgb[1] = cv::Mat(); - } + // We are done with the debug frame. + t.debug.submit(); + // We are done with the frame. xrt_frame_reference(&xf, NULL); - xrt_frame_reference(&t.debug.frame, NULL); + if (nearest_world.got_one) { #if 0 //! @todo something less arbitrary for the lever arm?