t_hsv_filter.c 8.41 KB
Newer Older
1
2
3
4
5
6
7
8
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  A simple HSV filter.
 * @author Jakob Bornecrantz <jakob@collabora.com>
 */

9
#include "util/u_var.h"
10
11
#include "util/u_misc.h"
#include "util/u_debug.h"
12
#include "util/u_frame.h"
13
14
15
16
17
#include "util/u_format.h"

#include "tracking/t_tracking.h"

#include <stdio.h>
18
#include <assert.h>
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

#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;
136
	struct xrt_frame_node node;
137
138
139
140
141

	struct xrt_frame_sink *sinks[NUM_CHANNELS];

	struct t_hsv_filter_params params;

142
143
144
145
	struct xrt_frame *frame0;
	struct xrt_frame *frame1;
	struct xrt_frame *frame2;
	struct xrt_frame *frame3;
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160

	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);
161
162
163
164
165
166
167
168
169
	uint8_t v0 = (bits & (1 << 0)) ? 0xff : 0x00;
	uint8_t v1 = (bits & (1 << 1)) ? 0xff : 0x00;
	uint8_t v2 = (bits & (1 << 2)) ? 0xff : 0x00;
	uint8_t v3 = (bits & (1 << 3)) ? 0xff : 0x00;

	*dst0 = v0;
	*dst1 = v1;
	*dst2 = v2;
	*dst3 = v3;
170
171
}

172
XRT_NO_INLINE static void
173
174
process_frame_yuv(struct t_hsv_filter *f, struct xrt_frame *xf)
{
175
176
177
178
179
	struct xrt_frame *f0 = f->frame0;
	struct xrt_frame *f1 = f->frame1;
	struct xrt_frame *f2 = f->frame2;
	struct xrt_frame *f3 = f->frame3;

180
181
	for (uint32_t y = 0; y < xf->height; y++) {
		uint8_t *src = (uint8_t *)xf->data + y * xf->stride;
182
183
184
185
		uint8_t *dst0 = f0->data + y * f0->stride;
		uint8_t *dst1 = f1->data + y * f1->stride;
		uint8_t *dst2 = f2->data + y * f2->stride;
		uint8_t *dst3 = f3->data + y * f3->stride;
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201

		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;
		}
	}
}

202
XRT_NO_INLINE static void
203
204
process_frame_yuyv(struct t_hsv_filter *f, struct xrt_frame *xf)
{
205
206
207
208
209
	struct xrt_frame *f0 = f->frame0;
	struct xrt_frame *f1 = f->frame1;
	struct xrt_frame *f2 = f->frame2;
	struct xrt_frame *f3 = f->frame3;

210
211
	for (uint32_t y = 0; y < xf->height; y++) {
		uint8_t *src = (uint8_t *)xf->data + y * xf->stride;
212
213
214
215
		uint8_t *dst0 = f0->data + y * f0->stride;
		uint8_t *dst1 = f1->data + y * f1->stride;
		uint8_t *dst2 = f2->data + y * f2->stride;
		uint8_t *dst3 = f3->data + y * f3->stride;
216
217
218
219
220
221
222
223

		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;

224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
			uint8_t bits0 =
			    t_hsv_filter_sample(&f->table, y1, cb, cr);
			uint8_t bits1 =
			    t_hsv_filter_sample(&f->table, y2, cb, cr);

			uint8_t v0 = (bits0 & (1 << 0)) ? 0xff : 0x00;
			uint8_t v1 = (bits0 & (1 << 1)) ? 0xff : 0x00;
			uint8_t v2 = (bits0 & (1 << 2)) ? 0xff : 0x00;
			uint8_t v3 = (bits0 & (1 << 3)) ? 0xff : 0x00;
			uint8_t v4 = (bits1 & (1 << 0)) ? 0xff : 0x00;
			uint8_t v5 = (bits1 & (1 << 1)) ? 0xff : 0x00;
			uint8_t v6 = (bits1 & (1 << 2)) ? 0xff : 0x00;
			uint8_t v7 = (bits1 & (1 << 3)) ? 0xff : 0x00;

			*(uint16_t *)dst0 = v0 | v4 << 8;
			*(uint16_t *)dst1 = v1 | v5 << 8;
			*(uint16_t *)dst2 = v2 | v6 << 8;
			*(uint16_t *)dst3 = v3 | v7 << 8;

			dst0 += 2;
			dst1 += 2;
			dst2 += 2;
			dst3 += 2;
247
248
249
250
251
252
253
		}
	}
}

