aux/track: Add stub calibration tracker

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)

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()

src/xrt/auxiliary/tracking/t_calibration.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Calibration code.
+ * @author Pete Black <pblack@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ */
+
+#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 <opencv2/opencv.hpp>
+
+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;
+}

src/xrt/auxiliary/tracking/t_convert.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Code to build conversion tables and convert images.
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ */
+
+#include "tracking/t_tracking.h"
+
+#include <opencv2/opencv.hpp>
+
+
+/*
+ *
+ * '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);
+}

src/xrt/auxiliary/tracking/t_debug_hsv_filter.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  HSV filter debug code.
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ */
+
+#include "util/u_misc.h"
+#include "util/u_debug.h"
+#include "util/u_format.h"
+#include "tracking/t_tracking.h"
+
+#include <opencv2/opencv.hpp>
+
+
+/*
+ *
+ * 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(&params, sinks, &d.sink);
+
+	return 0;
+}

src/xrt/auxiliary/tracking/t_debug_hsv_picker.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  HSV Picker Debugging code.
+ * @author Pete Black <pblack@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ */
+
+#include "util/u_misc.h"
+#include "util/u_debug.h"
+#include "util/u_format.h"
+
+#include "tracking/t_tracking.h"
+
+#include <opencv2/opencv.hpp>
+
+
+/*
+ *
+ * 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<uint8_t>(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];