gstreamer/gst/patchdetect/gstpatchdetect.c

1236 lines
30 KiB
C
Raw Normal View History

/* GStreamer
* Copyright (C) 2011 David Schleef <ds@entropywave.com>
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Library General Public
* License as published by the Free Software Foundation; either
* version 2 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Library General Public License for more details.
*
* You should have received a copy of the GNU Library General Public
* License along with this library; if not, write to the
* Free Software Foundation, Inc., 51 Franklin Street, Suite 500,
* Boston, MA 02110-1335, USA.
*/
/**
* SECTION:element-gstpatchdetect
*
* The patchdetect element detects color patches from a color
* calibration chart. Currently, the patches for the 24-square
* Munsell ColorChecker are hard-coded into the element. When
* a color chart is detected in the video stream, a message is
* sent to the bus containing the detected color values of each
* of the patches.
*
* <refsect2>
* <title>Example launch line</title>
* |[
* gst-launch -v dv1394src ! dvdemux ! dvdec ! patchdetect ! xvimagesink
* ]|
* </refsect2>
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gst/gst.h>
#include <gst/base/gstbasetransform.h>
#include <gst/video/video.h>
#include <math.h>
#include <string.h>
#include "gstpatchdetect.h"
GST_DEBUG_CATEGORY_STATIC (gst_patchdetect_debug_category);
#define GST_CAT_DEFAULT gst_patchdetect_debug_category
/* prototypes */
static void gst_patchdetect_set_property (GObject * object,
guint property_id, const GValue * value, GParamSpec * pspec);
static void gst_patchdetect_get_property (GObject * object,
guint property_id, GValue * value, GParamSpec * pspec);
static void gst_patchdetect_dispose (GObject * object);
static void gst_patchdetect_finalize (GObject * object);
static gboolean
gst_patchdetect_get_unit_size (GstBaseTransform * trans, GstCaps * caps,
guint * size);
static gboolean
gst_patchdetect_set_caps (GstBaseTransform * trans, GstCaps * incaps,
GstCaps * outcaps);
static gboolean gst_patchdetect_start (GstBaseTransform * trans);
static gboolean gst_patchdetect_stop (GstBaseTransform * trans);
static gboolean gst_patchdetect_event (GstBaseTransform * trans,
GstEvent * event);
static GstFlowReturn gst_patchdetect_transform_ip (GstBaseTransform * trans,
GstBuffer * buf);
static gboolean gst_patchdetect_src_event (GstBaseTransform * trans,
GstEvent * event);
enum
{
PROP_0
};
/* pad templates */
static GstStaticPadTemplate gst_patchdetect_sink_template =
GST_STATIC_PAD_TEMPLATE ("sink",
GST_PAD_SINK,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (GST_VIDEO_CAPS_YUV ("I420"))
);
static GstStaticPadTemplate gst_patchdetect_src_template =
GST_STATIC_PAD_TEMPLATE ("src",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (GST_VIDEO_CAPS_YUV ("I420"))
);
/* class initialization */
#define DEBUG_INIT(bla) \
GST_DEBUG_CATEGORY_INIT (gst_patchdetect_debug_category, "patchdetect", 0, \
"debug category for patchdetect element");
GST_BOILERPLATE_FULL (GstPatchdetect, gst_patchdetect, GstBaseTransform,
GST_TYPE_BASE_TRANSFORM, DEBUG_INIT);
static void
gst_patchdetect_base_init (gpointer g_class)
{
GstElementClass *element_class = GST_ELEMENT_CLASS (g_class);
gst_element_class_add_pad_template (element_class,
gst_static_pad_template_get (&gst_patchdetect_sink_template));
gst_element_class_add_pad_template (element_class,
gst_static_pad_template_get (&gst_patchdetect_src_template));
gst_element_class_set_static_metadata (element_class, "Color Patch Detector",
"Video/Analysis", "Detects color patches from a color calibration chart",
"David Schleef <ds@entropywave.com>");
}
static void
gst_patchdetect_class_init (GstPatchdetectClass * klass)
{
GObjectClass *gobject_class = G_OBJECT_CLASS (klass);
GstBaseTransformClass *base_transform_class =
GST_BASE_TRANSFORM_CLASS (klass);
gobject_class->set_property = gst_patchdetect_set_property;
gobject_class->get_property = gst_patchdetect_get_property;
gobject_class->dispose = gst_patchdetect_dispose;
gobject_class->finalize = gst_patchdetect_finalize;
base_transform_class->get_unit_size =
GST_DEBUG_FUNCPTR (gst_patchdetect_get_unit_size);
base_transform_class->set_caps = GST_DEBUG_FUNCPTR (gst_patchdetect_set_caps);
base_transform_class->start = GST_DEBUG_FUNCPTR (gst_patchdetect_start);
base_transform_class->stop = GST_DEBUG_FUNCPTR (gst_patchdetect_stop);
base_transform_class->event = GST_DEBUG_FUNCPTR (gst_patchdetect_event);
base_transform_class->transform_ip =
GST_DEBUG_FUNCPTR (gst_patchdetect_transform_ip);
base_transform_class->src_event =
GST_DEBUG_FUNCPTR (gst_patchdetect_src_event);
}
static void
gst_patchdetect_init (GstPatchdetect * patchdetect,
GstPatchdetectClass * patchdetect_class)
{
}
void
gst_patchdetect_set_property (GObject * object, guint property_id,
const GValue * value, GParamSpec * pspec)
{
g_return_if_fail (GST_IS_PATCHDETECT (object));
switch (property_id) {
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
break;
}
}
void
gst_patchdetect_get_property (GObject * object, guint property_id,
GValue * value, GParamSpec * pspec)
{
g_return_if_fail (GST_IS_PATCHDETECT (object));
switch (property_id) {
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
break;
}
}
void
gst_patchdetect_dispose (GObject * object)
{
g_return_if_fail (GST_IS_PATCHDETECT (object));
/* clean up as possible. may be called multiple times */
G_OBJECT_CLASS (parent_class)->dispose (object);
}
void
gst_patchdetect_finalize (GObject * object)
{
g_return_if_fail (GST_IS_PATCHDETECT (object));
/* clean up object here */
G_OBJECT_CLASS (parent_class)->finalize (object);
}
static gboolean
gst_patchdetect_get_unit_size (GstBaseTransform * trans, GstCaps * caps,
guint * size)
{
int width, height;
GstVideoFormat format;
gboolean ret;
ret = gst_video_format_parse_caps (caps, &format, &width, &height);
*size = gst_video_format_get_size (format, width, height);
return ret;
}
static gboolean
gst_patchdetect_set_caps (GstBaseTransform * trans, GstCaps * incaps,
GstCaps * outcaps)
{
GstPatchdetect *patchdetect = GST_PATCHDETECT (trans);
int width, height;
GstVideoFormat format;
gboolean ret;
ret = gst_video_format_parse_caps (incaps, &format, &width, &height);
if (ret) {
patchdetect->format = format;
patchdetect->width = width;
patchdetect->height = height;
}
return ret;
}
static gboolean
gst_patchdetect_start (GstBaseTransform * trans)
{
return TRUE;
}
static gboolean
gst_patchdetect_stop (GstBaseTransform * trans)
{
return TRUE;
}
static gboolean
gst_patchdetect_event (GstBaseTransform * trans, GstEvent * event)
{
return TRUE;
}
typedef struct
{
guint8 *y;
int ystride;
guint8 *u;
int ustride;
guint8 *v;
int vstride;
int width;
int height;
int t;
} Frame;
typedef struct
{
int y, u, v;
int diff_y, diff_u, diff_v;
gboolean match;
int patch_block;
int color;
int count;
int sum_x;
int sum_y;
} Stats;
typedef struct
{
int r, g, b;
int y, u, v;
} Color;
typedef struct
{
int x, y;
int patch1, patch2;
gboolean valid;
} Point;
typedef struct
{
int xmin, xmax;
int ymin, ymax;
int val;
int y, u, v;
int count;
int cen_x, cen_y;
gboolean valid;
} Patch;
static Color patch_colors[24] = {
{115, 82, 68, 92, 119, 143},
{194, 150, 130, 152, 115, 148},
{98, 122, 157, 119, 146, 116},
{87, 108, 67, 102, 112, 120},
{133, 128, 177, 130, 149, 128},
{103, 189, 170, 161, 128, 91},
{214, 126, 44, 135, 83, 170},
{80, 91, 166, 97, 162, 120},
{193, 90, 99, 113, 122, 173},
{94, 60, 108, 77, 146, 141},
{157, 188, 64, 164, 77, 119},
{224, 163, 46, 160, 70, 160},
{56, 61, 150, 73, 168, 122},
{70, 148, 73, 124, 103, 97},
{175, 54, 60, 85, 118, 181},
{231, 199, 31, 182, 51, 149},
{187, 86, 149, 112, 146, 170},
{8, 133, 161, 109, 153, 72},
{243, 243, 243, 225, 128, 128},
{200, 200, 200, 188, 128, 128},
{160, 160, 160, 153, 128, 128},
{122, 122, 122, 121, 128, 128},
{85, 85, 85, 89, 128, 128},
{52, 52, 52, 61, 128, 128}
};
static void
get_block_stats (Frame * frame, int x, int y, Stats * stats)
{
int i, j;
guint8 *data;
int max;
int min;
int sum;
max = 0;
min = 255;
sum = 0;
for (j = 0; j < 8; j++) {
data = frame->y + frame->ystride * (j + y) + x;
for (i = 0; i < 8; i++) {
max = MAX (max, data[i]);
min = MIN (min, data[i]);
sum += data[i];
}
}
stats->y = sum / 64;
stats->diff_y = MAX (max - stats->y, stats->y - min);
max = 0;
min = 255;
sum = 0;
for (j = 0; j < 4; j++) {
data = frame->u + frame->ustride * (j + y / 2) + x / 2;
for (i = 0; i < 4; i++) {
max = MAX (max, data[i]);
min = MIN (min, data[i]);
sum += data[i];
}
}
stats->u = sum / 16;
stats->diff_u = MAX (max - stats->u, stats->u - min);
max = 0;
min = 255;
sum = 0;
for (j = 0; j < 4; j++) {
data = frame->v + frame->vstride * (j + y / 2) + x / 2;
for (i = 0; i < 4; i++) {
max = MAX (max, data[i]);
min = MIN (min, data[i]);
sum += data[i];
}
}
stats->v = sum / 16;
stats->diff_v = MAX (max - stats->v, stats->v - min);
stats->patch_block = -1;
stats->match = FALSE;
#define MATCH 15
if (stats->diff_y < MATCH && stats->diff_u < MATCH && stats->diff_v < MATCH) {
stats->match = TRUE;
}
}
static void
paint_block (Frame * frame, int x, int y, int value)
{
int i, j;
guint8 *data;
for (j = 0; j < 8; j++) {
data = frame->y + frame->ystride * (j + y) + x;
for (i = 0; i < 8; i++) {
data[i] = value;
}
}
for (j = 0; j < 4; j++) {
data = frame->u + frame->ustride * (j + y / 2) + x / 2;
for (i = 0; i < 4; i++) {
data[i] = 128;
}
}
for (j = 0; j < 4; j++) {
data = frame->v + frame->vstride * (j + y / 2) + x / 2;
for (i = 0; i < 4; i++) {
data[i] = 128;
}
}
}
static gboolean
patch_check (Frame * frame, guint8 * patchpix, int x, int y, int w, int h)
{
int i, j;
for (j = y; j < y + h; j++) {
for (i = x; i < x + w; i++) {
if (patchpix[j * frame->width + i] != 0)
return FALSE;
}
}
return TRUE;
}
static void
patch_start (Frame * frame, guint8 * patchpix, Patch * patch, int x, int y,
int w, int h)
{
int i, j;
for (j = y; j < y + h; j++) {
for (i = x; i < x + w; i++) {
patchpix[j * frame->width + i] = patch->val;
}
}
patch->xmin = MAX (1, x - 1);
patch->xmax = MIN (x + w + 1, frame->width - 1);
patch->ymin = MAX (1, y - 1);
patch->ymax = MIN (y + h + 1, frame->height - 1);
patch->count = w * h;
}
static void
patch_grow (Frame * frame, guint8 * patchpix, Patch * patch)
{
gboolean growmore = FALSE;
guint8 *ydata, *udata, *vdata;
int i, j;
int count = 5;
#define MAXDIFF 15
do {
for (j = patch->ymin; j < patch->ymax; j++) {
ydata = frame->y + frame->ystride * j;
udata = frame->u + frame->ustride * (j / 2);
vdata = frame->v + frame->vstride * (j / 2);
for (i = patch->xmin; i < patch->xmax; i++) {
if (patchpix[j * frame->width + i] != 0)
continue;
if (patchpix[(j + 1) * frame->width + i] == patch->val ||
patchpix[(j - 1) * frame->width + i] == patch->val ||
patchpix[j * frame->width + i + 1] == patch->val ||
patchpix[j * frame->width + i - 1] == patch->val) {
int diff = ABS (ydata[i] - patch->y) +
ABS (udata[i / 2] - patch->u) + ABS (vdata[i / 2] - patch->v);
if (diff < MAXDIFF) {
patchpix[j * frame->width + i] = patch->val;
patch->xmin = MIN (patch->xmin, MAX (i - 1, 1));
patch->xmax = MAX (patch->xmax, MIN (i + 2, frame->width - 1));
patch->ymin = MIN (patch->ymin, MAX (j - 1, 1));
patch->ymax = MAX (patch->ymax, MIN (j + 2, frame->height - 1));
patch->count++;
growmore = TRUE;
}
}
}
}
for (j = patch->ymax - 1; j >= patch->ymin; j--) {
ydata = frame->y + frame->ystride * j;
udata = frame->u + frame->ustride * (j / 2);
vdata = frame->v + frame->vstride * (j / 2);
for (i = patch->xmax - 1; i >= patch->xmin; i--) {
if (patchpix[j * frame->width + i] != 0)
continue;
if (patchpix[(j + 1) * frame->width + i] == patch->val ||
patchpix[(j - 1) * frame->width + i] == patch->val ||
patchpix[j * frame->width + i + 1] == patch->val ||
patchpix[j * frame->width + i - 1] == patch->val) {
int diff = ABS (ydata[i] - patch->y) +
ABS (udata[i / 2] - patch->u) + ABS (vdata[i / 2] - patch->v);
if (diff < MAXDIFF) {
patchpix[j * frame->width + i] = patch->val;
patch->xmin = MIN (patch->xmin, MAX (i - 1, 1));
patch->xmax = MAX (patch->xmax, MIN (i + 2, frame->width - 1));
patch->ymin = MIN (patch->ymin, MAX (j - 1, 1));
patch->ymax = MAX (patch->ymax, MIN (j + 2, frame->height - 1));
patch->count++;
growmore = TRUE;
}
}
}
}
count--;
} while (growmore && count > 0);
#if 0
for (j = patch->ymin; j < patch->ymax; j++) {
guint8 *data;
data = frame->y + frame->ystride * j;
for (i = patch->xmin; i < patch->xmax; i++) {
if (patchpix[j * frame->width + i] != patch->val)
continue;
if ((i + j + frame->t) & 0x4) {
data[i] = 16;
}
}
}
#endif
}
#if 0
static void
find_cluster (Point * points, int n_points, int *result_x, int *result_y)
{
int dist;
int ave_x, ave_y;
int i;
for (dist = 50; dist >= 10; dist -= 5) {
int sum_x, sum_y;
int n_valid;
sum_x = 0;
sum_y = 0;
n_valid = 0;
for (i = 0; i < n_points; i++) {
if (!points[i].valid)
continue;
sum_x += points[i].x;
sum_y += points[i].y;
n_valid++;
}
ave_x = sum_x / n_valid;
ave_y = sum_y / n_valid;
for (i = 0; i < n_points; i++) {
int d;
if (!points[i].valid)
continue;
d = (points[i].x - ave_x) * (points[i].x - ave_x);
d += (points[i].y - ave_y) * (points[i].y - ave_y);
if (d > dist * dist)
points[i].valid = FALSE;
}
}
*result_x = ave_x;
*result_y = ave_y;
}
#endif
typedef struct _Matrix Matrix;
struct _Matrix
{
double m[4][4];
};
#if 0
static void
dump_4x4 (double a[4][4], double b[4][4])
{
int j;
int i;
for (j = 0; j < 4; j++) {
g_print ("[ ");
for (i = 0; i < 4; i++) {
g_print ("%8.2g", a[i][j]);
if (i != 4 - 1)
g_print (", ");
}
g_print ("|");
for (i = 0; i < 4; i++) {
g_print ("%8.2g", b[i][j]);
if (i != 4 - 1)
g_print (", ");
}
g_print ("]\n");
}
g_print ("\n");
}
#endif
static void
invert_matrix (double m[10][10], int n)
{
int i, j, k;
double tmp[10][10] = { {0} };
double x;
for (i = 0; i < n; i++) {
tmp[i][i] = 1;
}
for (j = 0; j < n; j++) {
for (k = 0; k < n; k++) {
if (k == j)
continue;
x = m[j][k] / m[j][j];
for (i = 0; i < n; i++) {
m[i][k] -= x * m[i][j];
tmp[i][k] -= x * tmp[i][j];
}
}
x = m[j][j];
for (i = 0; i < n; i++) {
m[i][j] /= x;
tmp[i][j] /= x;
}
}
memcpy (m, tmp, sizeof (tmp));
}
static GstFlowReturn
gst_patchdetect_transform_ip (GstBaseTransform * trans, GstBuffer * buf)
{
GstPatchdetect *patchdetect = GST_PATCHDETECT (trans);
Frame frame;
Point *points;
int i, j;
int blocks_x, blocks_y;
int n_points;
int n_patches;
Patch *patches;
guint8 *patchpix;
int vec1_x, vec1_y;
int vec2_x, vec2_y;
Color detected_colors[24];
gboolean detected = FALSE;
frame.y = GST_BUFFER_DATA (buf);
frame.ystride = gst_video_format_get_row_stride (patchdetect->format,
0, patchdetect->width);
frame.u =
frame.y + gst_video_format_get_component_offset (patchdetect->format, 1,
patchdetect->width, patchdetect->height);
frame.ustride =
gst_video_format_get_row_stride (patchdetect->format, 1,
patchdetect->width);
frame.v =
frame.y + gst_video_format_get_component_offset (patchdetect->format, 2,
patchdetect->width, patchdetect->height);
frame.vstride =
gst_video_format_get_row_stride (patchdetect->format, 2,
patchdetect->width);
frame.width = patchdetect->width;
frame.height = patchdetect->height;
frame.t = patchdetect->t;
patchdetect->t++;
blocks_y = (patchdetect->height & (~7)) / 8;
blocks_x = (patchdetect->width & (~7)) / 8;
patchpix = g_malloc0 (patchdetect->width * patchdetect->height);
patches = g_malloc0 (sizeof (Patch) * 256);
n_patches = 0;
for (j = 0; j < blocks_y; j += 4) {
for (i = 0; i < blocks_x; i += 4) {
Stats block = { 0 };
get_block_stats (&frame, i * 8, j * 8, &block);
patches[n_patches].val = n_patches + 2;
if (block.match) {
if (patch_check (&frame, patchpix, i * 8, j * 8, 8, 8)) {
patch_start (&frame, patchpix, patches + n_patches, i * 8, j * 8, 8,
8);
patches[n_patches].y = block.y;
patches[n_patches].u = block.u;
patches[n_patches].v = block.v;
patch_grow (&frame, patchpix, patches + n_patches);
n_patches++;
g_assert (n_patches < 256);
}
}
}
}
{
int n;
for (n = 0; n < n_patches; n++) {
Patch *patch = &patches[n];
int xsum;
int ysum;
if (patch->count > 10000)
continue;
patch->valid = TRUE;
xsum = 0;
ysum = 0;
for (j = patch->ymin; j < patch->ymax; j++) {
for (i = patch->xmin; i < patch->xmax; i++) {
if (patchpix[j * frame.width + i] != patch->val)
continue;
xsum += i;
ysum += j;
}
}
patch->cen_x = xsum / patch->count;
patch->cen_y = ysum / patch->count;
}
}
points = g_malloc0 (sizeof (Point) * 1000);
n_points = 0;
for (i = 0; i < n_patches; i++) {
for (j = i + 1; j < n_patches; j++) {
int dist_x, dist_y;
if (i == j)
continue;
dist_x = patches[i].cen_x - patches[j].cen_x;
dist_y = patches[i].cen_y - patches[j].cen_y;
if (dist_x < 0) {
dist_x = -dist_x;
dist_y = -dist_y;
}
if (ABS (2 * dist_y) < dist_x && dist_x < 100) {
points[n_points].x = dist_x;
points[n_points].y = dist_y;
points[n_points].valid = TRUE;
points[n_points].patch1 = i;
points[n_points].patch2 = j;
n_points++;
g_assert (n_points < 1000);
}
}
}
{
int dist;
int ave_x = 0, ave_y = 0;
for (dist = 50; dist >= 10; dist -= 5) {
int sum_x, sum_y;
int n_valid;
sum_x = 0;
sum_y = 0;
n_valid = 0;
for (i = 0; i < n_points; i++) {
if (!points[i].valid)
continue;
sum_x += points[i].x;
sum_y += points[i].y;
n_valid++;
}
if (n_valid == 0)
continue;
ave_x = sum_x / n_valid;
ave_y = sum_y / n_valid;
for (i = 0; i < n_points; i++) {
int d;
if (!points[i].valid)
continue;
d = (points[i].x - ave_x) * (points[i].x - ave_x);
d += (points[i].y - ave_y) * (points[i].y - ave_y);
if (d > dist * dist)
points[i].valid = FALSE;
}
}
vec1_x = ave_x;
vec1_y = ave_y;
}
n_points = 0;
for (i = 0; i < n_patches; i++) {
for (j = i + 1; j < n_patches; j++) {
int dist_x, dist_y;
if (i == j)
continue;
dist_x = patches[i].cen_x - patches[j].cen_x;
dist_y = patches[i].cen_y - patches[j].cen_y;
if (dist_y < 0) {
dist_x = -dist_x;
dist_y = -dist_y;
}
if (ABS (2 * dist_x) < dist_y && dist_y < 100) {
points[n_points].x = dist_x;
points[n_points].y = dist_y;
points[n_points].valid = TRUE;
points[n_points].patch1 = i;
points[n_points].patch2 = j;
n_points++;
g_assert (n_points < 1000);
}
}
}
{
int dist;
int ave_x = 0, ave_y = 0;
for (dist = 50; dist >= 10; dist -= 5) {
int sum_x, sum_y;
int n_valid;
sum_x = 0;
sum_y = 0;
n_valid = 0;
for (i = 0; i < n_points; i++) {
if (!points[i].valid)
continue;
sum_x += points[i].x;
sum_y += points[i].y;
n_valid++;
}
if (n_valid == 0)
continue;
ave_x = sum_x / n_valid;
ave_y = sum_y / n_valid;
for (i = 0; i < n_points; i++) {
int d;
if (!points[i].valid)
continue;
d = (points[i].x - ave_x) * (points[i].x - ave_x);
d += (points[i].y - ave_y) * (points[i].y - ave_y);
if (d > dist * dist)
points[i].valid = FALSE;
}
}
vec2_x = ave_x;
vec2_y = ave_y;
}
#if 0
for (i = 0; i < n_points; i++) {
if (!points[i].valid)
continue;
paint_block (&frame, 4 * points[i].x, 240 + 4 * points[i].y, 16);
}
#endif
#if 0
paint_block (&frame, 360, 240, 16);
paint_block (&frame, 360 + vec1_x, 240 + vec1_y, 16);
paint_block (&frame, 360 + vec2_x, 240 + vec2_y, 16);
#endif
{
double m00, m01, m10, m11;
double det;
double v1, v2;
double ave_v1 = 0, ave_v2 = 0;
det = vec1_x * vec2_y - vec1_y * vec2_x;
m00 = vec2_y / det;
m01 = -vec2_x / det;
m10 = -vec1_y / det;
m11 = vec1_x / det;
for (i = 0; i < n_patches - 1; i++) {
int count = 0;
double sum_v1 = 0;
double sum_v2 = 0;
if (!patches[i].valid)
continue;
n_points = 0;
for (j = i + 1; j < n_patches; j++) {
int diff_x = patches[j].cen_x - patches[i].cen_x;
int diff_y = patches[j].cen_y - patches[i].cen_y;
if (!patches[j].valid)
continue;
v1 = diff_x * m00 + diff_y * m01;
v2 = diff_x * m10 + diff_y * m11;
if (v1 > -0.5 && v1 < 5.5 && v2 > -0.5 && v2 < 3.5 &&
ABS (v1 - rint (v1)) < 0.1 && ABS (v2 - rint (v2)) < 0.1) {
sum_v1 += v1 - rint (v1);
sum_v2 += v2 - rint (v2);
count++;
}
}
ave_v1 = sum_v1 / count;
ave_v2 = sum_v2 / count;
if (count > 20) {
int k;
for (j = 0; j < 4; j++) {
for (k = 0; k < 6; k++) {
Stats block;
int xx;
int yy;
xx = patches[i].cen_x + (ave_v1 + k) * vec1_x + (ave_v2 +
j) * vec2_x;
yy = patches[i].cen_y + (ave_v1 + k) * vec1_y + (ave_v2 +
j) * vec2_y;
get_block_stats (&frame, xx - 4, yy - 4, &block);
//GST_ERROR("%d %d: %d %d %d", k, j, block.y, block.u, block.v);
detected_colors[k + j * 6].y = block.y;
detected_colors[k + j * 6].u = block.u;
detected_colors[k + j * 6].v = block.v;
paint_block (&frame, xx - 4, yy - 4, 16);
}
}
detected = TRUE;
#if 0
for (j = i + 1; j < n_patches; j++) {
int diff_x = patches[j].cen_x - patches[i].cen_x;
int diff_y = patches[j].cen_y - patches[i].cen_y;
int xx;
int yy;
if (!patches[j].valid)
continue;
v1 = diff_x * m00 + diff_y * m01;
v2 = diff_x * m10 + diff_y * m11;
if (v1 > -0.5 && v1 < 5.5 && v2 > -0.5 && v2 < 3.5 &&
ABS (v1 - rint (v1)) < 0.1 && ABS (v2 - rint (v2)) < 0.1) {
v1 = rint (v1);
v2 = rint (v2);
xx = patches[i].cen_x + (ave_v1 + v1) * vec1_x + (ave_v2 +
v2) * vec2_x;
yy = patches[i].cen_y + (ave_v1 + v1) * vec1_y + (ave_v2 +
v2) * vec2_y;
paint_block (&frame, patches[j].cen_x, patches[j].cen_y, 128);
paint_block (&frame, xx, yy, 16);
}
}
paint_block (&frame, patches[i].cen_x, patches[i].cen_y, 240);
#endif
break;
}
}
}
#define N 10
if (detected) {
int i, j, k;
int n = N;
double diff = 0;
double matrix[10][10] = { {0} };
double vy[10] = { 0 };
double vu[10] = { 0 };
double vv[10] = { 0 };
double *by = patchdetect->by;
double *bu = patchdetect->bu;
double *bv = patchdetect->bv;
double flip_diff = 0;
for (i = 0; i < 24; i++) {
diff += ABS (detected_colors[i].y - patch_colors[i].y);
diff += ABS (detected_colors[i].u - patch_colors[i].u);
diff += ABS (detected_colors[i].v - patch_colors[i].v);
flip_diff += ABS (detected_colors[23 - i].y - patch_colors[i].y);
flip_diff += ABS (detected_colors[23 - i].u - patch_colors[i].u);
flip_diff += ABS (detected_colors[23 - i].v - patch_colors[i].v);
}
GST_ERROR ("uncorrected error %g (flipped %g)", diff / 24.0,
flip_diff / 24.0);
if (flip_diff < diff) {
for (i = 0; i < 12; i++) {
Color tmp;
tmp = detected_colors[i];
detected_colors[i] = detected_colors[23 - i];
detected_colors[23 - i] = tmp;
}
}
for (i = 0; i < 24; i++) {
int dy = detected_colors[i].y - patch_colors[i].y;
int du = detected_colors[i].u - patch_colors[i].u;
int dv = detected_colors[i].v - patch_colors[i].v;
int py = detected_colors[i].y - 128;
int pu = detected_colors[i].u - 128;
int pv = detected_colors[i].v - 128;
int w = (i < 18) ? 1 : 2;
double z[10];
diff += ABS (dy) + ABS (du) + ABS (dv);
z[0] = 1;
z[1] = py;
z[2] = pu;
z[3] = pv;
z[4] = py * py;
z[5] = py * pu;
z[6] = py * pv;
z[7] = pu * pu;
z[8] = pu * pv;
z[9] = pv * pv;
for (j = 0; j < n; j++) {
for (k = 0; k < n; k++) {
matrix[j][k] += w * z[j] * z[k];
}
vy[j] += w * dy * z[j];
vu[j] += w * du * z[j];
vv[j] += w * dv * z[j];
}
}
invert_matrix (matrix, n);
for (i = 0; i < n; i++) {
by[i] = 0;
bu[i] = 0;
bv[i] = 0;
for (j = 0; j < n; j++) {
by[i] += matrix[i][j] * vy[j];
bu[i] += matrix[i][j] * vu[j];
bv[i] += matrix[i][j] * vv[j];
}
}
//GST_ERROR("a %g %g %g b %g %g %g", ay, au, av, by, bu, bv);
diff = 0;
for (i = 0; i < 24; i++) {
double cy, cu, cv;
double z[10];
int py = detected_colors[i].y - 128;
int pu = detected_colors[i].u - 128;
int pv = detected_colors[i].v - 128;
z[0] = 1;
z[1] = py;
z[2] = pu;
z[3] = pv;
z[4] = py * py;
z[5] = py * pu;
z[6] = py * pv;
z[7] = pu * pu;
z[8] = pu * pv;
z[9] = pv * pv;
cy = 0;
cu = 0;
cv = 0;
for (j = 0; j < n; j++) {
cy += by[j] * z[j];
cu += bu[j] * z[j];
cv += bv[j] * z[j];
}
diff += fabs (patch_colors[i].y - (128 + py - cy));
diff += fabs (patch_colors[i].u - (128 + pu - cu));
diff += fabs (patch_colors[i].v - (128 + pv - cv));
}
GST_ERROR ("average error %g", diff / 24.0);
patchdetect->valid = 3000;
}
if (patchdetect->valid > 0) {
int n = N;
guint8 *u1, *u2;
guint8 *v1, *v2;
double *by = patchdetect->by;
double *bu = patchdetect->bu;
double *bv = patchdetect->bv;
patchdetect->valid--;
u1 = g_malloc (frame.width);
u2 = g_malloc (frame.width);
v1 = g_malloc (frame.width);
v2 = g_malloc (frame.width);
for (j = 0; j < frame.height; j += 2) {
for (i = 0; i < frame.width / 2; i++) {
u1[2 * i + 0] = frame.u[(j / 2) * frame.ustride + i];
u1[2 * i + 1] = u1[2 * i + 0];
u2[2 * i + 0] = u1[2 * i + 0];
u2[2 * i + 1] = u1[2 * i + 0];
v1[2 * i + 0] = frame.v[(j / 2) * frame.vstride + i];
v1[2 * i + 1] = v1[2 * i + 0];
v2[2 * i + 0] = v1[2 * i + 0];
v2[2 * i + 1] = v1[2 * i + 0];
}
for (i = 0; i < frame.width; i++) {
int k;
double z[10];
double cy, cu, cv;
int y, u, v;
int py, pu, pv;
y = frame.y[(j + 0) * frame.ystride + i];
u = u1[i];
v = v1[i];
py = y - 128;
pu = u - 128;
pv = v - 128;
z[0] = 1;
z[1] = py;
z[2] = pu;
z[3] = pv;
z[4] = py * py;
z[5] = py * pu;
z[6] = py * pv;
z[7] = pu * pu;
z[8] = pu * pv;
z[9] = pv * pv;
cy = 0;
cu = 0;
cv = 0;
for (k = 0; k < n; k++) {
cy += by[k] * z[k];
cu += bu[k] * z[k];
cv += bv[k] * z[k];
}
frame.y[(j + 0) * frame.ystride + i] = CLAMP (rint (y - cy), 0, 255);
u1[i] = CLAMP (rint (u - cu), 0, 255);
v1[i] = CLAMP (rint (v - cv), 0, 255);
y = frame.y[(j + 1) * frame.ystride + i];
u = u2[i];
v = v2[i];
py = y - 128;
pu = u - 128;
pv = v - 128;
z[0] = 1;
z[1] = py;
z[2] = pu;
z[3] = pv;
z[4] = py * py;
z[5] = py * pu;
z[6] = py * pv;
z[7] = pu * pu;
z[8] = pu * pv;
z[9] = pv * pv;
cy = 0;
cu = 0;
cv = 0;
for (k = 0; k < n; k++) {
cy += by[k] * z[k];
cu += bu[k] * z[k];
cv += bv[k] * z[k];
}
frame.y[(j + 1) * frame.ystride + i] = CLAMP (rint (y - cy), 0, 255);
u2[i] = CLAMP (rint (u - cu), 0, 255);
v2[i] = CLAMP (rint (v - cv), 0, 255);
}
for (i = 0; i < frame.width / 2; i++) {
frame.u[(j / 2) * frame.ustride + i] = (u1[2 * i + 0] +
u1[2 * i + 1] + u2[2 * i + 0] + u2[2 * i + 1] + 2) >> 2;
frame.v[(j / 2) * frame.vstride + i] = (v1[2 * i + 0] +
v1[2 * i + 1] + v2[2 * i + 0] + v2[2 * i + 1] + 2) >> 2;
}
}
g_free (u1);
g_free (u2);
g_free (v1);
g_free (v2);
}
g_free (points);
g_free (patches);
g_free (patchpix);
return GST_FLOW_OK;
}
static gboolean
gst_patchdetect_src_event (GstBaseTransform * trans, GstEvent * event)
{
return TRUE;
}
static gboolean
plugin_init (GstPlugin * plugin)
{
gst_element_register (plugin, "patchdetect", GST_RANK_NONE,
gst_patchdetect_get_type ());
return TRUE;
}
GST_PLUGIN_DEFINE (GST_VERSION_MAJOR,
GST_VERSION_MINOR,
patchdetect,
"patchdetect element",
plugin_init, VERSION, "LGPL", PACKAGE_NAME, GST_PACKAGE_ORIGIN)