static void
ensure_buf_allocated(struct t_hsv_filter *f, struct xrt_frame *xf)
{
254
255
	uint32_t w = xf->width;
	uint32_t h = xf->height;
256

257
258
259
260
	u_frame_create_one_off(XRT_FORMAT_L8, w, h, &f->frame0);
	u_frame_create_one_off(XRT_FORMAT_L8, w, h, &f->frame1);
	u_frame_create_one_off(XRT_FORMAT_L8, w, h, &f->frame2);
	u_frame_create_one_off(XRT_FORMAT_L8, w, h, &f->frame3);
261
262
263
}

static void
264
265
266
267
push_buf(struct t_hsv_filter *f,
         struct xrt_frame *xf,
         struct xrt_frame_sink *xsink,
         struct xrt_frame **frame)
268
269
{
	if (xsink == NULL) {
270
		xrt_frame_reference(frame, NULL);
271
272
273
		return;
	}

274
275
276
277
	(*frame)->timestamp = xf->timestamp;
	(*frame)->source_id = xf->source_id;
	(*frame)->source_sequence = xf->source_sequence;
	(*frame)->source_timestamp = xf->source_timestamp;
278

279
	xsink->push_frame(xsink, *frame);
280

281
	xrt_frame_reference(frame, NULL);
282
283
284
285
286
287
288
289
290
}

static void
push_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf)
{
	struct t_hsv_filter *f = (struct t_hsv_filter *)xsink;


	switch (xf->format) {
291
292
293
294
295
296
297
298
	case XRT_FORMAT_YUV888:
		ensure_buf_allocated(f, xf);
		process_frame_yuv(f, xf);
		break;
	case XRT_FORMAT_YUV422:
		ensure_buf_allocated(f, xf);
		process_frame_yuyv(f, xf);
		break;
299
300
301
302
303
304
	default:
		fprintf(stderr, "ERROR: Bad format '%s'",
		        u_format_str(xf->format));
		return;
	}

305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
	push_buf(f, xf, f->sinks[0], &f->frame0);
	push_buf(f, xf, f->sinks[1], &f->frame1);
	push_buf(f, xf, f->sinks[2], &f->frame2);
	push_buf(f, xf, f->sinks[3], &f->frame3);

	assert(f->frame0 == NULL);
	assert(f->frame1 == NULL);
	assert(f->frame2 == NULL);
	assert(f->frame3 == NULL);
}

static void
break_apart(struct xrt_frame_node *node)
{}

static void
destroy(struct xrt_frame_node *node)
{
	struct t_hsv_filter *f = container_of(node, struct t_hsv_filter, node);
324
	u_var_remove_root(f);
325
	free(f);
326
327
328
}

int
329
330
t_hsv_filter_create(struct xrt_frame_context *xfctx,
                    struct t_hsv_filter_params *params,
331
332
333
334
335
                    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->base.push_frame = push_frame;
336
337
338
	f->node.break_apart = break_apart;
	f->node.destroy = destroy;
	f->params = *params;
339
340
341
342
343
344
345
	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);

346
347
	xrt_frame_context_add(xfctx, &f->node);

348
349
350
351
352
353
	u_var_add_root(f, "HSV Filter", true);
	u_var_add_sink(f, &f->sinks[0], "Red");
	u_var_add_sink(f, &f->sinks[1], "Purple");
	u_var_add_sink(f, &f->sinks[2], "Blue");
	u_var_add_sink(f, &f->sinks[3], "White");

354
355
356
357
	*out_sink = &f->base;

	return 0;
}