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.
486 lines
11 KiB
486 lines
11 KiB
|
|
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <mmintrin.h>
|
|
#include <unistd.h>
|
|
#include <math.h>
|
|
#include <sys/time.h>
|
|
#include "config.h"
|
|
#include "gui.h"
|
|
#include "detect.h"
|
|
#include "convert.h"
|
|
#include "configuration.h" // needed to get V4L formats
|
|
|
|
extern Detect detect;
|
|
extern PosCtl posctl;
|
|
|
|
//
|
|
// 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);
|
|
imageRGBout.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;
|
|
autodetecttype = 0;
|
|
autofollowtype = 0;
|
|
};
|
|
|
|
|
|
|
|
Detect::~Detect() {
|
|
running = 0;
|
|
if (thread) {
|
|
g_thread_join (thread);
|
|
thread = NULL;
|
|
}
|
|
};
|
|
|
|
|
|
|
|
/*
|
|
* copy the framedata into the next thread. Source data must be already mutex locked.
|
|
* raw data will only be copied if the source is not an compressed stream
|
|
*/
|
|
int Detect::CopyNewFrame(VideoFrame *newframe, VideoFrameRaw *rawframe) {
|
|
if (newframe == NULL) return -1;
|
|
LockInMutex();
|
|
|
|
if (rawframe->pixfmt != V4L2_PIX_FMT_MJPEG)
|
|
inRawFrame.CopyFrom(rawframe);
|
|
else
|
|
inRawFrame.Delete();
|
|
|
|
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 doutput;
|
|
struct timeval timestamp;
|
|
double dt;
|
|
int oldX, oldY;
|
|
|
|
objectX = -1;
|
|
objectY = -1;
|
|
doutput.detmatrix = (uint32_t*)malloc (sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);
|
|
gettimeofday (×tamp, NULL);
|
|
|
|
while (running) {
|
|
// check for new frame
|
|
dt = get_cycletime(×tamp);
|
|
LockInMutex();
|
|
if (inFrameNew == 1) {
|
|
inFrameNew = 0;
|
|
|
|
oldX = objectX;
|
|
oldY = objectY;
|
|
|
|
LockMutex(); // lock Config
|
|
|
|
//
|
|
// 1. check near the last known position.
|
|
//
|
|
if (objectX >= objectW/2 && objectX <= (inFrame.w-objectW/2) &&
|
|
objectY >= objectH/2 && objectY <= (inFrame.h-objectH/2))
|
|
{
|
|
switch (autofollowtype) {
|
|
case AUTOFOLLOW_BRIGHT:
|
|
InputDetect(&objectX, &objectY);
|
|
break;
|
|
case AUTOFOLLOW_CROSSC:
|
|
InputDetectCrossC(&objectX, &objectY);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (abs(oldX - objectX) >= DET_MAXSHIFT/5 || abs(oldY - objectY) >= DET_MAXSHIFT/5) {
|
|
objectX = -1;
|
|
objectY = -1;
|
|
}
|
|
}
|
|
|
|
//
|
|
// 2. 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;
|
|
}
|
|
}
|
|
|
|
oldFrame.CopyFrom(&inFrame);
|
|
UnLockInMutex();
|
|
|
|
// copy output image for gtk
|
|
LockImageMutex();
|
|
imageRGBout.CopyFrom(&image);
|
|
if (imageRawout.RectCopyFrom(&inRawFrame, (objectX-objectW/2), (objectY-objectH/2), objectW, objectH) == 0)
|
|
imageRawout.CopyFrom(&imageRGBout);
|
|
UnLockImageMutex();
|
|
|
|
doutput.posx = objectX;
|
|
doutput.posy = objectY;
|
|
doutput.image = &imageRGBout;
|
|
doutput.rawimage = &imageRawout;
|
|
if (detmatrix != NULL)
|
|
memcpy (doutput.detmatrix, detmatrix, sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);
|
|
|
|
posctl.Loop (objectX, objectY, dt);
|
|
|
|
UnLockMutex(); // unlock detect dataset
|
|
|
|
gdk_threads_add_idle(cb_thread_detect , &doutput);
|
|
}
|
|
else
|
|
UnLockInMutex();
|
|
|
|
usleep (10000); // FIXME:: why is it here?
|
|
}
|
|
free (doutput.detmatrix);
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
detmatrix = (uint32_t*) malloc (sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);
|
|
|
|
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;
|
|
|
|
image.SetSize (objectW, objectH);
|
|
SetInputSize(inFrame.w, inFrame.h);
|
|
pxs = inFrame.data;
|
|
pxi = image.data;
|
|
*posx = 0;
|
|
*posy = 0;
|
|
|
|
//
|
|
// 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);
|
|
}
|
|
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;
|
|
|
|
// 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);
|
|
for (i = 0; i < 3; i++) pxi[didx+i] = pxs[idx+i];
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/* returns approx. 8-bit gray value */
|
|
inline uint8_t calc_gray(uint8_t r, uint8_t g, uint8_t b) {
|
|
/* Gray ~ 0.3*R + 0.59*G + 0.11*B -> approx. 0.28125*R + 0.625*G + 0.09375*B */
|
|
return (r >> 2) + (r >> 5) + (g >> 1) + (g >> 4) + (b >> 4) + (b >> 5);
|
|
}
|
|
|
|
/* returns 4-vector product */
|
|
inline uint32_t calc_vector(uint16_t a1, uint16_t a2, uint16_t b1, uint16_t b2, uint16_t c1, uint16_t c2, uint16_t d1, uint16_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 52
|
|
void Detect::InputDetectCrossC(int *posx, int *posy) {
|
|
unsigned char *pxi; // input image
|
|
unsigned char *pxo; // old image
|
|
int inx, iny, oldx, oldy;
|
|
|
|
int shiftx, shifty, x, y, ini, oldi, mxi, mxx, mxy;
|
|
uint32_t sp;
|
|
|
|
if (oldFrame.h != inFrame.h || oldFrame.w != inFrame.w || *posx == -1 || *posy == -1) {
|
|
*posx = -1;
|
|
*posy = -1;
|
|
return;
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
mxx = mxy = 0;
|
|
for (shifty = 0; shifty < DET_MAXSHIFT; shifty++) {
|
|
mxi = shifty * DET_MAXSHIFT;
|
|
for (shiftx = 0; shiftx < DET_MAXSHIFT; shiftx++, mxi++) {
|
|
sp = 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 - DET_MAXSHIFT/2;
|
|
iny = oldy + shifty - DET_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) {
|
|
uint16_t ogray[4];
|
|
uint16_t igray[4];
|
|
ogray[0] = calc_gray( pxo[oldi+0], pxo[oldi+1], pxo[oldi+2]);
|
|
ogray[1] = calc_gray( pxo[oldi+3], pxo[oldi+4], pxo[oldi+5]);
|
|
ogray[2] = calc_gray( pxo[oldi+6], pxo[oldi+7], pxo[oldi+8]);
|
|
ogray[3] = calc_gray( pxo[oldi+9], pxo[oldi+10], pxo[oldi+11]);
|
|
igray[0] = calc_gray( pxi[oldi+0], pxi[ini+1], pxi[ini+2]);
|
|
igray[1] = calc_gray( pxi[oldi+3], pxi[ini+4], pxi[ini+5]);
|
|
igray[2] = calc_gray( pxi[oldi+6], pxi[ini+7], pxi[ini+8]);
|
|
igray[3] = calc_gray( pxi[oldi+9], pxi[ini+10], pxi[ini+11]);
|
|
sp += calc_vector( ogray[0], igray[0],
|
|
ogray[1], igray[1],
|
|
ogray[2], igray[2],
|
|
ogray[3], igray[3]);
|
|
x+=3; oldi += 9; ini += 9; oldx+=3; inx+=3;
|
|
}
|
|
}
|
|
}
|
|
detmatrix[mxi] = sp;
|
|
if (detmatrix[mxx + (mxy * DET_MAXSHIFT)] < sp) {
|
|
mxx = shiftx;
|
|
mxy = shifty;
|
|
}
|
|
}
|
|
}
|
|
|
|
*posx += (mxx-DET_MAXSHIFT/2);
|
|
*posy += (mxy-DET_MAXSHIFT/2);
|
|
CopyObjectImage (*posx, *posy);
|
|
return;
|
|
}
|
|
|
|
|
|
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();
|
|
}
|
|
|