819 lines
24 KiB
C++
819 lines
24 KiB
C++
// HeadDetector.cpp : Defines the entry point for the DLL application.
|
|
//
|
|
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <memory.h>
|
|
#include <string.h>
|
|
#include "HeadDetector.h"
|
|
#include "HAAR.h"
|
|
#include "MAP_SpFunctions.h"
|
|
#include "Viola2.h"
|
|
#include "BottomFaceShapeExtraction.h"
|
|
|
|
/****************************************************/
|
|
u32 HAAR_Detect(MAP *pSrcMAP,HAAR_Classifier *pHFF,MAP *pDrawMAP,float Factor,float gAddX,float gAddY,u32 AGE,float Angle, MAP8_Quad *MAP8_QuadStack, u32 *MAP8_QuadStackNumber, MAP8_Quad *pBlendQuad = NULL);
|
|
u32 MAP8Q_IsIn(MAP8_Quad *p1,MAP8_Quad *p2);
|
|
int __cdecl TRACKEDPOINT_FACE_Compare(const void *elem1, const void *elem2 );
|
|
|
|
void DrawCross(MAP *image, Vector *v, u32 color, int len, char orient = 3);
|
|
u32 ASM_GetCenterG(MAP *pSrc,s32 X1,s32 Y1,s32 X2,s32 Y2,POINT *pCenter);
|
|
void ASM_DrawFace(MAP *pSrc,DETECTEDFace *pDF);
|
|
void ASM_DetectFeatures(MAP *pFeatureMap, MAP *pSourceMap, MAP *pSourceMapDRW, DETECTEDFace *pLocalDF);
|
|
|
|
//#pragma optimize("",off)
|
|
|
|
/****************************************************/
|
|
/**** DEFINES **************************/
|
|
#define ANGLE_TRACK
|
|
|
|
#define FMDMS_FM (85)
|
|
#define FMDMS FMDMS_FM
|
|
#define GROW_FACTOR (ulNumberOfHeads ? 1.10 : 1.04)
|
|
#define BASE_FACTOR 2.0
|
|
#define START_WindowX 30
|
|
#define ASM_COMMPUTE_SHAPE_SIZEMAP 84
|
|
#define WINDOW_SUM_FACTOR_II 0.5f
|
|
|
|
#define ANGLE_FACTOR_TRAK (3.1415927f / 12.0f)
|
|
#define GROW_FACTOR_TRAK 0.10f
|
|
|
|
#ifdef ANGLE_TRACK
|
|
#define GROW_TRAK_WINDOW 0.1f
|
|
#else
|
|
#define GROW_TRAK_WINDOW 0.2f
|
|
#endif
|
|
|
|
|
|
#ifdef _MANAGED
|
|
#pragma managed(push, off)
|
|
#endif
|
|
|
|
#ifdef HEADDETECTOR_DLL_MODE
|
|
BOOL APIENTRY DllMain( HMODULE hModule,
|
|
DWORD ul_reason_for_call,
|
|
LPVOID lpReserved
|
|
)
|
|
{
|
|
switch (ul_reason_for_call)
|
|
{
|
|
case DLL_PROCESS_ATTACH:
|
|
case DLL_THREAD_ATTACH:
|
|
case DLL_THREAD_DETACH:
|
|
case DLL_PROCESS_DETACH:
|
|
break;
|
|
}
|
|
return TRUE;
|
|
}
|
|
#endif
|
|
|
|
#ifdef _MANAGED
|
|
#pragma managed(pop)
|
|
#endif
|
|
|
|
|
|
HeadDetector::HeadDetector(void)
|
|
{
|
|
memset(this, 0, sizeof(*this));
|
|
}
|
|
|
|
void HeadDetector::CopyAsync()
|
|
{
|
|
memcpy(ASYNC_ASM_AllDF, ASM_AllDF ,sizeof(ASYNC_ASM_AllDF));
|
|
ASYNC_ulNumberOfHeads = ulNumberOfHeads;
|
|
}
|
|
|
|
u32 HeadDetector::GetNumberOfFace()
|
|
{
|
|
HeadTrackerRequested = 4;
|
|
return ASYNC_ulNumberOfHeads;
|
|
}
|
|
void HeadDetector::DrawDetectedFaces(u32 Enable)
|
|
{
|
|
HeadTrackerRequested_DRAW = Enable;
|
|
}
|
|
u32 HeadDetector::GetFace(u32 INdex, Vector *Center,Vector *Size)
|
|
{
|
|
if (INdex >= ASYNC_ulNumberOfHeads) return 0;
|
|
*Center = ASYNC_ASM_AllDF[INdex].HeadPos;
|
|
*Size = ASYNC_ASM_AllDF[INdex].HeadSize;
|
|
return 1;
|
|
}
|
|
u32 HeadDetector::GetFaceFeature(u32 i, u32 FeatureIndexn, Vector *Center)
|
|
{
|
|
if (i >= ASYNC_ulNumberOfHeads) return 0;
|
|
switch(FeatureIndexn)
|
|
{
|
|
case 0: *Center = ASYNC_ASM_AllDF[i].RightEye ; return (u32)ASYNC_ASM_AllDF[i].RightEye .z;break;
|
|
case 1: *Center = ASYNC_ASM_AllDF[i].LeftEye ; return (u32)ASYNC_ASM_AllDF[i].LeftEye .z;break;
|
|
case 2: *Center = ASYNC_ASM_AllDF[i].Nose ; return (u32)ASYNC_ASM_AllDF[i].Nose .z;break;
|
|
case 3: *Center = ASYNC_ASM_AllDF[i].RightMouse ; return (u32)ASYNC_ASM_AllDF[i].RightMouse .z;break;
|
|
case 4: *Center = ASYNC_ASM_AllDF[i].LeftMouse ; return (u32)ASYNC_ASM_AllDF[i].LeftMouse .z;break;
|
|
case 5: *Center = ASYNC_ASM_AllDF[i].UpMouse ; return (u32)ASYNC_ASM_AllDF[i].UpMouse .z;break;
|
|
case 6: *Center = ASYNC_ASM_AllDF[i].DownMouse ; return (u32)ASYNC_ASM_AllDF[i].DownMouse .z;break;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void HeadDetector::SetDrawImage(MAP *pImgDraw)
|
|
{
|
|
pSrcDRAW = pImgDraw;
|
|
//HeadTrackerRequested_DRAW = pImgDraw != NULL;
|
|
}
|
|
|
|
|
|
#include <crtdbg.h>
|
|
|
|
int HeadDetector::ProcessImage(MAP *pImg)
|
|
{
|
|
pSrc = pImg;
|
|
|
|
MAP SRC2,Local,LocalO2;
|
|
static s32 *TEMPA = NULL;
|
|
static s32 *TEMPB = NULL;
|
|
static s32 TEMPASize = 0;
|
|
|
|
if (!HeadTrackerRequested)
|
|
{
|
|
ulNumberOfHeads = ulOldNumberOfHeads = 0;
|
|
return 0;
|
|
}
|
|
HeadTrackerRequested--;
|
|
|
|
float CurrentFactor = 1.0f;
|
|
u32 PreviousMAP8_QuadStackNumber = MAP8_QuadStackNumber;
|
|
MAP8_QuadStackNumber = 0;
|
|
|
|
static MAP BMP;
|
|
static u32 Bfirst = 1;
|
|
|
|
if (pHFF && *((u32*)&pHFF) != pSrc->PITCH)
|
|
{
|
|
HAAR_Classifier_free(&pHFFOptimized);
|
|
pHFF = NULL;
|
|
}
|
|
if (pHFF == NULL)
|
|
{
|
|
pHFF = HAAR_Create_FaceFront();
|
|
pHFFOptimized = HAAR_Optimize(pHFF,CurrentFactor,pSrc->PITCH);
|
|
HAAR_Classifier_free(&pHFF);
|
|
*((u32*)&pHFF) = pSrc->PITCH;
|
|
}
|
|
|
|
if (512 + (pSrc->SY + 32) * (pSrc->PITCH + 32) * 4 > TEMPASize)
|
|
{
|
|
if (TEMPA) free(TEMPA);
|
|
if (TEMPB) free(TEMPB);
|
|
TEMPASize = 512 + pSrc->SY * (pSrc->PITCH + 16) * 4;
|
|
TEMPASize = 512 + (pSrc->SY + 32) * (pSrc->PITCH + 32) * 4;
|
|
TEMPA = (s32 *)malloc(TEMPASize);
|
|
TEMPB = (s32 *)malloc(TEMPASize);
|
|
}
|
|
|
|
|
|
Local = *pSrc; Local.staticBase = 1;
|
|
Local.SetBase(TEMPA);
|
|
SRC2 = *pSrc; SRC2.staticBase = 1;
|
|
SRC2.SetBase(TEMPB);
|
|
|
|
COPY(pSrc,&SRC2);
|
|
/* Track Existing quads ************************************************************************************/
|
|
memmove(
|
|
MAP8_QuadStack+MAX_MAP8_QUAD-PreviousMAP8_QuadStackNumber,
|
|
MAP8_QuadStack,PreviousMAP8_QuadStackNumber*sizeof(MAP8_Quad));
|
|
for (int tq = MAX_MAP8_QUAD-PreviousMAP8_QuadStackNumber ; tq < MAX_MAP8_QUAD ; tq++)
|
|
{
|
|
MAP ClmapDst,ClmapDst2,ClmapDst3,ClmapSrc;
|
|
float Factor,WX,WY,WW,WH,OX,OY;
|
|
if (MAP8_QuadStack[tq].W)
|
|
{
|
|
MAP8_Quad SaveQ = MAP8_QuadStack[tq];
|
|
#ifdef ANGLE_TRACK
|
|
for (int An = -1 ; An < 2 ; An++)
|
|
#endif
|
|
{
|
|
for (int Fr = -1 ; Fr < 2 ; Fr++)
|
|
{
|
|
WX = MAP8_QuadStack[tq].X1;
|
|
WY = MAP8_QuadStack[tq].Y1;
|
|
WW = MAP8_QuadStack[tq].X2;
|
|
WH = MAP8_QuadStack[tq].Y2;
|
|
WW -= WX;
|
|
WH -= WY;
|
|
|
|
WX += WW*0.5f;
|
|
WY += WH*0.5f;
|
|
OX = WX;
|
|
OY = WY;
|
|
Factor = 1.0f + (float)Fr * GROW_FACTOR_TRAK;
|
|
WW *= Factor;
|
|
WH *= Factor;
|
|
WX -= WW*0.5f;
|
|
WY -= WH*0.5f;
|
|
|
|
Factor = pHFFOptimized->WindowSize.X / (WW);
|
|
WX -= GROW_TRAK_WINDOW * WW;
|
|
WY -= GROW_TRAK_WINDOW * WH;
|
|
WW += GROW_TRAK_WINDOW * 2.0f * WW;
|
|
WH += GROW_TRAK_WINDOW * 2.0f * WH;
|
|
if (WX<0.0f) WX=0.0f;
|
|
if (WY<0.0f) WY=0.0f;
|
|
ClmapDst = Local; ClmapDst.staticBase = 1;
|
|
ClmapSrc = *pSrc; ClmapSrc.staticBase = 1;
|
|
if (WX + WW > pSrc->SX) WX = pSrc->SX - WW ;
|
|
if (WY + WH > pSrc->SY) WY = pSrc->SY - WH ;
|
|
ClmapSrc.MoveBase((u32)WX + (u32)WY * ClmapSrc.PITCH);
|
|
ClmapSrc.SX = WW;
|
|
ClmapSrc.SY = WH;
|
|
ClmapDst.SX = WW * Factor;
|
|
ClmapDst.SY = WH * Factor;
|
|
#ifndef ANGLE_TRACK
|
|
COPY_OX_RGBA(&ClmapSrc,&ClmapDst);
|
|
RGBA_2_II(&ClmapDst,&ClmapDst);
|
|
HAAR_Detect(&ClmapDst,pHFFOptimized,pSrc,1.0f / Factor,WX,WY,MAP8_QuadStack[tq].Age+1,0, MAP8_QuadStack, &MAP8_QuadStackNumber);
|
|
#else
|
|
ClmapDst3 = ClmapDst2 = ClmapDst;
|
|
ClmapDst2.SetBase(SRC2.GetBase());
|
|
ClmapDst2.SX <<= 1;
|
|
ClmapDst2.SY <<= 1;
|
|
ClmapDst.SX <<= 1;
|
|
ClmapDst.SY <<= 1;
|
|
COPY_OX_RGBA(&ClmapSrc,&ClmapDst2);
|
|
COPY_PATCH_RGBA(
|
|
&ClmapDst2,
|
|
&ClmapDst,
|
|
ClmapDst2.SX<<7,ClmapDst2.SY<<7,
|
|
0,0,
|
|
ClmapDst2.SX<<8,ClmapDst2.SY<<8,
|
|
ClmapDst2.SX<<8,ClmapDst2.SY<<8,
|
|
MAP8_QuadStack[tq].Angle + (float)An * ANGLE_FACTOR_TRAK,1);//*/
|
|
ClmapDst2 = ClmapDst;
|
|
ClmapDst.SX >>= 1;
|
|
ClmapDst.SY >>= 1;
|
|
COPY_O2_RGBA(&ClmapDst2,&ClmapDst);
|
|
|
|
COPY_BW(&ClmapDst,&ClmapDst);
|
|
SHIFT(&ClmapDst,2);
|
|
COPY_RGBA(&ClmapDst,&ClmapDst);
|
|
|
|
RGBA_2_II(&ClmapDst,&ClmapDst);
|
|
HAAR_Detect(&ClmapDst,pHFFOptimized,pSrc,1.0f / Factor,WX,WY,MAP8_QuadStack[tq].Age+1,MAP8_QuadStack[tq].Angle + (float)An * ANGLE_FACTOR_TRAK, MAP8_QuadStack, &MAP8_QuadStackNumber, &SaveQ);
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
/* find RAW quads ******************************************************************************************/
|
|
u32 Counter = 250;//;
|
|
float CurrentFactorPo2 = BASE_FACTOR;
|
|
while (Counter--)
|
|
{
|
|
LocalO2 = Local;
|
|
MAP *pSrcLocal;
|
|
pSrcLocal = &SRC2;
|
|
CurrentFactorPo2 *= GROW_FACTOR;
|
|
LocalO2.SX = pSrc->SX / CurrentFactorPo2;
|
|
LocalO2.SY = pSrc->SY / CurrentFactorPo2;
|
|
if ((LocalO2.SX < SRC2.SX>>1) && (LocalO2.SY < SRC2.SY>>1))
|
|
{
|
|
MAP SRC3;
|
|
SRC3 = SRC2; SRC3.staticBase = 1;
|
|
SRC2.SX >>= 1;
|
|
SRC2.SY >>= 1;
|
|
COPY_O2_RGBA(&SRC3,&SRC2);
|
|
}
|
|
|
|
if ((pHFFOptimized->WindowSize.X < LocalO2.SX) &&
|
|
(pHFFOptimized->WindowSize.Y < LocalO2.SY))
|
|
{
|
|
if (START_WindowX < pHFFOptimized->WindowSize.X * CurrentFactorPo2)
|
|
{
|
|
COPY_OX_RGBA(pSrcLocal,&LocalO2);
|
|
RGBA_2_II(&LocalO2,&LocalO2);
|
|
HAAR_Detect(&LocalO2,pHFFOptimized,pSrc,CurrentFactorPo2,0.0f,0.0f,0,0,MAP8_QuadStack, &MAP8_QuadStackNumber);
|
|
}
|
|
} else
|
|
Counter = 0;
|
|
Local = LocalO2;
|
|
}
|
|
|
|
_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
|
|
if (!Bfirst) COPY(&BMP,pSrc);
|
|
_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
|
|
|
|
ulOldNumberOfHeads = ulNumberOfHeads;
|
|
ulNumberOfHeads = 0;
|
|
/* Merge computed quads */
|
|
if (MAP8_QuadStackNumber)
|
|
{
|
|
qsort(MAP8_QuadStack,MAP8_QuadStackNumber,sizeof(MAP8_Quad),TRACKEDPOINT_FACE_Compare);
|
|
|
|
MAP8_Quad *pBaseQ,*pLastQ,*pParseQ;
|
|
pLastQ = MAP8_QuadStack + MAP8_QuadStackNumber;
|
|
pBaseQ = MAP8_QuadStack;
|
|
while (pBaseQ < pLastQ)
|
|
{
|
|
if (pBaseQ->W != 0.0f)
|
|
{
|
|
pParseQ = pBaseQ+1;
|
|
while (pParseQ < pLastQ)
|
|
{
|
|
// recover ?
|
|
if (MAP8Q_IsIn(pBaseQ,pParseQ))
|
|
{
|
|
// -> Merge
|
|
if (pBaseQ->Age > pParseQ->Age)
|
|
*pBaseQ = *pBaseQ;
|
|
else
|
|
if (pBaseQ->Age < pParseQ->Age)
|
|
*pBaseQ = *pParseQ;
|
|
else
|
|
{
|
|
pBaseQ->X1 += pParseQ->X1;
|
|
pBaseQ->Y1 += pParseQ->Y1;
|
|
pBaseQ->X2 += pParseQ->X2;
|
|
pBaseQ->Y2 += pParseQ->Y2;
|
|
pBaseQ->Angle += pParseQ->Angle;
|
|
pBaseQ->W += pParseQ->W ;
|
|
pBaseQ->Age = pBaseQ->Age > pParseQ->Age ? pBaseQ->Age : pParseQ->Age;
|
|
}
|
|
pParseQ->W = 0.0f;
|
|
}
|
|
pParseQ++;
|
|
}
|
|
}
|
|
pBaseQ++;
|
|
}//*/
|
|
|
|
pBaseQ = MAP8_QuadStack;
|
|
_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
|
|
while (pBaseQ < pLastQ)
|
|
{
|
|
|
|
if (pBaseQ -> W > 1.0f)
|
|
{
|
|
|
|
pBaseQ ->X1 /= pBaseQ->W;
|
|
pBaseQ ->Y1 /= pBaseQ->W;
|
|
pBaseQ ->X2 /= pBaseQ->W;
|
|
pBaseQ ->Y2 /= pBaseQ->W;
|
|
pBaseQ ->Angle /= pBaseQ->W;
|
|
pBaseQ->W = 1;
|
|
if (pBaseQ->Age >= 0)
|
|
{
|
|
DETECTEDFace LocalDF;
|
|
LocalDF.BaseHead = *pBaseQ;
|
|
Local = *pSrc; Local.staticBase = 1;
|
|
Local.SetBase(TEMPA);
|
|
_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
|
|
ASM_DetectFeatures(&Local, pSrc, pSrcDRAW, &LocalDF);
|
|
_ASSERTE( _CrtIsValidHeapPointer(pSrc->GetBase()));
|
|
if (ulNumberOfHeads < MAX_HEAD_DETECTED)
|
|
{
|
|
/* Retreive old one (if exist)*/
|
|
ASM_AllDF[ulNumberOfHeads++] = LocalDF;
|
|
}
|
|
}
|
|
}
|
|
pBaseQ++;
|
|
}
|
|
};
|
|
|
|
for (int i=0 ; i < ulOldNumberOfHeads ; i++)
|
|
{
|
|
ASM_AllDF[i].HeadPos.x *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].HeadPos.y *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].HeadSize.x *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].HeadSize.y *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].LeftEye *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].RightEye *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].LeftMouse *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].RightMouse *= pSrcDRAW->SX / pSrc->SX;
|
|
ASM_AllDF[i].Nose *= pSrcDRAW->SX / pSrc->SX;
|
|
}
|
|
if (HeadTrackerRequested_DRAW)
|
|
for (int i=0 ; i < ulNumberOfHeads ; i++)
|
|
ASM_DrawFace(pSrcDRAW,&ASM_AllDF[i]);
|
|
|
|
return ulNumberOfHeads;
|
|
}
|
|
|
|
|
|
|
|
/*******************************************************************************************/
|
|
/***** FUNCTIONS ***************************************************************************/
|
|
/*******************************************************************************************/
|
|
|
|
#define TURN_POINT() \
|
|
{\
|
|
float SaveX = (float)Center.x - (float)(pFeatureMap->SX>>1);\
|
|
float SaveY = (float)Center.y - (float)(pFeatureMap->SY>>1);\
|
|
Center.x = (float)(pFeatureMap->SX>>1) + SaveX * cosf(-pLocalDF->BaseHead.Angle) + SaveY * sinf(-pLocalDF->BaseHead.Angle);\
|
|
Center.y = (float)(pFeatureMap->SY>>1) - SaveX * sinf(-pLocalDF->BaseHead.Angle) + SaveY * cosf(-pLocalDF->BaseHead.Angle);\
|
|
};\
|
|
|
|
void RecenterFace(DETECTEDFace *pLocalDF)
|
|
{
|
|
if ((pLocalDF->LeftEye.z!=0) && (pLocalDF->RightEye.z!=0))
|
|
{
|
|
Vector Distance;
|
|
float Dis;
|
|
Distance = pLocalDF->LeftEye-pLocalDF->RightEye;
|
|
Dis = Distance.NormXY();
|
|
pLocalDF->HeadPos = (pLocalDF->LeftEye+pLocalDF->RightEye) * 0.5f;
|
|
pLocalDF->HeadPos.x -= Distance.y * 0.5f;
|
|
pLocalDF->HeadPos.y -= Distance.x * 0.5f;
|
|
pLocalDF->HeadPos.z = pLocalDF->BaseHead.Angle = asinf(Distance.y / Dis);
|
|
pLocalDF->HeadSize.x = pLocalDF->HeadSize.y = Dis*2.4;
|
|
}
|
|
}
|
|
|
|
|
|
u32 ASM_IsFeatureColor(u32 Color)
|
|
{
|
|
u8 R,G,B;
|
|
u8 MAX,MIN;
|
|
MIN = MAX = R = (u8)((Color >> 16) & 0xff);
|
|
G = (u8)((Color >> 8) & 0xff);
|
|
B = (u8)((Color >> 0) & 0xff);
|
|
if (G < MIN) MIN = G;
|
|
if (B < MIN) MIN = B;
|
|
if (G > MAX) MAX = G;
|
|
if (B > MAX) MAX = B;
|
|
if (MAX-MIN > 100)
|
|
{
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
#define ASM8_SCLALAR float
|
|
ASM8_SCLALAR ASM8_Feature(s32 *Ptr,HAAR_Feature *pFeatures,u32 Num,ASM8_SCLALAR WindowSum)
|
|
{
|
|
ASM8_SCLALAR All;
|
|
HAAR_Feature *pFeaturesLast;
|
|
s32 *Ptr2;
|
|
All = 0;
|
|
pFeaturesLast = pFeatures + Num;
|
|
while (pFeatures < pFeaturesLast)
|
|
{
|
|
ASM8_SCLALAR BigSumm;
|
|
HAAR_RECT *pFeat;
|
|
pFeat = pFeatures->Feat;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
BigSumm = (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
|
|
pFeat++;
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
BigSumm += (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
|
|
pFeat++;
|
|
if (pFeat->W)
|
|
{
|
|
Ptr2 = Ptr+pFeat->Offset0;
|
|
BigSumm += (ASM8_SCLALAR)pFeat->W * (ASM8_SCLALAR)(*(Ptr2) - *(Ptr2+pFeat->Offset1) - *(Ptr2+pFeat->Offset2) + *(Ptr2+pFeat->Offset1+pFeat->Offset2));
|
|
}
|
|
BigSumm *= HAAR_SCALAR_MULT ;
|
|
BigSumm = (ASM8_SCLALAR)pFeatures->Thresh * ((ASM8_SCLALAR)WindowSum) - BigSumm;
|
|
All += pFeatures->LeftRight[*(u32 *)&BigSumm>>31];
|
|
pFeatures++;
|
|
}
|
|
return All;
|
|
}
|
|
|
|
|
|
u32 ASM8_DETECT(s32 *Ptr,HAAR_Classifier *p_HAAR,ASM8_SCLALAR WindowSumm )
|
|
{
|
|
u32 REturnV;
|
|
HAAR_Stage *p_Current;
|
|
p_Current = p_HAAR->p_Stages;
|
|
REturnV = 0;
|
|
u32 StCounter = 0;
|
|
while (p_Current)
|
|
{
|
|
float Res;
|
|
Res = (float)ASM8_Feature(Ptr,p_Current->pStageFeatures,p_Current->ulNumberOfFeatures,WindowSumm);
|
|
REturnV = (*(u32*)&Res)>>31;
|
|
p_Current = p_Current->pNext[REturnV];
|
|
if ((StCounter++) > 20)
|
|
p_Current = NULL;
|
|
}
|
|
return REturnV^1;
|
|
};
|
|
|
|
|
|
u32 MAP8Q_IsIn(MAP8_Quad *p1,MAP8_Quad *p2)
|
|
{
|
|
float X,Y,W;
|
|
W = p1->W / p2->W;
|
|
X = (p2->X1 + p2->X2) * 0.5f * W;
|
|
Y = (p2->Y1 + p2->Y2) * 0.5f * W;
|
|
if ((X > p1->X1) &&(Y > p1->Y1) &&(X < p1->X2) &&(Y < p1->Y2) )return 1;
|
|
return 0;
|
|
}
|
|
|
|
int __cdecl TRACKEDPOINT_FACE_Compare(const void *elem1, const void *elem2 )
|
|
{
|
|
if (((MAP8_Quad *)elem2)->X1 < ((MAP8_Quad *)elem1)->X1)
|
|
return -1;
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
|
|
void DrawCross(MAP *image, Vector *v, u32 color, int len, char orient)
|
|
{
|
|
if (!v) return;
|
|
POINT p = { v->x, v->y };
|
|
DrawCross(image, p, color, len, orient);
|
|
}
|
|
|
|
void ASM_DrawFace(MAP *pSrc,DETECTEDFace *pDF)
|
|
{
|
|
DrawQuad_R(pSrc,pDF->HeadPos.x - pDF->HeadSize.x * 0.375f,pDF->HeadPos.y - pDF->HeadSize.y * 0.375f,
|
|
pDF->HeadPos.x + pDF->HeadSize.x * 0.375f,pDF->HeadPos.y + pDF->HeadSize.y * 0.375f,
|
|
0xFF0000 ,3,pDF->HeadPos.z);//*/
|
|
if (pDF->RightEye.z) DrawCross(pSrc,&pDF->RightEye,0xff00ff00,4);
|
|
if (pDF->Nose.z) DrawCross(pSrc,&pDF->Nose,0xff00ff00,4);
|
|
if (pDF->RightMouse.z) DrawCross(pSrc,&pDF->RightMouse,0xff00ff00,4);
|
|
if (pDF->LeftEye.z) DrawCross(pSrc,&pDF->LeftEye,0xff00ff00,4);
|
|
if (pDF->LeftMouse.z) DrawCross(pSrc,&pDF->LeftMouse,0xff00ff00,4);//*/
|
|
}
|
|
|
|
|
|
u32 ASM_GetCenterG(MAP *pSrc,s32 X1,s32 Y1,s32 X2,s32 Y2,POINT *pCenter)
|
|
{
|
|
u32 Weight = 0;
|
|
pCenter->x = pCenter->y = 0;
|
|
for (int X = X1 ; X < X2 ;X++)
|
|
for (int Y = Y1 ; Y < Y2 ;Y++)
|
|
{
|
|
u32 D;
|
|
D = *((u32*)pSrc->GetBase() + X + Y * pSrc->PITCH)>>24;
|
|
if (D)
|
|
{
|
|
pCenter->x += X * D;
|
|
pCenter->y += Y * D;
|
|
Weight += D;
|
|
}
|
|
}
|
|
if (Weight)
|
|
{
|
|
pCenter->x /= Weight;
|
|
pCenter->y /= Weight;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void ASM_DetectFeatures(MAP *pFeatureMap, MAP *pSourceMap, MAP *pSourceMapDRW, DETECTEDFace *pLocalDF)
|
|
{
|
|
POINT A,B;
|
|
DETECTEDFace SaveDf;
|
|
u32 SX,SY;
|
|
MAP8_Quad SBH;
|
|
MAP *pFeatureMapSave;
|
|
pFeatureMapSave = pFeatureMap;
|
|
SaveDf = *pLocalDF;
|
|
memset((void*)pLocalDF,0,sizeof(*pLocalDF));
|
|
SBH = pLocalDF->BaseHead = SaveDf.BaseHead;
|
|
pLocalDF->DeadTime = SaveDf.DeadTime;
|
|
|
|
A.x = (SBH.X1 / SBH.W);
|
|
A.y = SBH.Y1 / SBH.W;
|
|
B.x = SBH.X2 / SBH.W;
|
|
B.y = SBH.Y2 / SBH.W;
|
|
SX = B.x - A.x;
|
|
SY = B.y - A.y;
|
|
A.x += SX / 10;
|
|
B.x += SX / 10;
|
|
pLocalDF->HeadPos.x = (A.x + B.x) * 0.5f;
|
|
pLocalDF->HeadPos.y = (A.y + B.y) * 0.5f;
|
|
pLocalDF->HeadPos.z = pLocalDF->BaseHead.Angle;
|
|
pLocalDF->HeadSize.x = (B.x - A.x);
|
|
pLocalDF->HeadSize.y = (B.y - A.y);
|
|
pLocalDF->HeadSize.z = 0.0f;
|
|
|
|
COPY_PATCH_RGBA(pSourceMap,pFeatureMap,(SX<<7) + (A.x<<8),(SY<<7) + (A.y<<8),0,0,SX<<8,SY<<8,FMDMS<<8,FMDMS<<8,pLocalDF->BaseHead.Angle,1);
|
|
|
|
MAP FeatureMap;
|
|
|
|
FeatureMap = *pFeatureMap;
|
|
FeatureMap.SetBase(pFeatureMap->GetBase()) ;
|
|
FeatureMap.SX = FMDMS;
|
|
FeatureMap.SY = FMDMS;
|
|
pFeatureMap = &FeatureMap;
|
|
AND(pFeatureMap,0xffffff);
|
|
POINT Center;
|
|
s32 DeltaH;
|
|
s32 DeltaV;
|
|
/* Eyes Detection *****************************************************************************************/
|
|
DeltaH = ((FMDMS>>1) - (FMDMS>>3))>>2;
|
|
DeltaV = ((FMDMS>>1) - (FMDMS>>3))>>2;
|
|
s32 EyeFinder[4],MCFinder[4];
|
|
EyeFinder[0] = (FMDMS>>3) + DeltaH; // X1
|
|
EyeFinder[1] = (FMDMS>>1) + DeltaV; // Y1
|
|
EyeFinder[2] = (FMDMS>>1) - DeltaH; // X2
|
|
EyeFinder[3] = (FMDMS>>1) + (FMDMS>>2) + (FMDMS>>3) - DeltaV; // Y2
|
|
|
|
Feature_DETECT(pFeatureMap,NULL,0,0);
|
|
if (ASM_GetCenterG(pFeatureMap,EyeFinder[0],EyeFinder[1],EyeFinder[2],EyeFinder[3],&Center))
|
|
{
|
|
TURN_POINT();
|
|
pLocalDF->RightEye.x = Center.x;
|
|
pLocalDF->RightEye.y = Center.y;
|
|
pLocalDF->RightEye.z = 1.0f;
|
|
}
|
|
|
|
FLIP_H(pFeatureMap);
|
|
Feature_DETECT(pFeatureMap,NULL,0,0);
|
|
if (ASM_GetCenterG(pFeatureMap,EyeFinder[0],EyeFinder[1],EyeFinder[2],EyeFinder[3],&Center))
|
|
{
|
|
Center.x = pFeatureMap->SX - Center.x;
|
|
TURN_POINT();
|
|
pLocalDF->LeftEye.x = Center.x;
|
|
pLocalDF->LeftEye.y = Center.y;
|
|
pLocalDF->LeftEye.z = 1.0f;
|
|
|
|
}
|
|
FLIP_H(pFeatureMap);
|
|
/* NOSE *****************************************************************************************************************/
|
|
MCFinder[0] = (FMDMS>>1)-(FMDMS>>3); // X1
|
|
MCFinder[2] = (FMDMS>>1)+(FMDMS>>3); // X2
|
|
MCFinder[1] = (FMDMS>>1)-(FMDMS>>3); // Y1
|
|
MCFinder[3] = (FMDMS>>1)+(FMDMS>>3); // Y2
|
|
Feature_DETECT(pFeatureMap,NULL,0,2);
|
|
if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
|
|
{
|
|
TURN_POINT();
|
|
pLocalDF->Nose.x = Center.x;
|
|
pLocalDF->Nose.y = Center.y;
|
|
pLocalDF->Nose.z = 1.0f;
|
|
|
|
}
|
|
/* Mouth *****************************************************************************************************************/
|
|
/* Left corner (on the screen)*****************************************************************************************************************/
|
|
|
|
Feature_DETECT(pFeatureMap,NULL,0,1);
|
|
MCFinder[0] = (FMDMS>>2); // X1
|
|
MCFinder[1] = (FMDMS>>2)-(FMDMS>>3); // Y1
|
|
MCFinder[2] = (FMDMS>>1)-(FMDMS>>4); // X2
|
|
MCFinder[3] = (FMDMS>>1)-(FMDMS>>3); // Y2
|
|
|
|
if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
|
|
{
|
|
TURN_POINT();
|
|
pLocalDF->RightMouse.x = Center.x;
|
|
pLocalDF->RightMouse.y = Center.y;
|
|
pLocalDF->RightMouse.z = 1.0f;
|
|
}
|
|
|
|
/* Right corner (on the screen)*****************************************************************************************************************/
|
|
FLIP_H(pFeatureMap);
|
|
Feature_DETECT(pFeatureMap,NULL,0,1);
|
|
if (ASM_GetCenterG(pFeatureMap,MCFinder[0],MCFinder[1],MCFinder[2],MCFinder[3],&Center))
|
|
{
|
|
Center.x = pFeatureMap->SX - Center.x;
|
|
TURN_POINT();
|
|
pLocalDF->LeftMouse.x = Center.x;
|
|
pLocalDF->LeftMouse.y = Center.y;
|
|
pLocalDF->LeftMouse.z = 1.0f;
|
|
|
|
}
|
|
FLIP_H(pFeatureMap);
|
|
|
|
/* Transform Feature Coordinate into real space */
|
|
{
|
|
Vector Add,Mul;
|
|
Add.x = SBH.X1 + (SBH.X2-SBH.X1)/10.0f;
|
|
Add.y = SBH.Y1;
|
|
Add.z = 0.0f;
|
|
Mul.x = (float)(SBH.X2-SBH.X1) / (float)FMDMS;
|
|
Mul.y = (float)(SBH.Y2-SBH.Y1) / (float)FMDMS;
|
|
Mul.z = 1.0f;
|
|
pLocalDF->RightEye = Add + (pLocalDF->RightEye & Mul);
|
|
pLocalDF->LeftEye = (pLocalDF->LeftEye & Mul) + Add;
|
|
pLocalDF->Nose = (pLocalDF->Nose & Mul) + Add;
|
|
pLocalDF->RightMouse = (pLocalDF->RightMouse & Mul) + Add;
|
|
pLocalDF->LeftMouse = (pLocalDF->LeftMouse & Mul) + Add;
|
|
pLocalDF->UpMouse = (pLocalDF->UpMouse & Mul) + Add;
|
|
pLocalDF->DownMouse = (pLocalDF->DownMouse & Mul) + Add;
|
|
}
|
|
|
|
DETECTEDFace LocalDF;
|
|
if (0)
|
|
{
|
|
LocalDF = *pLocalDF;
|
|
RecenterFace(pLocalDF);
|
|
{
|
|
/* compute the shape of the face */
|
|
LocalDF.HeadPos.x -= LocalDF.HeadSize.x/20.0;
|
|
pFeatureMap = pFeatureMapSave;
|
|
FeatureMap.SetBase(pFeatureMap->GetBase()) ;
|
|
FeatureMap.SX = ASM_COMMPUTE_SHAPE_SIZEMAP;
|
|
FeatureMap.SY = (s32)((float)ASM_COMMPUTE_SHAPE_SIZEMAP * ((float)SY / (float)SX));
|
|
pFeatureMap = &FeatureMap;
|
|
pFeatureMapSave = pFeatureMap;
|
|
|
|
s32 SourceSizeX = (s32)((float)LocalDF.HeadSize.x * 1.10f);
|
|
s32 SourceSizeY = (s32)((float)LocalDF.HeadSize.y * 1.10f);
|
|
|
|
MAP32_SET(pFeatureMap,0);
|
|
/* compute a "working map" close to the template of the face shape */
|
|
COPY_PATCH_RGBA(
|
|
pSourceMap,
|
|
pFeatureMap,
|
|
((s32)(LocalDF.HeadPos.x)<<8),
|
|
((s32)LocalDF.HeadPos.y<<8),
|
|
0,
|
|
0,
|
|
SourceSizeX<<8,SourceSizeY<<8,
|
|
FeatureMap.SX<<8,FeatureMap.SY<<8,LocalDF.HeadPos.z,1);
|
|
MATRIX TMatrix;
|
|
MATRIX TMatrix2;
|
|
TMatrix.Identity();
|
|
TMatrix2.Identity();
|
|
float ScaleX,ScaleY;
|
|
ScaleX = (float)FeatureMap.SX / (float)SourceSizeX ;
|
|
ScaleY = (float)FeatureMap.SY / (float)SourceSizeY;
|
|
TMatrix2.T.x = - LocalDF.HeadPos.x;
|
|
TMatrix2.T.y = - LocalDF.HeadPos.y;
|
|
TMatrix.I.x *= ScaleX;
|
|
TMatrix.J.y *= ScaleY;
|
|
TMatrix.RotateAround_K(-LocalDF.HeadPos.z);
|
|
TMatrix2 *= TMatrix;
|
|
TMatrix2.T.x += (float)FeatureMap.SX * 0.5f;
|
|
TMatrix2.T.y += (float)FeatureMap.SY * 0.5f;
|
|
|
|
if ((LocalDF.RightMouse.z!=0) && (LocalDF.LeftMouse.z!=0))
|
|
/* Compute shape with the mouth */
|
|
ComputeBottomShape(pFeatureMap,pSourceMapDRW,&LocalDF,&TMatrix2);
|
|
else
|
|
/* Compute shape without mouth */
|
|
ComputeBottomShape(pFeatureMap,pSourceMapDRW,NULL,&TMatrix2);
|
|
}
|
|
}
|
|
}
|
|
|
|
u32 HAAR_Detect(MAP *pSrcMAP,HAAR_Classifier *pHFF,MAP *pDrawMAP,
|
|
float Factor,float gAddX,float gAddY,u32 AGE,float Angle,
|
|
MAP8_Quad *MAP8_QuadStack, u32 *MAP8_QuadStackNumber, MAP8_Quad *pBlendQuad)
|
|
{
|
|
s32 *pSrc,*pSrcL,*pSrcLL,dX,dY = 0,AddX,AddY;
|
|
u32 p1,p2;
|
|
if (pHFF->WindowSize.X > pSrcMAP->SX) return 0 ;
|
|
if (pHFF->WindowSize.Y > pSrcMAP->SY) return 0;
|
|
|
|
AddX = 1;
|
|
AddY = 1;
|
|
|
|
p1 = pHFF->WindowSize.X ;
|
|
p2 = pSrcMAP->PITCH*(pHFF->WindowSize.Y );
|
|
|
|
pSrc = pSrcMAP->GetBase();
|
|
pSrcLL = pSrc + pSrcMAP->SX + pSrcMAP->PITCH * (pSrcMAP->SY - pHFF->WindowSize.Y);
|
|
pSrcL = pSrc + pSrcMAP->SX - pHFF->WindowSize.X;
|
|
while (pSrc<pSrcLL)
|
|
{
|
|
pSrcL = pSrc + pSrcMAP->SX - pHFF->WindowSize.X;
|
|
dX = 0;
|
|
while (pSrc<pSrcL)
|
|
{
|
|
ASM8_SCLALAR WindowSumm ;
|
|
WindowSumm = (ASM8_SCLALAR)(*(pSrc) - *(pSrc + p1) - *(pSrc + p2) + *(pSrc + p1 + p2));
|
|
if (ASM8_DETECT(pSrc,pHFF,WindowSumm * WINDOW_SUM_FACTOR_II))
|
|
{
|
|
if (*MAP8_QuadStackNumber < MAX_MAP8_QUAD)
|
|
{
|
|
memset(&MAP8_QuadStack[*MAP8_QuadStackNumber],0,sizeof(MAP8_QuadStack[*MAP8_QuadStackNumber]));
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].X1 = Factor*(float)(dX ) + gAddX;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Y1 = Factor*(float)(dY ) + gAddY;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].X2 = Factor*(float)(dX+pHFF->WindowSize.X) + gAddX;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Y2 = Factor*(float)(dY+pHFF->WindowSize.Y) + gAddY;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].W = 1.0f;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Age = AGE;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Angle = Angle;
|
|
|
|
if (pBlendQuad)
|
|
{
|
|
#define BlendFactorQuad 1.0f
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Angle += pBlendQuad->Angle * BlendFactorQuad;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].X1 += pBlendQuad->X1 * BlendFactorQuad;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].X2 += pBlendQuad->X2 * BlendFactorQuad;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Y1 += pBlendQuad->Y1 * BlendFactorQuad;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].Y2 += pBlendQuad->Y2 * BlendFactorQuad;
|
|
MAP8_QuadStack[*MAP8_QuadStackNumber].W += pBlendQuad->W * BlendFactorQuad;
|
|
}
|
|
|
|
if (*MAP8_QuadStackNumber < MAX_MAP8_QUAD)
|
|
(*MAP8_QuadStackNumber)++;
|
|
}
|
|
}
|
|
dX+=AddX;
|
|
pSrc+=AddX;
|
|
}
|
|
dY+=AddY;
|
|
pSrc = pSrcMAP->GetBase() + dY*pSrcMAP->PITCH ;
|
|
}
|
|
return 1;
|
|
}
|
|
|