From 79402d00812dac96ae70dde3d9753d9cc09ad588 Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Tue, 23 Jul 2019 17:29:14 +0100 Subject: [PATCH] aux/track: Add stub calibration tracker --- CMakeLists.txt | 3 + src/xrt/auxiliary/CMakeLists.txt | 25 ++ src/xrt/auxiliary/tracking/t_calibration.cpp | 260 +++++++++++++++ src/xrt/auxiliary/tracking/t_convert.cpp | 94 ++++++ .../auxiliary/tracking/t_debug_hsv_filter.cpp | 119 +++++++ .../auxiliary/tracking/t_debug_hsv_picker.cpp | 235 ++++++++++++++ .../auxiliary/tracking/t_debug_hsv_viewer.cpp | 182 +++++++++++ src/xrt/auxiliary/tracking/t_hsv_filter.c | 307 ++++++++++++++++++ src/xrt/auxiliary/tracking/t_tracking.h | 167 ++++++++++ 9 files changed, 1392 insertions(+) create mode 100644 src/xrt/auxiliary/tracking/t_calibration.cpp create mode 100644 src/xrt/auxiliary/tracking/t_convert.cpp create mode 100644 src/xrt/auxiliary/tracking/t_debug_hsv_filter.cpp create mode 100644 src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp create mode 100644 src/xrt/auxiliary/tracking/t_debug_hsv_viewer.cpp create mode 100644 src/xrt/auxiliary/tracking/t_hsv_filter.c create mode 100644 src/xrt/auxiliary/tracking/t_tracking.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 4de2b4696..3b641cbee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,9 @@ endif() if(BUILD_WITH_OPENCV) add_definitions(-DXRT_HAVE_OPENCV) + + # Tracking requires OpenCV + set(BUILD_TRACKING TRUE) endif() if(BUILD_WITH_JPEG) diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 5be6d57ca..e95a9e87c 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -16,6 +16,16 @@ set(OS_SOURCE_FILES os/os_hid_hidraw.c ) +set(TRACKING_SOURCE_FILES + tracking/t_calibration.cpp + tracking/t_convert.cpp + tracking/t_debug_hsv_filter.cpp + tracking/t_debug_hsv_picker.cpp + tracking/t_debug_hsv_viewer.cpp + tracking/t_hsv_filter.c + tracking/t_tracking.h + ) + set(UTIL_SOURCE_FILES util/u_misc.c util/u_misc.h @@ -60,3 +70,18 @@ set_property(TARGET aux_math PROPERTY POSITION_INDEPENDENT_CODE ON) target_include_directories(aux_math SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR} ) + + +if(BUILD_TRACKING) + # Tracking library. + # Use OBJECT to not create a archive, since it just gets in the way. + add_library(aux_tracking OBJECT ${TRACKING_SOURCE_FILES}) + set_property(TARGET aux_tracking PROPERTY POSITION_INDEPENDENT_CODE ON) + + # Math files has extra include(s). + target_include_directories(aux_tracking SYSTEM + PRIVATE + ${EIGEN3_INCLUDE_DIR} + ${OpenCV_INCLUDE_DIRS} + ) +endif() diff --git a/src/xrt/auxiliary/tracking/t_calibration.cpp b/src/xrt/auxiliary/tracking/t_calibration.cpp new file mode 100644 index 000000000..03e5ad0e4 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_calibration.cpp @@ -0,0 +1,260 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Calibration code. + * @author Pete Black + * @author Jakob Bornecrantz + */ + +#include "util/u_sink.h" +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_format.h" +#include "tracking/t_tracking.h" + +#include + +DEBUG_GET_ONCE_BOOL_OPTION(hsv_filter, "T_DEBUG_HSV_FILTER", false) +DEBUG_GET_ONCE_BOOL_OPTION(hsv_picker, "T_DEBUG_HSV_PICKER", false) +DEBUG_GET_ONCE_BOOL_OPTION(hsv_viewer, "T_DEBUG_HSV_VIEWER", false) + + +/* + * + * Structs + * + */ + +class Calibration +{ +public: + struct xrt_frame_sink base = {}; + + struct + { + cv::Mat rgb = {}; + struct xrt_frame_sink *sink = {}; + } gui; + + cv::Mat grey; + + char text[512]; +}; + +/*! + * Holds `cv::Mat`s used during frame processing when processing a yuyv frame. + */ +struct t_frame_yuyv +{ +public: + //! Full frame size, each block is split across two cols. + cv::Mat data_full = {}; + //! Half horizontal width covering a complete block of two pixels. + cv::Mat data_half = {}; +}; + + +/* + * + * Small helpers. + * + */ + +static void +send_rgb_frame(struct xrt_frame_sink *xsink, cv::Mat &rgb) +{ + struct xrt_frame xf = {}; + + xf.format = XRT_FORMAT_R8G8B8; + xf.width = rgb.cols; + xf.height = rgb.rows; + xf.data = rgb.data; + + u_format_size_for_dimensions(xf.format, xf.width, xf.height, &xf.stride, + &xf.size); + + xsink->push_frame(xsink, &xf); +} + +static void +ensure_buffers_are_allocated(class Calibration &c, int rows, int cols) +{ + if (c.gui.rgb.cols == cols && c.gui.rgb.rows == rows) { + return; + } + + c.grey = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(0)); + c.gui.rgb = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(0, 0, 0)); +} + +static void +print_txt(cv::Mat &rgb, const char *text, double fontScale) +{ + int fontFace = 0; + int thickness = 2; + cv::Size textSize = + cv::getTextSize(text, fontFace, fontScale, thickness, NULL); + + cv::Point textOrg((rgb.cols - textSize.width) / 2, textSize.height * 2); + + cv::putText(rgb, text, textOrg, fontFace, fontScale, + cv::Scalar(192, 192, 192), thickness); +} + +static void +make_gui_str(class Calibration &c) +{ + auto &rgb = c.gui.rgb; + + int cols = 800; + int rows = 100; + ensure_buffers_are_allocated(c, rows, cols); + + cv::rectangle(rgb, cv::Point2f(0, 0), cv::Point2f(cols, rows), + cv::Scalar(0, 0, 0), -1, 0); + + print_txt(rgb, c.text, 1.0); + + send_rgb_frame(c.gui.sink, c.gui.rgb); +} + +static void +make_calibration_frame(class Calibration &c) +{ + auto &rgb = c.gui.rgb; + + if (rgb.rows == 0 || rgb.cols == 0) { + ensure_buffers_are_allocated(c, 480, 640); + cv::rectangle(c.gui.rgb, cv::Point2f(0, 0), + cv::Point2f(rgb.cols, rgb.rows), + cv::Scalar(0, 0, 0), -1, 0); + } + + /* + * Draw text + */ + + print_txt(rgb, "CALIBRATION MODE", 1.5); + + send_rgb_frame(c.gui.sink, rgb); +} + + +/* + * + * Main functions. + * + */ + +static void +process_frame_yuv(class Calibration &c, struct xrt_frame *xf) +{ + + int w = (int)xf->width; + int h = (int)xf->height; + + cv::Mat data(h, w, CV_8UC3, xf->data, xf->stride); + ensure_buffers_are_allocated(c, data.rows, data.cols); + + cv::cvtColor(data, c.gui.rgb, cv::COLOR_YUV2RGB); + cv::cvtColor(c.gui.rgb, c.grey, cv::COLOR_RGB2GRAY); +} + +static void +process_frame_yuyv(class Calibration &c, struct xrt_frame *xf) +{ + /* + * Cleverly extract the different channels. + * Cr/Cb are extracted at half width. + */ + int w = (int)xf->width; + int half_w = w / 2; + int h = (int)xf->height; + + class t_frame_yuyv f = {}; + + f.data_half = cv::Mat(h, half_w, CV_8UC4, xf->data, xf->stride); + f.data_full = cv::Mat(h, w, CV_8UC2, xf->data, xf->stride); + ensure_buffers_are_allocated(c, f.data_full.rows, f.data_full.cols); + + cv::cvtColor(f.data_full, c.gui.rgb, cv::COLOR_YUV2RGB_YUYV); + cv::cvtColor(f.data_full, c.grey, cv::COLOR_YUV2GRAY_YUYV); +} + + +/* + * + * Interface functions. + * + */ + +extern "C" void +t_calibration_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &c = *(struct Calibration *)xsink; + +#if 0 + if (xf->stereo_format != XRT_FS_STEREO_SBS) { + snprintf(c.text, sizeof(c.text), + "ERROR: Not side by side stereo!"); + make_gui_str(c); + return; + } +#endif + + // Fill both c.gui.rgb and c.grey with the data we got. + switch (xf->format) { + case XRT_FORMAT_YUV888: process_frame_yuv(c, xf); break; + case XRT_FORMAT_YUV422: process_frame_yuyv(c, xf); break; + default: + snprintf(c.text, sizeof(c.text), "ERROR: Bad format '%s'", + u_format_str(xf->format)); + make_gui_str(c); + return; + } + + make_calibration_frame(c); +} + + +/* + * + * Exported functions. + * + */ + +extern "C" int +t_calibration_create(struct xrt_frame_sink *gui, + struct xrt_frame_sink **out_sink) +{ + + auto &c = *(new Calibration()); + + c.gui.sink = gui; + + c.base.push_frame = t_calibration_frame; + + *out_sink = &c.base; + + snprintf(c.text, sizeof(c.text), "Waiting for camera"); + make_gui_str(c); + + int ret = 0; + if (debug_get_bool_option_hsv_filter()) { + ret = t_debug_hsv_filter_create(*out_sink, out_sink); + } + + if (debug_get_bool_option_hsv_picker()) { + ret = t_debug_hsv_picker_create(*out_sink, out_sink); + } + + if (debug_get_bool_option_hsv_viewer()) { + ret = t_debug_hsv_viewer_create(*out_sink, out_sink); + } + + // Ensure we only get yuv or yuyv frames. + u_sink_create_to_yuv_or_yuyv(*out_sink, out_sink); + + return ret; +} diff --git a/src/xrt/auxiliary/tracking/t_convert.cpp b/src/xrt/auxiliary/tracking/t_convert.cpp new file mode 100644 index 000000000..d42ce54a4 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_convert.cpp @@ -0,0 +1,94 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Code to build conversion tables and convert images. + * @author Jakob Bornecrantz + */ + +#include "tracking/t_tracking.h" + +#include + + +/* + * + * 'Exported' functions. + * + */ + +extern "C" void +t_convert_fill_table(struct t_convert_table *t) +{ + for (int y = 0; y < 256; y++) { + for (int u = 0; u < 256; u++) { + uint8_t *dst = &t->v[y][u][0][0]; + + for (int v = 0; v < 256; v++) { + dst[0] = y; + dst[1] = u; + dst[2] = v; + dst += 3; + } + } + } +} + +extern "C" void +t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t) +{ + size_t size = 256 * 256 * 256; + + t_convert_fill_table(t); + t_convert_in_place_y8u8v8_to_r8g8b8(size, 1, 0, t); +} + +extern "C" void +t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t) +{ + size_t size = 256 * 256 * 256; + + t_convert_fill_table(t); + t_convert_in_place_y8u8v8_to_h8s8v8(size, 1, 0, &t->v); +} + +extern "C" void +t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t) +{ + size_t size = 256 * 256 * 256; + + t_convert_fill_table(t); + t_convert_in_place_h8s8v8_to_r8g8b8(size, 1, 0, &t->v); +} + +extern "C" void +t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr) +{ + cv::Mat data(height, width, CV_8UC3, data_ptr, stride); + cv::cvtColor(data, data, cv::COLOR_YUV2RGB); +} + +extern "C" void +t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr) +{ + cv::Mat data(height, width, CV_8UC3, data_ptr, stride); + cv::Mat temp(height, width, CV_32FC3); + cv::cvtColor(data, temp, cv::COLOR_YUV2RGB); + cv::cvtColor(temp, data, cv::COLOR_RGB2HSV); +} + +extern "C" void +t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr) +{ + cv::Mat data(height, width, CV_8UC3, data_ptr, stride); + cv::cvtColor(data, data, cv::COLOR_YUV2RGB); +} diff --git a/src/xrt/auxiliary/tracking/t_debug_hsv_filter.cpp b/src/xrt/auxiliary/tracking/t_debug_hsv_filter.cpp new file mode 100644 index 000000000..0bef3f110 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_debug_hsv_filter.cpp @@ -0,0 +1,119 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief HSV filter debug code. + * @author Jakob Bornecrantz + */ + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_format.h" +#include "tracking/t_tracking.h" + +#include + + +/* + * + * Defines and structs + * + */ + +#define HSV0_WIN "HSV Channel #1 (Red)" +#define HSV1_WIN "HSV Channel #2 (Purple)" +#define HSV2_WIN "HSV Channel #3 (Blue)" +#define HSV3_WIN "HSV Channel #4 (White)" + +class DebugHSV +{ +public: + struct xrt_frame_sink base = {}; + struct xrt_frame_sink sinks[4] = {}; + + struct xrt_frame_sink *sink; + struct xrt_frame_sink *passthrough; +}; + + +/* + * + * Exported functions. + * + */ + +extern "C" void +t_debug_hsv_filter_frame0(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + cv::Mat tmp(xf->height, xf->width, CV_8UC1, xf->data, xf->stride); + + cv::imshow(HSV0_WIN, tmp); +} + +extern "C" void +t_debug_hsv_filter_frame1(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + cv::Mat tmp(xf->height, xf->width, CV_8UC1, xf->data, xf->stride); + + cv::imshow(HSV1_WIN, tmp); +} + +extern "C" void +t_debug_hsv_filter_frame2(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + cv::Mat tmp(xf->height, xf->width, CV_8UC1, xf->data, xf->stride); + + cv::imshow(HSV2_WIN, tmp); +} + +extern "C" void +t_debug_hsv_filter_frame3(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + cv::Mat tmp(xf->height, xf->width, CV_8UC1, xf->data, xf->stride); + + cv::imshow(HSV3_WIN, tmp); +} + +extern "C" void +t_debug_hsv_filter_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &d = *(struct DebugHSV *)xsink; + + d.sink->push_frame(d.sink, xf); + d.passthrough->push_frame(d.passthrough, xf); +} + +extern "C" int +t_debug_hsv_filter_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink) +{ + auto &d = *(new DebugHSV()); + + cv::namedWindow(HSV0_WIN); + cv::namedWindow(HSV1_WIN); + cv::namedWindow(HSV2_WIN); + cv::namedWindow(HSV3_WIN); + + cv::startWindowThread(); + + d.passthrough = passthrough; + d.sinks[0].push_frame = t_debug_hsv_filter_frame0; + d.sinks[1].push_frame = t_debug_hsv_filter_frame1; + d.sinks[2].push_frame = t_debug_hsv_filter_frame2; + d.sinks[3].push_frame = t_debug_hsv_filter_frame3; + d.base.push_frame = t_debug_hsv_filter_frame; + + struct xrt_frame_sink *sinks[4] = { + &d.sinks[0], + &d.sinks[1], + &d.sinks[2], + &d.sinks[3], + }; + + *out_sink = &d.base; + + t_hsv_filter_params params = T_HSV_DEFAULT_PARAMS(); + t_hsv_filter_create(¶ms, sinks, &d.sink); + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp b/src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp new file mode 100644 index 000000000..4f07b6a25 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp @@ -0,0 +1,235 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief HSV Picker Debugging code. + * @author Pete Black + * @author Jakob Bornecrantz + */ + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_format.h" + +#include "tracking/t_tracking.h" + +#include + + +/* + * + * Defines and structs + * + */ + +#define PICK_WIN "HSV Picker Debugger" + +#define max(a, b) (a > b ? a : b) +#define min(a, b) (a < b ? a : b) + +class DebugHSVPicker +{ +public: + struct xrt_frame_sink base = {}; + + struct + { + cv::Mat hsv = {}; + cv::Mat threshold = {}; + } debug; + + struct xrt_frame_sink *passthrough; + + struct t_convert_table yuv_to_hsv; +}; + +const int max_value_H = 360 / 2; +const int max_value = 256; +static int low_H = 0, low_S = 0, low_V = 0; +static int high_H = max_value_H, high_S = max_value, high_V = max_value; + + +/* + * + * Debug functions. + * + */ + +static void +ensure_debug_is_allocated(class DebugHSVPicker &d, int rows, int cols) +{ + if (d.debug.hsv.cols == cols && d.debug.hsv.rows == rows) { + return; + } + + d.debug.threshold = cv::Mat(rows, cols, CV_8UC1); + d.debug.hsv = cv::Mat(rows, cols, CV_8UC3); +} + +static void +process_frame_yuv(class DebugHSVPicker &d, struct xrt_frame *xf) +{ + for (uint32_t y = 0; y < xf->height; y++) { + uint8_t *src = (uint8_t *)xf->data + y * xf->stride; + auto hsv = d.debug.hsv.ptr(y); + for (uint32_t x = 0; x < xf->width; x++) { + uint8_t y = src[0]; + uint8_t cb = src[1]; + uint8_t cr = src[2]; + + uint8_t *hsv1 = d.yuv_to_hsv.v[y][cb][cr]; + + hsv[0] = hsv1[0]; + hsv[1] = hsv1[1]; + hsv[2] = hsv1[2]; + + hsv += 3; + src += 3; + } + } + + cv::inRange(d.debug.hsv, cv::Scalar(low_H, low_S, low_V), + cv::Scalar(high_H, high_S, high_V), d.debug.threshold); + cv::imshow(PICK_WIN, d.debug.threshold); +} + +static void +process_frame_yuyv(class DebugHSVPicker &d, struct xrt_frame *xf) +{ + for (uint32_t y = 0; y < xf->height; y++) { + uint8_t *src = (uint8_t *)xf->data + y * xf->stride; + auto hsv = d.debug.hsv.ptr(y); + for (uint32_t x = 0; x < xf->width; x += 2) { + uint8_t y1 = src[0]; + uint8_t cb = src[1]; + uint8_t y2 = src[2]; + uint8_t cr = src[3]; + + uint8_t *hsv1 = d.yuv_to_hsv.v[y1][cb][cr]; + uint8_t *hsv2 = d.yuv_to_hsv.v[y2][cb][cr]; + + hsv[0] = hsv1[0]; + hsv[1] = hsv1[1]; + hsv[2] = hsv1[2]; + hsv[3] = hsv2[0]; + hsv[4] = hsv2[1]; + hsv[5] = hsv2[2]; + + hsv += 6; + src += 4; + } + } + + cv::inRange(d.debug.hsv, cv::Scalar(low_H, low_S, low_V), + cv::Scalar(high_H, high_S, high_V), d.debug.threshold); + cv::imshow(PICK_WIN, d.debug.threshold); +} + +static void +process_frame(class DebugHSVPicker &d, struct xrt_frame *xf) +{ + ensure_debug_is_allocated(d, xf->height, xf->width); + + switch (xf->format) { + case XRT_FORMAT_YUV888: process_frame_yuv(d, xf); break; + case XRT_FORMAT_YUV422: process_frame_yuyv(d, xf); break; + default: + fprintf(stderr, "ERROR: Bad format '%s'", + u_format_str(xf->format)); + break; + } +} + +static void +on_low_H_thresh_trackbar(int, void *) +{ + low_H = min(high_H - 1, low_H); + cv::setTrackbarPos("Low H", PICK_WIN, low_H); +} + +static void +on_high_H_thresh_trackbar(int, void *) +{ + high_H = max(high_H, low_H + 1); + cv::setTrackbarPos("High H", PICK_WIN, high_H); +} + +static void +on_low_S_thresh_trackbar(int, void *) +{ + low_S = min(high_S - 1, low_S); + cv::setTrackbarPos("Low S", PICK_WIN, low_S); +} + +static void +on_high_S_thresh_trackbar(int, void *) +{ + high_S = max(high_S, low_S + 1); + cv::setTrackbarPos("High S", PICK_WIN, high_S); +} + +static void +on_low_V_thresh_trackbar(int, void *) +{ + low_V = min(high_V - 1, low_V); + cv::setTrackbarPos("Low V", PICK_WIN, low_V); +} + +static void +on_high_V_thresh_trackbar(int, void *) +{ + high_V = max(high_V, low_V + 1); + cv::setTrackbarPos("High V", PICK_WIN, high_V); +} + + +/* + * + * Exported functions. + * + */ + +extern "C" void +t_debug_hsv_picker_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &d = *(struct DebugHSVPicker *)xsink; + + process_frame(d, xf); + + d.passthrough->push_frame(d.passthrough, xf); +} + +extern "C" int +t_debug_hsv_picker_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink) +{ + auto &d = *(new DebugHSVPicker()); + + cv::namedWindow(PICK_WIN); + + // Trackbars to set thresholds for HSV values + cv::createTrackbar("Low H", PICK_WIN, &low_H, max_value_H, + on_low_H_thresh_trackbar); + cv::createTrackbar("High H", PICK_WIN, &high_H, max_value_H, + on_high_H_thresh_trackbar); + cv::createTrackbar("Low S", PICK_WIN, &low_S, max_value, + on_low_S_thresh_trackbar); + cv::createTrackbar("High S", PICK_WIN, &high_S, max_value, + on_high_S_thresh_trackbar); + cv::createTrackbar("Low V", PICK_WIN, &low_V, max_value, + on_low_V_thresh_trackbar); + cv::createTrackbar("High V", PICK_WIN, &high_V, max_value, + on_high_V_thresh_trackbar); + + cv::startWindowThread(); + + t_convert_make_y8u8v8_to_h8s8v8(&d.yuv_to_hsv); + + d.passthrough = passthrough; + + d.base.push_frame = t_debug_hsv_picker_frame; + + *out_sink = &d.base; + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_debug_hsv_viewer.cpp b/src/xrt/auxiliary/tracking/t_debug_hsv_viewer.cpp new file mode 100644 index 000000000..cd01d7d18 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_debug_hsv_viewer.cpp @@ -0,0 +1,182 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief HSV debug viewer code. + * @author Jakob Bornecrantz + */ + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_format.h" +#include "tracking/t_tracking.h" + +#include + + +/* + * + * Defines and structs + * + */ + +#define HSV_WIN "HSV Filter Tester" + +class DebugHSV +{ +public: + struct xrt_frame_sink base = {}; + + struct xrt_frame_sink *passthrough; + + cv::Mat bgr; + + int lum_value = 0; + + // No need to initialize these + struct t_convert_table yuv_to_rgb_table; + struct t_hsv_filter_large_table hsv_large; + struct t_hsv_filter_optimized_table hsv_opt; +}; + + +/* + * + * Debug functions. + * + */ + +static void +process_pixel(bool f1, + bool f1_diff, + uint8_t *hsv_cap, + uint8_t *hsv_opt, + uint8_t *hsv_diff, + uint8_t *rgb) +{ + if (f1) { + hsv_cap[0] = rgb[2]; + hsv_cap[1] = rgb[1]; + hsv_cap[2] = rgb[0]; + } else { + hsv_cap[0] = 0; + hsv_cap[1] = 0; + hsv_cap[2] = 0; + } + + if (f1_diff) { + hsv_opt[0] = rgb[2]; + hsv_opt[1] = rgb[1]; + hsv_opt[2] = rgb[0]; + } else { + hsv_opt[0] = 0; + hsv_opt[1] = 0; + hsv_opt[2] = 0; + } + + if (f1 > f1_diff) { + hsv_diff[0] = 0xff; + hsv_diff[1] = 0; + hsv_diff[2] = 0; + } else if (f1 < f1_diff) { + hsv_diff[0] = 0; + hsv_diff[1] = 0; + hsv_diff[2] = 0xff; + } else { + hsv_diff[0] = 0; + hsv_diff[1] = 0; + hsv_diff[2] = 0; + } +} + +#define SIZE 256 +#define NUM_CHAN 4 + +static void +process_frame(DebugHSV &d, struct xrt_frame *xf) +{ + uint32_t width = SIZE * 3; + uint32_t height = SIZE * NUM_CHAN; + + auto &bgr = d.bgr; + if (bgr.rows != (int)height || bgr.cols != (int)width) { + bgr = cv::Mat(height, width, CV_8UC3); + } + + for (uint32_t yp = 0; yp < SIZE; yp++) { + for (int chan = 0; chan < NUM_CHAN; chan++) { + auto hsv_cap = bgr.ptr(yp + SIZE * chan); + auto hsv_opt = + bgr.ptr(yp + SIZE * chan) + 256 * 3; + auto hsv_diff = + bgr.ptr(yp + SIZE * chan) + 512 * 3; + int mask = 1 << chan; + + for (uint32_t xp = 0; xp < SIZE; xp++) { + int y = d.lum_value; + int u = yp; + int v = xp; + + uint8_t *rgb = d.yuv_to_rgb_table.v[y][u][v]; + uint8_t large = d.hsv_large.v[y][u][v]; + uint8_t opt = + t_hsv_filter_sample(&d.hsv_opt, y, u, v); + + large = (large & mask) != 0; + opt = (opt & mask) != 0; + + process_pixel(large, opt, hsv_cap, hsv_opt, + hsv_diff, rgb); + + hsv_cap += 3; + hsv_opt += 3; + hsv_diff += 3; + } + } + } + + cv::imshow(HSV_WIN, bgr); +} + + +/* + * + * Exported functions. + * + */ + +extern "C" void +t_debug_hsv_viewer_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &d = *(struct DebugHSV *)xsink; + + process_frame(d, xf); + + d.passthrough->push_frame(d.passthrough, xf); +} + +extern "C" int +t_debug_hsv_viewer_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink) +{ + auto &d = *(new DebugHSV()); + + cv::namedWindow(HSV_WIN); + + cv::createTrackbar("Luma", HSV_WIN, &d.lum_value, 255, NULL); + + cv::startWindowThread(); + + d.passthrough = passthrough; + + d.base.push_frame = t_debug_hsv_viewer_frame; + + t_convert_make_y8u8v8_to_r8g8b8(&d.yuv_to_rgb_table); + struct t_hsv_filter_params params = T_HSV_DEFAULT_PARAMS(); + t_hsv_build_large_table(¶ms, &d.hsv_large); + t_hsv_build_optimized_table(¶ms, &d.hsv_opt); + + *out_sink = &d.base; + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_hsv_filter.c b/src/xrt/auxiliary/tracking/t_hsv_filter.c new file mode 100644 index 000000000..32d3ba411 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_hsv_filter.c @@ -0,0 +1,307 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief A simple HSV filter. + * @author Jakob Bornecrantz + */ + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_format.h" + +#include "tracking/t_tracking.h" + +#include + +#define MOD_180(v) ((uint32_t)(v) % 180) + +static inline bool +check_range(struct t_hsv_filter_color color, uint32_t h, uint32_t s, uint32_t v) +{ + bool bad = false; + bad |= s < color.s_min; + bad |= v < color.v_min; + bad |= MOD_180(h + (360 - color.hue_min)) >= color.hue_range; + return !bad; +} + +void +t_hsv_build_convert_table(struct t_hsv_filter_params *params, + struct t_convert_table *t) +{ + struct t_hsv_filter_large_table *temp = + U_TYPED_CALLOC(struct t_hsv_filter_large_table); + t_hsv_build_large_table(params, temp); + + uint8_t *dst = &t->v[0][0][0][0]; + + for (int y = 0; y < 256; y++) { + for (int u = 0; u < 256; u++) { + for (int v = 0; v < 256; v++) { + uint32_t mask = temp->v[y][u][v]; + + dst[0] = ((mask & 1) != 0) ? 0xff : 0x00; + dst[1] = ((mask & 2) != 0) ? 0xff : 0x00; + dst[2] = ((mask & 4) != 0) ? 0xff : 0x00; + + dst += 3; + } + } + } + + free(temp); +} + +void +t_hsv_build_large_table(struct t_hsv_filter_params *params, + struct t_hsv_filter_large_table *t) +{ + struct t_convert_table *temp = U_TYPED_CALLOC(struct t_convert_table); + t_convert_make_y8u8v8_to_h8s8v8(temp); + + uint8_t *dst = &t->v[0][0][0]; + for (int y = 0; y < 256; y++) { + for (int u = 0; u < 256; u++) { + for (int v = 0; v < 256; v++) { + uint32_t h = temp->v[y][u][v][0]; + uint8_t s = temp->v[y][u][v][1]; + uint8_t v2 = temp->v[y][u][v][2]; + + bool f0 = + check_range(params->color[0], h, s, v2); + bool f1 = + check_range(params->color[1], h, s, v2); + bool f2 = + check_range(params->color[2], h, s, v2); + bool f3 = s <= params->white.s_max && + v2 >= params->white.v_min; + + *dst = (f0 << 0) | (f1 << 1) | (f2 << 2) | + (f3 << 3); + dst += 1; + } + } + } + + free(temp); +} + +void +t_hsv_build_optimized_table(struct t_hsv_filter_params *params, + struct t_hsv_filter_optimized_table *t) +{ + struct t_hsv_filter_large_table *temp = + U_TYPED_CALLOC(struct t_hsv_filter_large_table); + t_hsv_build_large_table(params, temp); + + // Half of step, minues one + int offset = (T_HSV_STEP / 2) - 1; + + for (int y = 0; y < T_HSV_SIZE; y++) { + + int src_y = y * T_HSV_STEP + offset; + + for (int u = 0; u < T_HSV_SIZE; u++) { + + int src_u = u * T_HSV_STEP + offset; + int src_v = offset; + + for (int v = 0; v < T_HSV_SIZE; v++) { + t->v[y][u][v] = temp->v[src_y][src_u][src_v]; + + src_v += T_HSV_STEP; + } + } + } + + free(temp); +} + + +/* + * + * Sink filter + * + */ + +#define NUM_CHANNELS 4 + +struct t_hsv_filter +{ + struct xrt_frame_sink base; + + struct xrt_frame_sink *sinks[NUM_CHANNELS]; + + struct t_hsv_filter_params params; + + uint8_t *buf0; + uint8_t *buf1; + uint8_t *buf2; + uint8_t *buf3; + size_t buf_stride; + size_t buf_size; + uint32_t buf_width; + uint32_t buf_height; + + struct t_hsv_filter_optimized_table table; +}; + +static void +process_sample(struct t_hsv_filter *f, + uint8_t y, + uint8_t cb, + uint8_t cr, + uint8_t *dst0, + uint8_t *dst1, + uint8_t *dst2, + uint8_t *dst3) +{ + uint8_t bits = t_hsv_filter_sample(&f->table, y, cb, cr); + + *dst0 = (bits & (1 << 0)) ? 0xff : 0x00; + *dst1 = (bits & (1 << 1)) ? 0xff : 0x00; + *dst2 = (bits & (1 << 2)) ? 0xff : 0x00; + *dst3 = (bits & (1 << 3)) ? 0xff : 0x00; +} + +static void +process_frame_yuv(struct t_hsv_filter *f, struct xrt_frame *xf) +{ + for (uint32_t y = 0; y < xf->height; y++) { + uint8_t *src = (uint8_t *)xf->data + y * xf->stride; + uint8_t *dst0 = f->buf0 + y * f->buf_stride; + uint8_t *dst1 = f->buf1 + y * f->buf_stride; + uint8_t *dst2 = f->buf2 + y * f->buf_stride; + uint8_t *dst3 = f->buf3 + y * f->buf_stride; + + for (uint32_t x = 0; x < xf->width; x += 1) { + uint8_t y = src[0]; + uint8_t cb = src[1]; + uint8_t cr = src[2]; + src += 3; + + process_sample(f, y, cb, cr, dst0, dst1, dst2, dst3); + dst0 += 1; + dst1 += 1; + dst2 += 1; + dst3 += 1; + } + } +} + +static void +process_frame_yuyv(struct t_hsv_filter *f, struct xrt_frame *xf) +{ + for (uint32_t y = 0; y < xf->height; y++) { + uint8_t *src = (uint8_t *)xf->data + y * xf->stride; + uint8_t *dst0 = f->buf0 + y * f->buf_stride; + uint8_t *dst1 = f->buf1 + y * f->buf_stride; + uint8_t *dst2 = f->buf2 + y * f->buf_stride; + uint8_t *dst3 = f->buf3 + y * f->buf_stride; + + for (uint32_t x = 0; x < xf->width; x += 2) { + uint8_t y1 = src[0]; + uint8_t cb = src[1]; + uint8_t y2 = src[2]; + uint8_t cr = src[3]; + src += 4; + + process_sample(f, y1, cb, cr, dst0, dst1, dst2, dst3); + dst0 += 1; + dst1 += 1; + dst2 += 1; + dst3 += 1; + + process_sample(f, y2, cb, cr, dst0, dst1, dst2, dst3); + dst0 += 1; + dst1 += 1; + dst2 += 1; + dst3 += 1; + } + } +} + +static void +ensure_buf_allocated(struct t_hsv_filter *f, struct xrt_frame *xf) +{ + if (xf->width == f->buf_width && xf->width == f->buf_height) { + return; + } + + free(f->buf0); + free(f->buf1); + free(f->buf2); + free(f->buf3); + + f->buf_width = xf->width; + f->buf_height = xf->height; + f->buf_stride = f->buf_width; + f->buf_size = f->buf_stride * f->buf_height; + + f->buf0 = U_TYPED_ARRAY_CALLOC(uint8_t, f->buf_size); + f->buf1 = U_TYPED_ARRAY_CALLOC(uint8_t, f->buf_size); + f->buf2 = U_TYPED_ARRAY_CALLOC(uint8_t, f->buf_size); + f->buf3 = U_TYPED_ARRAY_CALLOC(uint8_t, f->buf_size); +} + +static void +push_buf(struct t_hsv_filter *f, struct xrt_frame_sink *xsink, uint8_t *buf) +{ + if (xsink == NULL) { + return; + } + + struct xrt_frame xf = {0}; + + xf.format = XRT_FORMAT_L8; + xf.width = f->buf_width; + xf.height = f->buf_height; + xf.stride = f->buf_stride; + xf.size = f->buf_size; + xf.data = buf; + + xsink->push_frame(xsink, &xf); +} + +static void +push_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + struct t_hsv_filter *f = (struct t_hsv_filter *)xsink; + + ensure_buf_allocated(f, xf); + + switch (xf->format) { + case XRT_FORMAT_YUV888: process_frame_yuv(f, xf); break; + case XRT_FORMAT_YUV422: process_frame_yuyv(f, xf); break; + default: + fprintf(stderr, "ERROR: Bad format '%s'", + u_format_str(xf->format)); + return; + } + + push_buf(f, f->sinks[0], f->buf0); + push_buf(f, f->sinks[1], f->buf1); + push_buf(f, f->sinks[2], f->buf2); + push_buf(f, f->sinks[3], f->buf3); +} + +int +t_hsv_filter_create(struct t_hsv_filter_params *params, + struct xrt_frame_sink *sinks[4], + struct xrt_frame_sink **out_sink) +{ + struct t_hsv_filter *f = U_TYPED_CALLOC(struct t_hsv_filter); + f->params = *params; + f->base.push_frame = push_frame; + f->sinks[0] = sinks[0]; + f->sinks[1] = sinks[1]; + f->sinks[2] = sinks[2]; + f->sinks[3] = sinks[3]; + + t_hsv_build_optimized_table(&f->params, &f->table); + + *out_sink = &f->base; + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h new file mode 100644 index 000000000..771e7440f --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -0,0 +1,167 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Tracking API interface. + * @author Pete Black + * @author Jakob Bornecrantz + */ + +#pragma once + +#include "xrt/xrt_frame.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/* + * + * Conversion functions. + * + */ + +struct t_convert_table +{ + uint8_t v[256][256][256][3]; +}; + +void +t_convert_fill_table(struct t_convert_table *t); + +void +t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t); + +void +t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t); + +void +t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t); + +void +t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr); + +void +t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr); + +void +t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width, + uint32_t height, + size_t stride, + void *data_ptr); + + +/* + * + * Filter functions. + * + */ + +#define T_HSV_SIZE 32 +#define T_HSV_STEP (256 / T_HSV_SIZE) + +#define T_HSV_DEFAULT_PARAMS() \ + { \ + { \ + {165, 30, 160, 100}, \ + {135, 30, 160, 100}, \ + {95, 30, 160, 100}, \ + }, \ + {128, 80}, \ + } + +struct t_hsv_filter_color +{ + uint8_t hue_min; + uint8_t hue_range; + + uint8_t s_min; + + uint8_t v_min; +}; + +struct t_hsv_filter_params +{ + struct t_hsv_filter_color color[3]; + + struct + { + uint8_t s_max; + uint8_t v_min; + } white; +}; + +struct t_hsv_filter_large_table +{ + uint8_t v[256][256][256]; +}; + +struct t_hsv_filter_optimized_table +{ + uint8_t v[T_HSV_SIZE][T_HSV_SIZE][T_HSV_SIZE]; +}; + +void +t_hsv_build_convert_table(struct t_hsv_filter_params *params, + struct t_convert_table *t); + +void +t_hsv_build_large_table(struct t_hsv_filter_params *params, + struct t_hsv_filter_large_table *t); + +void +t_hsv_build_optimized_table(struct t_hsv_filter_params *params, + struct t_hsv_filter_optimized_table *t); + +XRT_MAYBE_UNUSED static inline uint8_t +t_hsv_filter_sample(struct t_hsv_filter_optimized_table *t, + uint32_t y, + uint32_t u, + uint32_t v) +{ + return t->v[y / T_HSV_STEP][u / T_HSV_STEP][v / T_HSV_STEP]; +} + +int +t_hsv_filter_create(struct t_hsv_filter_params *params, + struct xrt_frame_sink *sinks[4], + struct xrt_frame_sink **out_sink); + + +/* + * + * Sink creation functions. + * + */ + +int +t_convert_yuv_or_yuyv_create(struct xrt_frame_sink *next, + struct xrt_frame_sink **out_sink); + +int +t_calibration_create(struct xrt_frame_sink *gui, + struct xrt_frame_sink **out_sink); + +int +t_debug_hsv_picker_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink); + +int +t_debug_hsv_viewer_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink); + +int +t_debug_hsv_filter_create(struct xrt_frame_sink *passthrough, + struct xrt_frame_sink **out_sink); + + +#ifdef __cplusplus +} +#endif