You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
483 lines
11 KiB
483 lines
11 KiB
|
|
|
|
#include <stdint.h>
|
|
#include <mmintrin.h>
|
|
#include <unistd.h>
|
|
#include <math.h>
|
|
#include "config.h"
|
|
#include "gui.h"
|
|
#include "detect.h"
|
|
|
|
extern Detect detect;
|
|
|
|
//
|
|
// C / C++ Wrapper for the thread function
|
|
//
|
|
gpointer _DetectThread (gpointer data) {
|
|
detect.Thread ();
|
|
return NULL;
|
|
};
|
|
|
|
|
|
|
|
Detect::Detect() { // @suppress("Class members should be properly initialized")
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
g_mutex_init (&mutexin);
|
|
g_mutex_init (&muteximage);
|
|
g_mutex_init (&mutex);
|
|
running = 1;
|
|
inFrame.SetSize(64, 64);
|
|
oldFrame.SetSize(64, 64);
|
|
image.SetSize(64, 64);
|
|
imagegtk.SetSize(64, 64);
|
|
thread = NULL;
|
|
thread = g_thread_new("Detect", _DetectThread, NULL);
|
|
objectW = 300;
|
|
objectH = 300;
|
|
objectX = -1;
|
|
objectY = -1;
|
|
posmaxx = 0;
|
|
posmaxy = 0;
|
|
maxx = NULL;
|
|
maxy = NULL;
|
|
detmatrix = NULL;
|
|
detmatrixv2 = NULL;
|
|
autodetecttype = 0;
|
|
autofollowtype = 0;
|
|
};
|
|
|
|
|
|
|
|
Detect::~Detect() {
|
|
running = 0;
|
|
if (thread) {
|
|
g_thread_join (thread);
|
|
thread = NULL;
|
|
}
|
|
};
|
|
|
|
|
|
|
|
int Detect::NewFrame(VideoFrame *newframe) {
|
|
if (newframe == NULL) return -1;
|
|
LockInMutex();
|
|
inFrame.CopyFrom(newframe);
|
|
inFrameNew = 1;
|
|
UnLockInMutex();
|
|
return 0;
|
|
};
|
|
|
|
|
|
//
|
|
// NewFrame: will set new frame
|
|
// Thread: newFrame |------> Find Object --- not found ---> send gui information
|
|
void Detect::Thread() {
|
|
DetectOutput output;
|
|
int errorinputtype = -1, x, y;
|
|
|
|
objectX = -1;
|
|
objectY = -1;
|
|
|
|
while (running) {
|
|
// check for new frame
|
|
LockInMutex();
|
|
if (inFrameNew == 1) {
|
|
inFrameNew = 0;
|
|
|
|
LockMutex(); // lock Config
|
|
|
|
//
|
|
// 1. if no position of the object is known, search brightest
|
|
// object on the inFrame image.
|
|
if (objectX < objectW/2 || objectX > (inFrame.w-objectW/2) ||
|
|
objectY < objectH/2 || objectY > (inFrame.h-objectH/2))
|
|
{
|
|
switch (autodetecttype) {
|
|
case AUTODETECT_BRIGHT:
|
|
InputDetect(&objectX, &objectY);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
//
|
|
// 2. check near the last known position.
|
|
//
|
|
else {
|
|
switch (autofollowtype) {
|
|
case AUTOFOLLOW_BRIGHT:
|
|
InputDetect(&objectX, &objectY);
|
|
break;
|
|
case AUTOFOLLOW_CROSSC:
|
|
InputDetectCrossC(&objectX, &objectY);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
oldFrame.CopyFrom(&inFrame);
|
|
UnLockInMutex();
|
|
|
|
// copy output image for gtk
|
|
LockImageMutex();
|
|
imagegtk.CopyFrom(&image);
|
|
UnLockImageMutex();
|
|
|
|
output.posx = objectX;
|
|
output.posy = objectY;
|
|
output.image = &imagegtk;
|
|
|
|
UnLockMutex(); // unlock Config
|
|
|
|
gdk_threads_add_idle(cb_thread_detect , &output);
|
|
}
|
|
else
|
|
UnLockInMutex();
|
|
|
|
usleep (10000);
|
|
}
|
|
}
|
|
|
|
|
|
void Detect::SetInputSize (int nw, int nh) {
|
|
if (posmaxx < nw) {
|
|
if (maxx != NULL) free (maxx);
|
|
maxx = (float*) malloc (nw * sizeof (float));
|
|
}
|
|
if (posmaxy < nh) {
|
|
if (maxy != NULL) free (maxy);
|
|
maxy = (float*) malloc (nh * sizeof (float));
|
|
}
|
|
|
|
if ((posmaxx * posmaxy) < (nw * nh)) {
|
|
printf ("%s:%d %s reset Object Position (%d,%d) < (%d,%d) detmatrix:%p\n", __FILE__, __LINE__, __FUNCTION__,
|
|
posmaxx, posmaxy, nw, nh, detmatrix);
|
|
|
|
if (detmatrix != NULL) {
|
|
free (detmatrix);
|
|
free (detmatrixv2);
|
|
}
|
|
detmatrix = (float*) malloc (sizeof(float) * nw * nh);
|
|
detmatrixv2 = (uint16_t*) malloc (sizeof(uint16_t) * nw * nh);
|
|
|
|
objectX = nw/2;
|
|
objectY = nw/2;
|
|
posmaxx = nw;
|
|
posmaxy = nh;
|
|
}
|
|
}
|
|
|
|
|
|
#define POWERVAL 10
|
|
// #define DEBUGTIMES 1
|
|
void Detect::InputDetect(int *posx, int *posy) {
|
|
int x, y, i, dx, dy, sy;
|
|
unsigned char *pxs, *pxi;
|
|
int idx, didx;
|
|
struct timeval t1, t2;
|
|
float f;
|
|
|
|
f = get_cycletime(&t1);
|
|
t2 = t1;
|
|
|
|
image.SetSize (objectW, objectH);
|
|
SetInputSize(inFrame.w, inFrame.h);
|
|
pxs = inFrame.data;
|
|
pxi = image.data;
|
|
*posx = 0;
|
|
*posy = 0;
|
|
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d setup memory time needed:%f\n", __FILE__, __LINE__, f);
|
|
#endif
|
|
//
|
|
// maximum brightness
|
|
for (x = 0; x < posmaxx; x++) maxx[x] = 0.0;
|
|
for (y = 0; y < posmaxy; y++) maxy[y] = 0.0;
|
|
for (x = 0; x < inFrame.w; x++)
|
|
for (y = 0; y < inFrame.h; y++) {
|
|
idx = 3*(inFrame.w * y + x);
|
|
maxx[x] += pow(pxs[idx+0] + pxs[idx+1] + pxs[idx+2], POWERVAL);
|
|
maxy[y] += pow(pxs[idx+0] + pxs[idx+1] + pxs[idx+2], POWERVAL);
|
|
}
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d calculated maximum:%f\n", __FILE__, __LINE__, f);
|
|
// select maximum
|
|
#endif
|
|
for (x = 1; x < inFrame.w; x++) if (maxx[x] > maxx[*posx]) *posx = x;
|
|
for (y = 1; y < inFrame.h; y++) if (maxy[y] > maxy[*posy]) *posy = y;
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d selected maximum:%f\n", __FILE__, __LINE__, f);
|
|
#endif
|
|
|
|
// select start corner
|
|
if (*posx < (objectW / 2)) x = 0;
|
|
else if (*posx + (objectW /2) > inFrame.w) x = inFrame.w - objectW;
|
|
else x = ((*posx) - (objectW / 2));
|
|
|
|
if (*posy < (objectH / 2)) sy = 0;
|
|
else if (*posy + (objectH / 2) > inFrame.h) sy = inFrame.h - objectH;
|
|
else sy = ((*posy) - (objectH / 2));
|
|
|
|
for (dx = 0; dx < image.w && x < inFrame.w; x++, dx++)
|
|
for (dy = 0, y = sy; dy < image.h && y < inFrame.h; y++, dy++) {
|
|
idx = 3* (inFrame.w * y + x);
|
|
didx = 3* (image.w * dy + dx);
|
|
// printf ("[%d , %d] --> [%d, %d] idx: %d --> %d Value:%d\n", x, y, dx, dy, idx, didx, pxs[idx+i]);
|
|
for (i = 0; i < 3; i++) pxi[didx+i] = pxs[idx+i];
|
|
}
|
|
|
|
// printf ("%s:%d %s pos: %d,%d \n", __FILE__, __LINE__, __FUNCTION__, *posx, *posy);
|
|
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d copy output:%f\n", __FILE__, __LINE__, f);
|
|
f = get_cycletime(&t2);
|
|
printf ("%s:%d time needed:%f\n", __FILE__, __LINE__, f);
|
|
#endif
|
|
}
|
|
|
|
inline float calc_vector(uint8_t a1, uint8_t a2, uint8_t b1, uint8_t b2, uint8_t c1, uint8_t c2, uint8_t d1, uint8_t d2) {
|
|
return a1 * a2 + b1 * b2 + c1 * c2 + d1 * d2;
|
|
// __m64 m1 = _mm_set_pi16 (a1, b1, c1, d1);
|
|
// __m64 m2 = _mm_set_pi16 (a2, b2, c2, d2);
|
|
// __m64 m3 = _mm_madd_pi16 (m1, m2);
|
|
// _mm_empty ();
|
|
// uint32_t *data = (uint32_t *) &m3;
|
|
// return data[0] + data[1];
|
|
}
|
|
|
|
#define OBJSIZE 50
|
|
#define MAXSHIFT 20
|
|
// #define DEBUGTIMES 1
|
|
void Detect::InputDetectCrossC(int *posx, int *posy) {
|
|
unsigned char *pxi; // input image
|
|
unsigned char *pxd; // destination image
|
|
unsigned char *pxo; // old image
|
|
int inx, iny, oldx, oldy;
|
|
|
|
struct timeval t1, t2;
|
|
int shiftx, shifty, x, y, ini, oldi, desti, mxi, mxx, mxy;
|
|
float f;
|
|
|
|
if (oldFrame.h != inFrame.h || oldFrame.w != inFrame.w || *posx == -1 || *posy == -1) {
|
|
*posx = -1;
|
|
*posy = -1;
|
|
return;
|
|
}
|
|
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
t2 = t1;
|
|
#endif
|
|
|
|
if (objectW > inFrame.w || objectH > inFrame.h) {
|
|
printf ("%s:%d %s objectW,H (%d,%d) > inFrame.W,H (%d, %d)\n", __FILE__, __LINE__, __FUNCTION__,
|
|
objectW, objectH, inFrame.w, inFrame.h);
|
|
*posx = -1;
|
|
*posy = -1;
|
|
return;
|
|
}
|
|
|
|
image.SetSize (objectW, objectH);
|
|
SetInputSize(inFrame.w, inFrame.h);
|
|
pxi = inFrame.data;
|
|
pxo = oldFrame.data;
|
|
pxd = image.data;
|
|
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d setup memory time needed:%f\n", __FILE__, __LINE__, f);
|
|
#endif
|
|
|
|
mxx = mxy = 0;
|
|
for (shifty = 0; shifty < MAXSHIFT; shifty++) {
|
|
// printf ("%s:%d %s shift (%d)\n", __FILE__, __LINE__, __FUNCTION__, shifty);
|
|
mxi = shifty * objectW;
|
|
for (shiftx = 0; shiftx < MAXSHIFT; shiftx++, mxi++) {
|
|
// fixme: help help
|
|
f = 0.0;
|
|
|
|
for (y = 0; y < OBJSIZE; y++) {
|
|
oldx = (*posx)- OBJSIZE/2;
|
|
oldy = (*posy)- OBJSIZE/2 + y;
|
|
oldi = 3 * (oldx + oldFrame.w * oldy);
|
|
|
|
inx = oldx + shiftx - MAXSHIFT/2;
|
|
iny = oldy + shifty - MAXSHIFT/2;
|
|
ini = 3* (inx + inFrame.w * iny);
|
|
|
|
for (x = 0; x < OBJSIZE; x++, oldi += 3, ini += 3, oldx++, inx++) {
|
|
if (oldx >= 0 && oldy >= 0 && oldx < oldFrame.w && oldy <= oldFrame.h &&
|
|
inx >= 0 && inx < inFrame.w && iny >= 0 && iny < inFrame.h) {
|
|
//f += (float)(pxo[oldi+0])*(float)(pxi[ini+0]);
|
|
f += calc_vector( pxo[oldi+0], pxi[ini+0],
|
|
pxo[oldi+3], pxi[ini+3],
|
|
pxo[oldi+6], pxi[ini+6],
|
|
pxo[oldi+9], pxi[ini+9]);
|
|
x+=3; oldi += 9; ini += 9; oldx+=3; inx+=3;
|
|
}
|
|
}
|
|
}
|
|
detmatrix[mxi] = f;
|
|
if (detmatrix[mxx + (mxy * objectW)] < f) {
|
|
mxx = shiftx;
|
|
mxy = shifty;
|
|
}
|
|
}
|
|
}
|
|
|
|
// printf ("%s:%d %s pos (%d,%d) dpos (%d,%d) newpos (%d,%d)\n", __FILE__, __LINE__, __FUNCTION__,
|
|
// *posx, *posy, (mxx-MAXSHIFT/2), (mxy-MAXSHIFT/2), *posx - (mxx-MAXSHIFT/2), *posy - (mxy-MAXSHIFT/2));
|
|
|
|
*posx += (mxx-MAXSHIFT/2);
|
|
*posy += (mxy-MAXSHIFT/2);
|
|
CopyObjectImage (*posx, *posy);
|
|
|
|
#ifdef DEBUGTIMES
|
|
f = get_cycletime(&t1);
|
|
printf ("%s:%d copy output:%f\n", __FILE__, __LINE__, f);
|
|
f = get_cycletime(&t2);
|
|
printf ("%s:%d time needed:%f\n", __FILE__, __LINE__, f);
|
|
#endif
|
|
}
|
|
|
|
|
|
void Detect::CopyObjectImage(int objx, int objy) {
|
|
int x, y, desti, ini;
|
|
unsigned char *pxi; // input image
|
|
unsigned char *pxd; // destination image
|
|
|
|
pxi = inFrame.data;
|
|
pxd = image.data;
|
|
|
|
//
|
|
// copy image data to destination
|
|
for (x = 0; x < objectW; x++) for (y = 0; y < objectH; y++) {
|
|
desti = 3*(x + y * objectW);
|
|
ini = 3*((x + objx - objectW/2) + (y + objy - objectH/2) * inFrame.w);
|
|
if (desti < 0 || desti > objectW*objectH*3) continue;
|
|
if (ini < 0 || ini > inFrame.w*inFrame.h*3) continue;
|
|
pxd[desti+0] = pxi[ini+0];
|
|
pxd[desti+1] = pxi[ini+1];
|
|
pxd[desti+2] = pxi[ini+2];
|
|
}
|
|
}
|
|
|
|
|
|
//
|
|
// set/get the automatic follow mode
|
|
void Detect::SetFollowType(int intype) {
|
|
LockMutex();
|
|
autofollowtype = intype;
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
int Detect::GetFollowType() {
|
|
int i;
|
|
|
|
LockMutex();
|
|
i = autofollowtype;
|
|
UnLockMutex();
|
|
|
|
return i;
|
|
}
|
|
|
|
|
|
// set/get the automatic detection mode
|
|
void Detect::SetDetectType(int intype) {
|
|
LockMutex();
|
|
autodetecttype = intype;
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
int Detect::GetDetectType() {
|
|
int i;
|
|
|
|
LockMutex();
|
|
i = autodetecttype;
|
|
UnLockMutex();
|
|
|
|
return i;
|
|
}
|
|
|
|
|
|
//
|
|
// to set a new size we need to disable the detection for some moments.
|
|
// this will not setup any memory for te calculation buffers
|
|
void Detect::SetObjectSize(int neww, int newh) {
|
|
int orgtype = autodetecttype;
|
|
|
|
// save detection type
|
|
LockMutex();
|
|
autodetecttype = -1;
|
|
UnLockMutex();
|
|
|
|
usleep (500000); // wait 0.5 sec
|
|
|
|
LockImageMutex();
|
|
objectH = newh;
|
|
objectW = neww;
|
|
UnLockImageMutex();
|
|
|
|
// restore orginal detection type
|
|
LockMutex();
|
|
autodetecttype = orgtype;
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
void Detect::GetObjectSize (int *ow, int *oh) {
|
|
LockMutex();
|
|
*ow = objectW;
|
|
*oh = objectH;
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
void Detect::SetMinBrightness(int value) {
|
|
LockMutex();
|
|
minBright = value;
|
|
UnLockMutex();
|
|
}
|
|
|
|
|
|
int Detect::GetMinBrightness() {
|
|
int i;
|
|
|
|
LockMutex();
|
|
i = minBright;
|
|
UnLockMutex();
|
|
|
|
return i;
|
|
}
|
|
|
|
|
|
|
|
void Detect::GetObjectPos(int *x, int *y) {
|
|
LockMutex();
|
|
*x = objectX;
|
|
*y = objectY;
|
|
UnLockMutex();
|
|
}
|
|
|
|
void Detect::SetObjectPos(int x, int y) {
|
|
printf ("%s:%d %s new position:%d,%d\n", __FILE__, __LINE__, __FUNCTION__, x, y);
|
|
|
|
LockMutex();
|
|
image.SetSize (objectW, objectH);
|
|
SetInputSize(inFrame.w, inFrame.h);
|
|
objectX = x;
|
|
objectY = y;
|
|
CopyObjectImage (objectX, objectY);
|
|
UnLockMutex();
|
|
}
|
|
|