Commit 79402d00 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz
Browse files

aux/track: Add stub calibration tracker

parent a2ffb188
Pipeline #57470 passed with stages
in 1 minute and 12 seconds
......@@ -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)
......
......@@ -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()
// 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;
}
// 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);
}
// 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;
}
// 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];