JD2022-TU1/main/extern/Camcam/HeadDetector/Viola2.cpp

512 lines
15 KiB
C++

#include "memory.h"
#include "HeadDetector.h"
#include "math.h"
#include "stdlib.h"
#include "Viola2.h"
static u32 LEARN_BASE = 1;
#define LEARN_BASE_X 32
#define LEARN_BASE_Y 16
#define WINDOW_X 2
#define WINDOW_Y 2
#define DELTA_X 8
#define DELTA_Y 0
typedef struct GC_C_
{
float X,Y;
float XC,YC,Div;
u32 PITCH;
u32 Counter;
u32 Or;
} GC_C;
#define RAIDUS 8
#define RAIDUSD 5
#define BORDER_LIMIT (RAIDUS + 1)
#define MAX_IMAGE_SIZE 320
void GET_BLOBCENTER(GC_C *GG,s32 *Pixel,s32 And,s32 AddX,s32 AddY)
{
Pixel += AddX + AddY * GG->PITCH;
if (!(*Pixel & And)) return;
GG->Or |= *Pixel;
GG->Counter += *Pixel>>8;
GG->X += AddX;
GG->Y += AddY;
float Weight;
Weight = *Pixel & 0xff;
Weight *= Weight ;
GG->XC += GG->X * Weight;
GG->YC += GG->Y * Weight;
GG->Div += Weight;
*Pixel &= ~And;
GET_BLOBCENTER(GG,Pixel,And,+1,0);
GET_BLOBCENTER(GG,Pixel,And,-1,0);
GET_BLOBCENTER(GG,Pixel,And,+0,+1);
GET_BLOBCENTER(GG,Pixel,And,-0,-1);
GET_BLOBCENTER(GG,Pixel,And,+1,+1);
GET_BLOBCENTER(GG,Pixel,And,-1,+1);
GET_BLOBCENTER(GG,Pixel,And,+1,-1);
GET_BLOBCENTER(GG,Pixel,And,-1,-1);
GG->X -= AddX;
GG->Y -= AddY;
}
POINT *pExternPOINTS = NULL;
u32 ulExternPOINTSNum;
u32 ulExternPOINTSNumMAX;
u32 FEAT_ColorToLearn;
u32 ASM_IsFeatureColor(u32 Color);
void COPY_BW_FeatureColor(MAP *MapSRC,MAP *MapDST)
{
s32 *LAST,*DST,*SRC;
DST = MapDST->GetBase();
SRC = MapSRC->GetBase();
for (u32 SizeY = 0; SizeY < MapDST->SY;SizeY ++)
{
LAST = DST + MapDST->SX;
while (DST < LAST)
{
u32 A;
if (ASM_IsFeatureColor(*(SRC)))
A = ((*(SRC+1) & 0xff0000) + ((*(SRC+1) & 0xff00)<<9) + ((*(SRC+1) & 0xff)<<16))>>(16);
else
A = ((*(SRC) & 0xff0000) + ((*(SRC) & 0xff00)<<9) + ((*(SRC) & 0xff)<<16))>>(16);
*(DST) = A;
DST++;
SRC++;
}
SRC+=MapSRC->PITCH-MapSRC->SX;
DST+=MapDST->PITCH-MapDST->SX;
}
}
void MAP32_NormalizeBW(MAP *MapBW,u32 One,u32 Border)
{
s32 *LAST,*SRC;
float Summ,SqrSumm,Number;
Number = Summ = SqrSumm = 0;
if (Border > MapBW->SX >> 1) Border = MapBW->SX >> 1;
if (Border > MapBW->SY >> 1) Border = MapBW->SY >> 1;
#define InerRadius Border
for (u32 SizeY = InerRadius; SizeY < MapBW->SY - InerRadius;SizeY ++)
{
SRC = MapBW->GetBase() + SizeY * MapBW->PITCH + InerRadius;
LAST = SRC + MapBW->SX - InerRadius * 2;
while (SRC < LAST)
{
Summ += *SRC;
SqrSumm += *SRC * *SRC;
Number ++;
SRC++;
}
}
if (Number == 0) return;
float Sub,Mul,Add;
Summ /= Number;
SqrSumm /= Number;
Sub = Summ ;// Average
Mul = ((float)One/4.0f) / sqrtf(SqrSumm - Summ*Summ); // Eq type
Add = One / 2.0f; // Recenter
for (u32 SizeY = 0; SizeY < MapBW->SY ;SizeY ++)
{
SRC = MapBW->GetBase() + SizeY * MapBW->PITCH ;
LAST = SRC + MapBW->SX ;
while (SRC < LAST)
{
float Local = ((float)*SRC - Sub) * Mul + Add;
*SRC = Local;
SRC++;
}
}
}
//#pragma optimize("",off)
void Viola2_Exec(void *FEAT_Base,MAP *CurrentMap,u32 LearnMode,u32 BlurM = 2)
{
static u32 LC = 0;
u32 WBC;
static s32 *WorkingBuf[6] = {NULL,NULL,NULL,NULL,NULL,NULL};
static u32 WorkingBufSize = 0;
if (!FEAT_Base) return;
if (CurrentMap->SX * CurrentMap->SY == 0) return;
LEARN_BASE = LearnMode;
MAP CurrentMapVirt;
CurrentMapVirt = *CurrentMap;
if ((!LEARN_BASE)&&(CurrentMapVirt.SX > MAX_IMAGE_SIZE)&&(CurrentMapVirt.SX < 700))
{
float FACTOR;
FACTOR = (float)MAX_IMAGE_SIZE / CurrentMapVirt.SX;
CurrentMapVirt.SX *=FACTOR;
CurrentMapVirt.SY *=FACTOR;
COPY_OX_RGBA(CurrentMap,&CurrentMapVirt);
}
CurrentMap = &CurrentMapVirt;
/**************** Init working maps **************/
MAP M1,M2,M3,M4,M5,M6;
if (WorkingBufSize < 512 + (CurrentMap->SX + 16)* CurrentMap->SY)
{
WBC = 6;
WorkingBufSize = 512 + (CurrentMap->SX + 16)* CurrentMap->SY;
while (WBC--)
{
if(WorkingBuf[WBC]) free(WorkingBuf[WBC]);
WorkingBuf[WBC] = (s32*)malloc(WorkingBufSize * 4 + CurrentMap->PITCH * 16 * 4);
memset(WorkingBuf[WBC],0,WorkingBufSize * 4);
}
}
M1 = M2 = M3 = M4 = M5 = M6 = *CurrentMap;
M6.PITCH = M5.PITCH = M4.PITCH = M3.PITCH = M2.PITCH = M1.PITCH = CurrentMap->SX;
M1.SetBase(WorkingBuf[0]);
M2.SetBase(WorkingBuf[1]);
M3.SetBase(WorkingBuf[2]);
M4.SetBase(WorkingBuf[3]);
M5.SetBase(WorkingBuf[4]);
M6.SetBase(WorkingBuf[5]);
if (LEARN_BASE)
COPY_BW_FeatureColor(CurrentMap,&M1);
else
{
COPY_BW(CurrentMap,&M1);
MAP32_NormalizeBW(&M1,4096,2);
}
SHIFT(&M1,2);
//if (LEARN_BASE) BlurM = 4;
if (BlurM) LF_GaussianFilter(&M1,&M1,&M3,0,BlurM);
LF_GaussianFilter(&M1,&M1,&M3,0,3);
LF_GaussianFilter(&M1,&M2,&M3,0,3);
MAX_CROSS(&M1,&M5);
MAX(&M5,&M3);
MIN_CROSS(&M1,&M5);
MIN(&M5,&M4);
SET(&M5,0);
//////////////////////////////////////////////////
// First -> Compute neighbourought flags
{
for (u32 y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
{
for (u32 x = BORDER_LIMIT +1; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
{
s32 *VMa,*VMi,VA;
u32 RES, RESM;
RES =RESM= 0;
VMi = M4.GetBase() + x + (y) * M4.PITCH;
VMa = M3.GetBase() + x + (y) * M3.PITCH;
VA = *(M2.GetBase() + x + (y) * M2.PITCH) ;
{
if ((*(VMi - RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 1;
if ((*(VMi - RAIDUS * M2.PITCH)) > VA) RES |= 2;
if ((*(VMi + RAIDUSD - RAIDUSD * M2.PITCH)) > VA) RES |= 4;
if ((*(VMi + RAIDUS )) > VA) RES |= 8;
if ((*(VMi + RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 16;
if ((*(VMi + RAIDUS * M2.PITCH)) > VA) RES |= 32;
if ((*(VMi - RAIDUSD + RAIDUSD * M2.PITCH)) > VA) RES |= 64;
if ((*(VMi - RAIDUS )) > VA) RES |= 128;
*(M5.GetBase() + x + y * M5.PITCH) = RES | ((*VMa - *VMi) << 8);
}
*(M2.GetBase() + x + y * M5.PITCH) = RES | ((*VMa - *VMi) << 8);
}
}
}
COPY(&M5,&M6);
//////////////////////////////////////////////////
// Second -> Compute non - symetry in flags
{
for (u32 y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
{
for (u32 x = BORDER_LIMIT +1; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
{
u32 RES,RESM,RESE;
RES =RESM= 0;
RES = *(M5.GetBase() + x + (y) * M5.PITCH) & 0xff;
*(M5.GetBase() + x + y * M5.PITCH) = *(M2.GetBase() + x + y * M2.PITCH) & 0xff00;
*(M2.GetBase() + x + y * M2.PITCH) &= 0xff;
if (GetBitNumber(RES) > 7)
{
*(M5.GetBase() + x + y * M5.PITCH) |= 0x4;
}
else
{
RESE = RES >> 1;
RESE |= RES << 7;
RES &= RESE;
RESE = RES >> 1;
RESE |= RES << 7;
RES &= RESE;//*/
if (GetBitNumber(RES) > 3)
{
*(M5.GetBase() + x + y * M5.PITCH) |= 0x2;
} else
if (GetBitNumber(RES) > 2)
{
*(M5.GetBase() + x + y * M5.PITCH) |= 0x1;
}
}
}
}
}
//////////////////////////////////////////////////
// Third -> extract "BLOB" centers
/* for (int y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
{
for (int x = BORDER_LIMIT+1 ; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
{
s32 *VMa;
GC_C GG;
VMa = M5.GetBase() + x + (y) * M5.PITCH;
if (*VMa)
{
GG.Div = GG.XC = GG.YC = GG.X = GG.Y = 0.0f;
GG.Counter = 0;GG.Or = 0;
GG.PITCH = M5.PITCH;
GET_BLOBCENTER(&GG,M5.GetBase(),7,x,y);
if ((GG.Counter > 10) )
{
//////////////////////////////////////////////////
// GG contain the new point.
// M6 contain the originals flags (used in fern)
// M5 contain the non - symetric flags (could be also used in fern - to test)
POINT P;
P.x = GG.XC / GG.Div;
P.y = GG.YC / GG.Div;
*( M6.GetBase() + P.x + P.y * M6.PITCH ) |= 0x10000;
}
}
}
}
//////////////////////////////////////////////////
// fourth -> extract "BLOB" centers
for (int y = 0; y < CurrentMap->SY; y++)
for (int x = 0; x < CurrentMap->SX; x++)
*(CurrentMap->GetBase() + x + y * CurrentMap->PITCH) &= 0xFfffff;
//*/
ORb(&M6,&M5);
ORb(&M5,&M2);
FEAT_OptimizeForImage(FEAT_Base,&M2);
for (u32 y = BORDER_LIMIT +1; y < CurrentMap->SY - BORDER_LIMIT-1; y++)
{
MAP *FeatureLMap;
FeatureLMap = &M2;
for (u32 x = BORDER_LIMIT+1 ; x < CurrentMap->SX - BORDER_LIMIT-1; x++)
{
s32 *VMa;
VMa = M6.GetBase() + x + (y) * M6.PITCH;
{
POINT P;
P.x = x;
P.y = y;
//////////////////////////////////////////////////
if (LEARN_BASE)
{
if (*(CurrentMap->GetBase() + P.x + P.y * CurrentMap->PITCH) == FEAT_ColorToLearn)
{
if ((P.x >= 8) && (P.y >= 8))
if ((P.x < FeatureLMap->SX - 8) && (P.y < FeatureLMap->SY - 8))
{
u32 INp;
*(CurrentMap->GetBase() + P.x + (P.y) * CurrentMap->PITCH) = *(CurrentMap->GetBase() + P.x + P.y * CurrentMap->PITCH + 1);
#define FX 1
#define FY 1
INp = 0;
for (int GX = -FX ; GX <= FX ; GX ++)
for (int GY = -FY ; GY <= FY ; GY ++)
INp |= FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
if (!INp)
for (int GX = -FX ; GX <= FX ; GX ++)
for (int GY = -FY ; GY <= FY ; GY ++)
FEAT_AddBase( FEAT_Base,
FeatureLMap->GetBase() + P.x + GX + (P.y + GY) * FeatureLMap->PITCH ,
FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH ,
FeatureLMap->PITCH ,
LEARN_BASE);
LC++;
}
}
} else
if ((*VMa & 0x10000) || (pExternPOINTS == NULL))
{
u32 INp;
INp = FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
if (pExternPOINTS == NULL)
{
if (INp)
*((u32*)(CurrentMap->GetBase() + P.x + (P.y) * CurrentMap->PITCH)) |= INp<<24;
else
*(CurrentMap->GetBase() + P.x + (P.y) * CurrentMap->PITCH) &= ~0xFf000000;
} else
{
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x +1 + P.y * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x -1 + P.y * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x + (P.y+1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x + (P.y+1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x +1 + (P.y+1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x -1 + (P.y+1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x +1 + (P.y-1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
INp += FEAT_IsIn(FEAT_Base,FeatureLMap->GetBase() + P.x -1 + (P.y-1) * FeatureLMap->PITCH ,FeatureLMap->GetBase() + P.x + P.y * FeatureLMap->PITCH , FeatureLMap->PITCH );
if (INp >= 5)
{
if (ulExternPOINTSNum < ulExternPOINTSNumMAX)
{
pExternPOINTS[ulExternPOINTSNum++] = P;
}
}
}
}
}
}
}
}
static u32 *MapSave = NULL;
static u32 MapSaveSize = 0;
u32 Feature_DETECT(MAP *CurrentMap,POINT *pDst,u32 MAXNUMBER,u32 Base)
{
static void* FEATBASE_EYE = NULL;
static void* FEATBASE_NOSE = NULL;
static void* FEATBASE_MOUTH = NULL;
static void* FEATBASE_MOUTH_UP = NULL;
static void* FEATBASE_MOUTH_DOWN = NULL;
void* FEATBASE = NULL;
switch (Base)
{
default:
case 0:
if (FEATBASE_EYE == NULL) FEATBASE_EYE = FEAT_LoadSaveBase(0);
FEATBASE = FEATBASE_EYE;
break;
case 1:
if (FEATBASE_MOUTH == NULL) FEATBASE_MOUTH = FEAT_LoadSaveBase(1);
FEATBASE = FEATBASE_MOUTH;
break;
case 2:
if (FEATBASE_NOSE == NULL) FEATBASE_NOSE = FEAT_LoadSaveBase(2);
FEATBASE = FEATBASE_NOSE;
break;
case 3:
if (FEATBASE_MOUTH_UP == NULL) FEATBASE_MOUTH_UP = FEAT_LoadSaveBase(3);
FEATBASE = FEATBASE_MOUTH_UP;
break;
case 4:
if (FEATBASE_MOUTH_DOWN == NULL) FEATBASE_MOUTH_DOWN = FEAT_LoadSaveBase(4);
FEATBASE = FEATBASE_MOUTH_DOWN;
break;
}
if (FEATBASE == NULL) return 0;
pExternPOINTS = pDst;
ulExternPOINTSNumMAX = MAXNUMBER;
ulExternPOINTSNum = 0;
Viola2_Exec(FEATBASE,CurrentMap,0);
pExternPOINTS = NULL;
ulExternPOINTSNumMAX = 0;
return ulExternPOINTSNum;
}
void Viola2(MAP *CurrentMap,u32 LearnMode)
{
u32 ScaleNegCounter = 6;
MAP CurrentMapVirt ;
static void *FEAT_Base;
/*
Feature_DETECT(CurrentMap,NULL,0,0);
return;
//*/
if (LearnMode)
{
if (FEAT_Base == NULL)
FEAT_Base = FEAT_NewBase();
if (CurrentMap->PITCH * CurrentMap->SY * 4 > MapSaveSize)
{
if (MapSave) free(MapSave);
MapSaveSize = CurrentMap->PITCH * CurrentMap->SY * 4 + CurrentMap->PITCH * 2;
MapSave = (u32 *) malloc(MapSaveSize);
}
memcpy ( MapSave , CurrentMap->GetBase() , CurrentMap->PITCH * CurrentMap->SY * 4 ) ;
}
float FACTOR = 1;
CurrentMapVirt = *CurrentMap;
while (ScaleNegCounter--)
{
if (LearnMode >= 100)
{
if (MapSave) free(MapSave);
MapSave = NULL;
MapSaveSize = 0;
FEAT_FinishLearn(FEAT_Base);
FEAT_LoadSaveBase(LearnMode-100,FEAT_Base);
FEAT_Base = NULL;
LearnMode = 0;
}
if (LearnMode == 1)
{
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);
/*FLIP_H(&CurrentMapVirt);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);*/
} else
{
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);
if (MapSave) memcpy ( CurrentMapVirt.GetBase() , MapSave , CurrentMapVirt.PITCH * CurrentMapVirt.SY * 4 ) ;
COPY_OX_RGBA(CurrentMap,&CurrentMapVirt);//*/
}
if (LearnMode == 2)
{
u32 NoiseC;
NoiseC = 1;
while (NoiseC--)
{
FLIP_V(&CurrentMapVirt);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,5);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,8);
FLIP_H(&CurrentMapVirt);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,5);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,8);
FLIP_V(&CurrentMapVirt);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,5);
Viola2_Exec(FEAT_Base,&CurrentMapVirt,LearnMode,8);
if (MapSave) memcpy ( CurrentMapVirt.GetBase() , MapSave , CurrentMapVirt.PITCH * CurrentMapVirt.SY * 4 ) ;
COPY_OX_RGBA(CurrentMap,&CurrentMapVirt);//*/
COPY_BW(&CurrentMapVirt,&CurrentMapVirt);
SHIFT(&CurrentMapVirt,2);
ADD_NOISE(&CurrentMapVirt);
COPY_RGBA(&CurrentMapVirt,&CurrentMapVirt);
}
}//*/
if (LearnMode == 2)
{
CurrentMapVirt.SX *= 0.8f;
CurrentMapVirt.SY *= 0.8f;
if (MapSave) memcpy ( CurrentMapVirt.GetBase() , MapSave , CurrentMapVirt.PITCH * CurrentMapVirt.SY * 4 ) ;
COPY_OX_RGBA(CurrentMap,&CurrentMapVirt);//*/
} else
ScaleNegCounter = 0;
}
}