789 lines
No EOL
20 KiB
C++
789 lines
No EOL
20 KiB
C++
#include "DEFINES.H"
|
|
|
|
#define PRIMESEND_FOCALE 1.3
|
|
|
|
|
|
#ifdef PRIMESENS_SUPPORT
|
|
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnCore.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnIO.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnUSB.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVBackgroundRemover.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVDynamicUserExtractor.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnCore.lib")
|
|
#pragma comment(lib, "..\\LIBS\\Input\\PrimesensLibs\\XnVision.lib")
|
|
|
|
#include <XnVision.h>
|
|
#include <XnVDynamicUserExtractor.h>
|
|
#include <XnVBackgroundRemover.h>
|
|
|
|
#include "TRACK.h"
|
|
#include "MEDIA.h"
|
|
|
|
//#pragma optimize("",off)
|
|
#define DepthMAX (4000)
|
|
u32 ColorMap[DepthMAX];
|
|
u32 Stats[DepthMAX];
|
|
static u32 NBV = 0;
|
|
|
|
static XnVDevice * pDevice = NULL;
|
|
static XnVUserExtractor * pUserExtractor = NULL;
|
|
static XnVDynamicUserExtractor * pDynamicUserExtractor = NULL;
|
|
static XnVLabelMatrix * pLabels = NULL;
|
|
|
|
static unsigned char * pAlphaMap = NULL;
|
|
static XnVDepthMap * pOrigDm = NULL;
|
|
static XnVBackgroundRemover * pBackgroundRemover = NULL;
|
|
|
|
float fPRIMESENS_Equalize = 0.0f;
|
|
u32 PrimeSendResetBackGround = 0;
|
|
XnVPlane GroundPlane = {0,0,0,0};
|
|
Vector UserCoodinate = Vector (0,0,0);
|
|
static s32 XMin = 10000000,YMin = 10000000;
|
|
static s32 XMax= -10000000,YMax= -10000000;
|
|
|
|
|
|
u16 CurrentDepth[1024*1024];
|
|
u16 CurrentDUser[1024*1024];
|
|
s32 CurrentColor[1024*1024];
|
|
|
|
|
|
u32 PrimeSensAlphaSilhouette = 0;
|
|
u32 PrimeUserExtraction = 0;
|
|
MAP LastMapUsed;
|
|
char ERRORTEXT[2048];
|
|
void PRIMESENS_2D_to_3D_Coordinate(MAP *ZMap, Vector *p2D , Vector *p3D);
|
|
void PRIMESENS_3D_to_2D_Coordinate(MAP *ZMap, Vector *p3D , Vector *p2D);
|
|
|
|
float TRK3DW_XMin = 0.5 - 0.15;
|
|
float TRK3DW_YMin = 0.5 - 0.15;
|
|
float TRK3DW_XMax = 0.5 + 0.15;
|
|
float TRK3DW_YMax = 0.5 + 0.35;
|
|
float TRK3DW_ZMin = 0.5 + 0.15;
|
|
float TRK3DW_ZMax = 0.5 + 0.35;
|
|
u32 TRK3DW_Flags = 0;
|
|
|
|
void *pZBUFFERMEDIA = 0;
|
|
class PRIMESENS_Grabber:public INPUT_IO
|
|
{
|
|
|
|
public:
|
|
u32 DepthMode;
|
|
MAP MaxDepth;
|
|
WORLD *pSourceScene;
|
|
int INIT (class WORLD*);
|
|
int FRAME (void);
|
|
int END (void);
|
|
IOTYPE GetType() {return IOtype_PRIMESENS;};
|
|
virtual void * GetData(int dataIdx) { return pZBUFFERMEDIA;};
|
|
};
|
|
|
|
s16 LocalMapBlur[1024*1024];
|
|
|
|
//#pragma optimize("",off)
|
|
|
|
u32 PRIMESENS_GetFloor(MATRIX *pDst)
|
|
{
|
|
|
|
extern Vector PRIMESENS_GroundZ;
|
|
extern float PRIMESENS_GroundDistance;
|
|
pDst->Identity();
|
|
|
|
if (PRIMESENS_GroundDistance)
|
|
{
|
|
pDst->T = Vector(0,0,0);
|
|
pDst->I = Vector(1,0,0);
|
|
pDst->J = Vector(0,0,1);
|
|
pDst->K = Vector(0,-1,0);
|
|
|
|
Vector Norm = Vector (0,0,1);
|
|
Vector vQ = PRIMESENS_GroundZ + Norm;
|
|
vQ.z = -vQ.z;
|
|
vQ .Normalize();
|
|
vQ = vQ^Norm ;
|
|
vQ = vQ * -1.0f;
|
|
Quat Q;
|
|
//vQ.z = 0.0f;
|
|
*(Vector *)&Q = vQ;
|
|
Q.NormalizeW();
|
|
MATRIX M;
|
|
M.Identity();
|
|
Q.TransforToMatrix(&M);
|
|
*pDst *= M;
|
|
pDst->RotateAround_I(-90.0f * 3.1415927f/ 180.0f) ;
|
|
pDst->T.x = pDst->T.y = 0.0f;
|
|
pDst->T.z = PRIMESENS_GroundDistance;
|
|
pDst->I *= -1;
|
|
pDst->J *= -1;
|
|
pDst->K *= -1;
|
|
|
|
float HAngle;
|
|
HAngle = asinf(pDst->K.x / sqrtf(pDst->K.y*pDst->K.y+pDst->K.z*pDst->K.z));
|
|
pDst->RotateAround_Z(-HAngle);
|
|
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
u32 PRIMESENS_GetUserMatrix(MATRIX *pDst)
|
|
{
|
|
|
|
return 0;
|
|
}
|
|
u32 PRIMESENS_GetUser2DCoordinate(Vector *pDst,u32 Mode)
|
|
{
|
|
|
|
switch(Mode)
|
|
|
|
{
|
|
case 0: // GroundGravity
|
|
{
|
|
extern Vector PRIMESENS_GroundZ;
|
|
extern float PRIMESENS_GroundDistance;
|
|
Vector Coord = UserCoodinate;
|
|
MATRIX Dst;
|
|
PRIMESENS_GetFloor(&Dst);
|
|
PRIMESENS_GroundZ.z = -PRIMESENS_GroundZ.z;
|
|
PRIMESENS_GroundZ.x = -PRIMESENS_GroundZ.x;
|
|
Coord -= PRIMESENS_GroundZ * ((PRIMESENS_GroundZ * Coord) - PRIMESENS_GroundDistance);
|
|
PRIMESENS_GroundZ.z = -PRIMESENS_GroundZ.z;
|
|
PRIMESENS_GroundZ.x = -PRIMESENS_GroundZ.x;
|
|
PRIMESENS_3D_to_2D_Coordinate(&LastMapUsed, &Coord , pDst);
|
|
}
|
|
return 1;
|
|
break;
|
|
case 1: // UserBVMin
|
|
pDst->z = 0.0f;
|
|
pDst->x = XMin;
|
|
pDst->y = YMin;
|
|
return 1;
|
|
break;
|
|
case 2: // UserBVMax
|
|
pDst->z = 0.0f;
|
|
pDst->x = XMax;
|
|
pDst->y = YMax;
|
|
return 1;
|
|
break;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int PRIMESENS_Grabber::FRAME (void)
|
|
{
|
|
ERRORTEXT[0] = '\0';
|
|
|
|
MediaFocale = PRIMESEND_FOCALE;
|
|
|
|
Vector MinV = Vector(TRK3DW_XMin * Image.SX,TRK3DW_YMin * Image.SY,TRK3DW_ZMin);
|
|
Vector MaxV = Vector(TRK3DW_XMax * Image.SX,TRK3DW_YMax * Image.SY,TRK3DW_ZMax);
|
|
|
|
LastMapUsed = Image;
|
|
// read a new frame
|
|
if (DepthMode == 3) // Normale Map
|
|
{
|
|
void MAP_PPRIMESENS_ComputeNormales(u16 *ZOoMap , MAP *pColorMap );
|
|
void MAP_PPRIMESENS_ComputeMatting(MAP *pColorMap );
|
|
s32 Counter = Image.SX * Image.SY * 3;
|
|
s32 *pBase;
|
|
|
|
XN_DEPTH_TYPE* pDepthUE = (XN_DEPTH_TYPE*)CurrentDUser;
|
|
XN_DEPTH_TYPE* pDepthRAW = (XN_DEPTH_TYPE*)CurrentDepth;
|
|
|
|
XN_IMAGE_TYPE* pImage = (XN_IMAGE_TYPE* )CurrentColor;
|
|
u8 *pIM = (u8*)pImage;
|
|
pBase = Image.BASE;
|
|
|
|
pDepthUE += Image.SX * Image.SY;
|
|
pDepthRAW += Image.SX * Image.SY;
|
|
|
|
while (Counter)
|
|
{
|
|
Counter-=3;
|
|
pDepthUE--;
|
|
pDepthRAW--;
|
|
*(pBase) = ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
|
|
if ((*pDepthUE))
|
|
*(pBase) |= 1<<24;
|
|
else
|
|
{
|
|
if (*pDepthRAW)
|
|
*(pBase) |= 2<<24;
|
|
}
|
|
pBase++;
|
|
|
|
}
|
|
|
|
MAP_PPRIMESENS_ComputeMatting( &Image );
|
|
}
|
|
else
|
|
if (DepthMode == 2) // ShadowMap
|
|
{
|
|
XN_DEPTH_TYPE* pDepth = (XN_DEPTH_TYPE*)CurrentDUser;
|
|
extern double MEGADX;
|
|
extern double MEGADY;
|
|
extern Vector MEGADZ;
|
|
void MAP_PPRIMESENS_ReLightWithZ(u16 *ZOoMap , MAP *pColorMap , MAP *pShadowMAP , Vector *GCenter , float DX, float DY);
|
|
//MAP_PPRIMESENS_ReLightWithZ(pDepth, &Image, NULL, &MEGADZ , MEGADX, MEGADY);
|
|
s32 *pBase;
|
|
pDepth += Image.SX * Image.SY;
|
|
pBase = Image.BASE;
|
|
memset(pBase,0,Image.SX * Image.SY * 4);
|
|
for (s32 Y = 0;Y<Image.SY ; Y++)
|
|
for (s32 X = 0 ;X<Image.SX; X++)
|
|
{
|
|
extern double MEGADX ;
|
|
extern double MEGADY ;
|
|
extern Vector MEGADZ ;
|
|
extern Vector PRIMESENS_GroundZ;
|
|
extern float PRIMESENS_GroundDistance;
|
|
|
|
float VirtuaklZ;
|
|
VirtuaklZ = (X - MEGADZ.x) * MEGADX + (Y - MEGADZ.y) * MEGADY + MEGADZ.z;
|
|
pDepth--;
|
|
if (*pDepth)
|
|
{
|
|
Vector p2D,p3D;
|
|
p2D = Vector(X,Y,*pDepth);
|
|
PRIMESENS_2D_to_3D_Coordinate(&Image, &p2D , &p3D);
|
|
//PRIMESENS_GroundZ.y = -PRIMESENS_GroundZ.y;
|
|
|
|
float Z = (PRIMESENS_GroundZ * p3D) + PRIMESENS_GroundDistance;
|
|
p3D -= PRIMESENS_GroundZ * Z;
|
|
|
|
if (Z < 500)
|
|
{
|
|
PRIMESENS_3D_to_2D_Coordinate(&Image, &p3D , &p2D);
|
|
if ((p2D.x > 0) && (p2D.y > 0) && ((p2D.x < Image.SX-1)) && (p2D.y < Image.SY - 15))
|
|
{
|
|
s32 *pBaseZShad = Image.BASE + (s32)p2D.x + ((s32)p2D.y ) * Image.PITCH;
|
|
float Z2;
|
|
Z2 = Z;
|
|
if (Z2 < 0) Z2 = 0;
|
|
Z2 = 255.0f * (500.0f - Z2) / 500.0f;
|
|
//if (*pBaseZShad == 0xffffffff)
|
|
{
|
|
u32 Counter,Color;
|
|
s32 Remont = (255 - Z2) / 20;
|
|
Counter = 3 + Remont;
|
|
if (Counter > 15) Counter = 15;
|
|
Remont/=2;
|
|
if (p2D.y < Remont) Remont = p2D.y;
|
|
pBaseZShad -= Remont * Image.PITCH;
|
|
Color = Z2;
|
|
Color -= Color>>3;
|
|
while (Counter--)
|
|
{
|
|
if (Color > *pBaseZShad)
|
|
*pBaseZShad = Color;
|
|
pBaseZShad += Image.PITCH;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//PRIMESENS_GroundZ.y = -PRIMESENS_GroundZ.y;
|
|
|
|
/* if ((s32)Z & 512)
|
|
*(pBase) = 0xffffff00;
|
|
else
|
|
*(pBase) = 0xffff00ff;//*/
|
|
}
|
|
else
|
|
{
|
|
|
|
/* if (VirtuaklZ < 0.0f)
|
|
*(pBase) = 0;*/
|
|
}
|
|
pBase++;
|
|
}
|
|
|
|
pBase = Image.BASE;
|
|
for (s32 Y = 0;Y<Image.SY ; Y++)
|
|
for (s32 X = 0 ;X<Image.SX; X++)
|
|
{
|
|
|
|
if (*pBase > 255) *pBase = 255;
|
|
*pBase |= *pBase<<8;
|
|
*pBase |= *pBase<<8;
|
|
*pBase |= *pBase<<8;
|
|
*pBase ^= 0xffffff;
|
|
pBase++;
|
|
}
|
|
|
|
} else
|
|
if (DepthMode == 1)
|
|
{
|
|
|
|
XnVStatus rc = pDevice->Read();
|
|
if (XNV_STATUS_OK != rc)
|
|
{
|
|
sprintf(ERRORTEXT, "Read failed: %s\n", XnGetStatusString(pDevice->GetLastStatus()));
|
|
OutputDebugString(ERRORTEXT);
|
|
return 0;
|
|
}
|
|
XN_DEPTH_TYPE* pDepth = pDevice->GetDepthMap()->Data();
|
|
XN_IMAGE_TYPE* pImage = pDevice->GetImageMap()->Data();
|
|
|
|
|
|
// extract the user
|
|
pDevice->GetDepthMap()->Clone(*pOrigDm);
|
|
if (PrimeUserExtraction)
|
|
{
|
|
// pDynamicUserExtractor->Init();
|
|
try { pDynamicUserExtractor->Run(pDevice->GetDepthMap(), pLabels); }
|
|
catch(...) {};
|
|
|
|
}
|
|
//pDepth = pOrigDm->Data();//*/
|
|
|
|
memcpy((s32*)CurrentDUser,pDepth,Image.SX * Image.SY * 2);
|
|
memcpy((s32*)CurrentDepth,pOrigDm->Data(),Image.SX * Image.SY * 2);
|
|
memcpy((s32*)CurrentColor,pImage,Image.SX * Image.SY * 3);
|
|
|
|
|
|
char debugstr[1024];
|
|
|
|
/* if (!pDynamicUserExtractor->GetFloorPlane(&GroundPlane))
|
|
{
|
|
OutputDebugString("Failed to get floor plane\n");
|
|
}
|
|
else
|
|
{
|
|
sprintf(debugstr, "Floor height: %f\n", GroundPlane.fHeight);
|
|
OutputDebugString(debugstr);
|
|
}*/
|
|
|
|
|
|
|
|
|
|
//GroundPlane.fHeight = 0.0f;
|
|
/*
|
|
if (!(pDynamicUserExtractor && pDynamicUserExtractor->GetFloorPlane(&GroundPlane)))
|
|
{
|
|
memset(&GroundPlane,0,sizeof(GroundPlane));
|
|
}
|
|
*/
|
|
|
|
|
|
// fix the silhouette
|
|
pAlphaMap = NULL;
|
|
if (PrimeSensAlphaSilhouette)
|
|
{
|
|
pBackgroundRemover->FixEdgesAndLearnBackRGB(pImage, pDepth, pOrigDm->Data(), pAlphaMap, false, true, false);
|
|
}
|
|
if (PrimeSendResetBackGround)
|
|
{
|
|
pBackgroundRemover->ResetBackRGB();
|
|
pDynamicUserExtractor->Reset();
|
|
}
|
|
PrimeSendResetBackGround = 0;
|
|
|
|
|
|
UserCoodinate = Vector(0,0,0);
|
|
void MAP_ComputePlanes(MAP *ZMap,u16 *PrimsensZMap);
|
|
MAP_ComputePlanes(&Image,pOrigDm->Data());
|
|
u32 Counter;
|
|
s32 *pBase;
|
|
XMin = 10000000;YMin = 10000000;
|
|
XMax= -10000000;YMax= -10000000;
|
|
float Summ = 0.0f,SummSqr = 0.0f;
|
|
u32 ulNumber = 0;
|
|
u8 *pu8AlphaPtr;
|
|
pBase = Image.BASE + Image.SX * Image.SY;
|
|
pu8AlphaPtr = pAlphaMap;
|
|
if (pu8AlphaPtr)
|
|
{
|
|
for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
|
|
for (s32 X = Image.SX-1 ;X>=0 ; X--)
|
|
{
|
|
pBase--;
|
|
//if ((*pDepth != 0) /*&& (*pDepth < 1500)*/)
|
|
//if (*pu8AlphaPtr)//(*pDepth != 0) /*&& (*pDepth < 1500)*/)
|
|
{
|
|
if (X < XMin) XMin = X;
|
|
if (X > XMax) XMax = X;
|
|
if (Y < YMin) YMin = Y;
|
|
if (Y > YMax) YMax = Y;
|
|
((u8*)pBase)[0] = (u8)pImage[2];
|
|
((u8*)pBase)[1] = (u8)pImage[1];
|
|
((u8*)pBase)[2] = (u8)pImage[0];
|
|
((u8*)pBase)[3] = *pu8AlphaPtr;
|
|
float brightness = (float)(((u32)pImage[2] + (u32)pImage[1] + (u32)pImage[1] + (u32)pImage[0])) * 0.25f;
|
|
Summ += brightness;
|
|
SummSqr += brightness * brightness;
|
|
ulNumber ++;
|
|
}
|
|
|
|
pImage+=3;
|
|
pDepth++;
|
|
if (pu8AlphaPtr)
|
|
pu8AlphaPtr++;
|
|
}
|
|
|
|
} else
|
|
{
|
|
for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
|
|
for (s32 X = Image.SX-1 ;X>=0 ; X--)
|
|
{
|
|
pBase--;
|
|
*(float *)pBase = (float)*pDepth;
|
|
if (*pDepth && (*pDepth < TRK3DW_ZMin)) *(u32 *)pBase = (TRK3DW_Flags & 2)>>1;
|
|
if (*pDepth > TRK3DW_ZMax)*(u32 *)pBase = (TRK3DW_Flags & 2)>>1;
|
|
|
|
/* float DepthColor = *pDepth;
|
|
if (DepthColor == 0) DepthColor = 10000000.0f;
|
|
DepthColor = 1000.0f / DepthColor;
|
|
DepthColor *= 256.0f ;
|
|
if (DepthColor < 0) DepthColor = 0;
|
|
u32 K = DepthColor;
|
|
if ((K & 15) != 0) DepthColor = 0;
|
|
if (DepthColor > 255) DepthColor = 255;*/
|
|
#if 0
|
|
if ((*pDepth != 0) /*&& (*pDepth < 1500)*/)
|
|
//if (*pu8AlphaPtr)//(*pDepth != 0) /*&& (*pDepth < 1500)*/)
|
|
{
|
|
if (X < XMin)
|
|
XMin = X;
|
|
if (X > XMax) XMax = X;
|
|
if (Y < YMin) YMin = Y;
|
|
if (Y > YMax) YMax = Y;
|
|
((u8*)pBase)[0] = (u8)pImage[2];
|
|
((u8*)pBase)[1] = (u8)pImage[1];
|
|
((u8*)pBase)[2] = (u8)pImage[0];
|
|
((u8*)pBase)[2] = ((u8*)pBase)[1] = ((u8*)pBase)[0] = (u8)((float)256 * (float)*pDepth / (float)4000 );
|
|
((u8*)pBase)[3] = 0xff;//*pu8AlphaPtr;
|
|
Vector p2D,p3D;
|
|
p2D = Vector(X,Y,*pDepth);
|
|
PRIMESENS_2D_to_3D_Coordinate(&Image, &p2D , &p3D);
|
|
UserCoodinate += p3D;
|
|
|
|
float brightness = (float)(((u32)pImage[2] + (u32)pImage[1] + (u32)pImage[1] + (u32)pImage[0])) * 0.25f;
|
|
Summ += brightness;
|
|
SummSqr += brightness * brightness;
|
|
ulNumber ++;
|
|
}
|
|
else
|
|
{
|
|
/*((u8*)pBase)[0] = (u8)pImage[2];
|
|
((u8*)pBase)[1] = (u8)pImage[1];
|
|
((u8*)pBase)[2] = (u8)pImage[0];
|
|
((u8*)pBase)[3] = 0x00;*/
|
|
((u8*)pBase)[0] = (u8)pImage[2];
|
|
((u8*)pBase)[1] = (u8)pImage[1];
|
|
((u8*)pBase)[2] = (u8)pImage[0];
|
|
((u8*)pBase)[3] = 0;
|
|
}
|
|
/*((u8*)pBase)[0] = (u8)DepthColor;
|
|
((u8*)pBase)[1] = (u8)DepthColor;
|
|
((u8*)pBase)[2] = (u8)DepthColor;
|
|
( (u8*)pBase)[3] = 0xff;//*pu8AlphaPtr;*/
|
|
#endif
|
|
|
|
pImage+=3;
|
|
pDepth++;
|
|
if (pu8AlphaPtr)
|
|
pu8AlphaPtr++;
|
|
}
|
|
|
|
/* First edge corection */
|
|
if (0)
|
|
{
|
|
MAP16 LocalMap;
|
|
LocalMap.BASE = (s16*)LocalMapBlur;
|
|
LocalMap.PITCH = LocalMap.SX = Image.SX;
|
|
LocalMap.SY = Image.SY;
|
|
u32 Countor = LocalMap.SX * LocalMap.SY;
|
|
while (Countor--)
|
|
LocalMapBlur[Countor] = ((u32)Image.BASE[Countor] )>>24;
|
|
//MAP_2_MAP16(&Image,&LocalMap,24);
|
|
MAP16_BLUR_Inplace(&LocalMap,2,1);
|
|
Countor = LocalMap.SX * LocalMap.SY;
|
|
while (Countor--)
|
|
{
|
|
if (LocalMapBlur[Countor] < 128)
|
|
LocalMapBlur[Countor] = 0;
|
|
else
|
|
LocalMapBlur[Countor] = 255;
|
|
}
|
|
Countor = LocalMap.SX * LocalMap.SY - 4;
|
|
while (Countor--)
|
|
{
|
|
LocalMapBlur[Countor+1] &= LocalMapBlur[Countor];
|
|
}
|
|
|
|
MAP16_2_MAP(&LocalMap,&Image,24);
|
|
}
|
|
}
|
|
|
|
if (ulNumber)
|
|
UserCoodinate *= 1.0f/(ulNumber);
|
|
|
|
pBase = Image.BASE + Image.SX * Image.SY;
|
|
pDepth = (XN_DEPTH_TYPE*)CurrentDUser;
|
|
pImage = (XN_IMAGE_TYPE* )CurrentColor;
|
|
|
|
for (s32 Y = Image.SY-1 ;Y>=0 ; Y--)
|
|
for (s32 X = Image.SX-1 ;X>=0 ; X--)
|
|
{
|
|
pBase--;
|
|
/* if (!((u8*)pBase)[3])
|
|
{
|
|
((u8*)pBase)[0] = (u8)pImage[2];
|
|
((u8*)pBase)[1] = (u8)pImage[1];
|
|
((u8*)pBase)[2] = (u8)pImage[0];
|
|
((u8*)pBase)[3] = 0xff;
|
|
}*/
|
|
pImage+=3;
|
|
pDepth++;
|
|
pu8AlphaPtr++;
|
|
}//*/
|
|
|
|
// EQUALIZE treatment
|
|
if (ulNumber && fPRIMESENS_Equalize)
|
|
{
|
|
u8 ConvertTable[256];
|
|
s32 *pBase,*pBaseLast;
|
|
Summ /= (float)ulNumber;
|
|
SummSqr /= (float)ulNumber;
|
|
SummSqr -= Summ * Summ;
|
|
if (SummSqr) // Then Normalize image
|
|
{
|
|
float NewAverage,Factor;
|
|
NewAverage = (128.0f + Summ) / 2.0f;
|
|
Factor = (1.0f + (100.0f / sqrt(SummSqr))) / 2.0f;
|
|
for (u32 i = 0 ; i < 256 ; i++)
|
|
{
|
|
float Res = ((float)i - Summ) * Factor + NewAverage;
|
|
if (Res > 255.0f) Res = 255.0f;
|
|
if (Res < 0.0f) Res = 0.0f;
|
|
ConvertTable[i] = (u8)Res;
|
|
}
|
|
|
|
pBase = Image.BASE;
|
|
pBaseLast = Image.BASE + Image.SX * Image.SY;
|
|
while (pBase < pBaseLast)
|
|
{
|
|
((u8*)pBase)[0] = ConvertTable[((u8*)pBase)[0]];
|
|
((u8*)pBase)[1] = ConvertTable[((u8*)pBase)[1]];
|
|
((u8*)pBase)[2] = ConvertTable[((u8*)pBase)[2]];
|
|
pBase++;
|
|
}
|
|
}
|
|
|
|
}
|
|
//DRawTEXT_Soft_B((u8*)ERRORTEXT,(u32*)Image.BASE,100,100,Image.PITCH, Image.SY , 0xffff0000);
|
|
//DrawQuad_R(&Image,XMin,YMin,XMax,YMax,0xffffffff,1,0.0f);
|
|
#define ZENV 2
|
|
u32 Counttttt = DepthMAX;
|
|
NBV = 0;
|
|
while (Counttttt--)
|
|
{
|
|
if (Stats[Counttttt])
|
|
NBV++;
|
|
}
|
|
|
|
DrawQuadFull(&Image,0,0,Image.SX,MinV.y,0);
|
|
DrawQuadFull(&Image,0,MinV.y,MinV.x,MaxV.y,0);
|
|
DrawQuadFull(&Image,MaxV.x,MinV.y,Image.SX,MaxV.y,0);
|
|
DrawQuadFull(&Image,0,MaxV.y,Image.SX,Image.SY,0);
|
|
|
|
} else
|
|
// ColorMap
|
|
{
|
|
s32 Counter = Image.SX * Image.SY * 3;
|
|
s32 *pBase;
|
|
XN_IMAGE_TYPE* pImage = (XN_IMAGE_TYPE* )CurrentColor;
|
|
u8 *pIM = (u8*)pImage;
|
|
pBase = Image.BASE;
|
|
XN_DEPTH_TYPE* pDepth = pDevice->GetDepthMap()->Data();
|
|
|
|
if (TRK3DW_Flags & 1)
|
|
{
|
|
while (Counter)
|
|
{
|
|
Counter-=3;
|
|
float ZZ = pDepth[Counter/3];
|
|
|
|
*(pBase) = 0xff000000 | ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
|
|
if (ZZ == 0)*pBase = 0;
|
|
if (ZZ < TRK3DW_ZMin) *pBase = 0;
|
|
if (ZZ > TRK3DW_ZMax) *pBase = 0;
|
|
|
|
pBase++;
|
|
}
|
|
} else
|
|
while (Counter)
|
|
{
|
|
Counter-=3;
|
|
*(pBase++) = 0xff000000 | ((u32)pIM[Counter]<<16) + ((u32)pIM[Counter + 1]<<8) + ((u32)pIM[Counter + 2]<<0);
|
|
}//*/
|
|
|
|
|
|
/*
|
|
if (pSourceScene->WSPC)
|
|
{
|
|
pSourceScene->WSPC->LastMin = Vector((pSourceScene->WSPC->DRAWMAP->SX>>1)-48,(pSourceScene->WSPC->DRAWMAP->SY>>1)-32,0);
|
|
pSourceScene->WSPC->LastMax = Vector((pSourceScene->WSPC->DRAWMAP->SX>>1)+48,(pSourceScene->WSPC->DRAWMAP->SY>>1)+64,0);
|
|
pSourceScene->WSPC->LastMin -= Vector(16,16,0);
|
|
pSourceScene->WSPC->LastMax += Vector(16,16,0);
|
|
}
|
|
*/
|
|
DrawQuad_R(&Image,MinV.x,MinV.y,MaxV.x,MaxV.y,0xff00,2,0);
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
// Local function variables
|
|
int PRIMESENS_Grabber::INIT(WORLD*psCene)
|
|
{
|
|
char ErrorName[2048];
|
|
DWORD Len = 2048;
|
|
|
|
pSourceScene = psCene;
|
|
|
|
|
|
GetCurrentDirectory(Len,ErrorName);
|
|
memset(Stats, 0 , sizeof(Stats));
|
|
|
|
if (NULL == pDevice)
|
|
{
|
|
XnStatus rcll;
|
|
XnVStatus rc = XnVDevice::IOInit(".\\Xiron.ini", &rcll);
|
|
|
|
// init XnVision
|
|
if (rc != XNV_STATUS_OK)
|
|
{
|
|
OutputDebugString(ErrorName);
|
|
sprintf(ErrorName,"Error initing XnVision: %s\n", XnGetStatusString(rcll));
|
|
OutputDebugString(ErrorName);
|
|
return 0;
|
|
}
|
|
|
|
// Create and open the device
|
|
pDevice = new XnVDevice();
|
|
rc = pDevice->OpenFromINI(".\\Xiron.ini", "DeviceInput");
|
|
if (rc != XNV_STATUS_OK)
|
|
{
|
|
sprintf(ErrorName, "Error opening device from %s: %s\n", ".\\Xiron.ini", XnGetStatusString(pDevice->GetLastStatus()));
|
|
OutputDebugString(ErrorName);
|
|
delete pDevice;
|
|
pDevice = NULL;
|
|
return 0;
|
|
}
|
|
|
|
// user extraction stuff
|
|
pUserExtractor = new XnVUserExtractor();
|
|
pDynamicUserExtractor = new XnVDynamicUserExtractor("", pDevice);
|
|
pLabels = new XnVLabelMatrix(pDevice->GetDepthMap()->m_nXRes, pDevice->GetDepthMap()->m_nYRes);
|
|
//pDynamicUserExtractor->Init();
|
|
pDynamicUserExtractor->SetFrontCutoff(1000);
|
|
|
|
// silhouette fixing stuff
|
|
pAlphaMap = (unsigned char*)_aligned_malloc (sizeof(unsigned char) * pDevice->GetImageMap()->m_nXRes * pDevice->GetImageMap()->m_nYRes, 16);
|
|
pOrigDm = new XnVDepthMap();
|
|
pOrigDm->Initialize(*pDevice->GetDepthMap(), pDevice->GetDepthMap()->m_nXRes, pDevice->GetDepthMap()->m_nYRes);
|
|
pBackgroundRemover = new XnVBackgroundRemover(pDevice->GetStreamDetails()->m_StreamProperties);
|
|
|
|
|
|
}
|
|
|
|
if (DepthMode == 1)
|
|
{
|
|
pDevice->SetIOParam(XN_IO_PARAM_RGB_REGISTRAR_STATE, XN_IO_REGMODE_DEPTH_TO_IMAGE);
|
|
Image.PITCH = MaxDepth.PITCH = MaxDepth.SX = Image.SX = pDevice->GetDepthMap()->m_nXRes;
|
|
MaxDepth.SY = Image.SY = pDevice->GetDepthMap()->m_nYRes;
|
|
Image.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
|
|
memset(Image.BASE , 0 , Image.SX * Image.SY * 4);
|
|
MaxDepth.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
|
|
memset(MaxDepth.BASE , 0 , MaxDepth.SX * MaxDepth.SY * 4);
|
|
|
|
pZBUFFERMEDIA = (void *)this;
|
|
|
|
|
|
/* Compute colorMAP */
|
|
for (int i = 0 ; i < DepthMAX ; i++)
|
|
{
|
|
ColorMap[i] = i & 0xff;
|
|
if (i & 0x100)
|
|
{
|
|
ColorMap[i] = 0xff - ColorMap[i];
|
|
}
|
|
ColorMap[i] |= (i & 0xffF0)<<4;
|
|
/* ColorMap[i] = i>>3;
|
|
ColorMap[i] |= ColorMap[i]<<8;
|
|
ColorMap[i] |= ColorMap[i]<<8;
|
|
if (i) ColorMap[i] ^= 0xffffff;
|
|
if (i & 0xf800) ColorMap[i] = 0;//*/
|
|
ColorMap[i] = 0;
|
|
if (i)
|
|
{
|
|
u32 OOD;
|
|
OOD = (u32)(((float)DepthMAX * 4.0f/ (float)256.0f) * (float)DepthMAX / (float)i);
|
|
//OOD /= DepthMAX;
|
|
ColorMap[i] = (OOD & 0x20)<<2;
|
|
ColorMap[i] = OOD ;
|
|
ColorMap[i] |= ColorMap[i]<<8;
|
|
ColorMap[i] |= ColorMap[i]<<8;
|
|
}
|
|
}
|
|
} else
|
|
{
|
|
Image.PITCH = Image.SX = pDevice->GetImageMap()->m_nXRes;
|
|
Image.SY = pDevice->GetImageMap()->m_nYRes;
|
|
Image.BASE = (s32*)CC_malloc(Image.SX * Image.SY * 4);
|
|
memset(Image.BASE , 0 , Image.SX * Image.SY * 4);
|
|
}
|
|
|
|
|
|
|
|
return 1;
|
|
}
|
|
|
|
int PRIMESENS_Grabber::END (void)
|
|
{
|
|
CC_free(MaxDepth.BASE);
|
|
return 0;
|
|
}
|
|
|
|
INPUT_IO *PRIMESENS_CREATE(WORLD *pScene,char *Filename)
|
|
{
|
|
INPUT_IO *Ret;
|
|
Ret = (INPUT_IO *)new(PRIMESENS_Grabber);
|
|
strcpy((char*)Ret->MediaName,Filename);
|
|
Ret->dTotalTime = 0;
|
|
Ret->dAvgFramePerSecond = 30.0;
|
|
if (Upper(Filename[0]) == 'N')
|
|
((PRIMESENS_Grabber*)Ret)->DepthMode = 3;
|
|
else
|
|
if (Upper(Filename[0]) == 'S')
|
|
((PRIMESENS_Grabber*)Ret)->DepthMode = 2;
|
|
else
|
|
if (Upper(Filename[0]) == 'D')
|
|
((PRIMESENS_Grabber*)Ret)->DepthMode = 1;
|
|
else
|
|
((PRIMESENS_Grabber*)Ret)->DepthMode = 0;
|
|
if (!Ret->INIT(pScene))
|
|
{
|
|
delete(Ret);
|
|
return NULL;
|
|
}
|
|
|
|
|
|
return (INPUT_IO *)Ret;
|
|
};
|
|
|
|
void PRIMESENS_SetTrackingWindow(Vector *Min, Vector *Max,u32 Flag)
|
|
{
|
|
TRK3DW_XMin = Min->x;
|
|
TRK3DW_YMin = Min->y;
|
|
TRK3DW_ZMin = Min->z;
|
|
TRK3DW_XMax = Max->x;
|
|
TRK3DW_YMax = Max->y;
|
|
TRK3DW_ZMax = Max->z;
|
|
TRK3DW_Flags = Flag;
|
|
}
|
|
|
|
#endif |