void KinectEasyGrabber::dumpToDisk(int frameIndex, char* frameBasename, USHORT* depthD16, BYTE* colorRGBX, LONG* colorCoordinates, LARGE_INTEGER depthTimeStamp, LARGE_INTEGER colorTimeStamp){
if(m_dumped) return;
FILE* fid = 0;
//-------------------------------------------------------
// DEPTH & PLAYER INDEX ---------------------------------
//-------------------------------------------------------
#ifdef RECORD_PLAYER
unsigned char* outputPlayerUC8 = new unsigned char[m_depthWidth * m_depthHeight];
for(USHORT* pBufferEnd = (depthD16 + (m_depthWidth * m_depthHeight));
depthD16 != pBufferEnd; depthD16++, outputPlayerUC8++){
*outputPlayerUC8 = NuiDepthPixelToPlayerIndex(*depthD16);
*depthD16 = NuiDepthPixelToDepth(*depthD16);
}
depthD16 -= (m_depthWidth * m_depthHeight); //restablezco el puntero
outputPlayerUC8 -= (m_depthWidth * m_depthHeight); //restablezco el puntero
// Player index -----------------------------
sprintf (frameBasename,"data/player/video_player_%d.pgm",frameIndex);
fid = fopen(frameBasename,"wb");
if(!fid){
printf("ERROR abriendo el archivo %d\n",frameBasename);
exit(-1);
}
fprintf(fid, "P5\n#TS=%llu\n", depthTimeStamp.QuadPart);
fprintf(fid, "%i %i\n%i\n", m_depthWidth, m_depthHeight, 7);
fwrite(outputPlayerUC8,1,m_depthWidth*m_depthHeight*sizeof(unsigned char),fid);
fclose(fid);
delete [] outputPlayerUC8;
#endif
//depth -----
sprintf (frameBasename,"data/depth/video_depth_%d.pgm",frameIndex);
fid = fopen(frameBasename,"wb");
if(!fid){
printf("ERROR abriendo el archivo %d\n",frameBasename);
exit(-1);
}
fprintf(fid, "P5\n#TS=%llu\n", depthTimeStamp.QuadPart);
fprintf(fid, "%i %i\n%i\n", m_depthWidth, m_depthHeight, 65535);
fwrite(depthD16,1,m_depthWidth*m_depthHeight*sizeof(USHORT),fid);
fclose(fid);
//-------------------------------------------------------
// COLOR FRAME ------------------------------------------
//-------------------------------------------------------
sprintf (frameBasename,"data/rgb/video_rgb_%d.ppm",frameIndex);
fid = fopen(frameBasename,"wb");
if(!fid){
printf("ERROR abriendo el archivo %d\n",frameBasename);
exit(-1);
}
fprintf(fid, "P6\n#TS=%llu\n", colorTimeStamp.QuadPart);
fprintf(fid, "%i %i\n%i\n", m_colorWidth, m_colorHeight, 255);
//optimize this---
BYTE firsts[6] = {colorRGBX[2],colorRGBX[1],colorRGBX[0],
colorRGBX[6],colorRGBX[5],colorRGBX[4]};
for(int i=0; i < 2; i++)
colorRGBX[i] = firsts[i];
//------
for(int i=2; i < m_colorWidth*m_colorHeight; i++){
colorRGBX[i*3+2] = colorRGBX[i*4];
colorRGBX[i*3+1] = colorRGBX[i*4+1];
colorRGBX[i*3] = colorRGBX[i*4+2];
}
fwrite(colorRGBX,1,m_colorWidth*m_colorHeight*3,fid);
fclose(fid);
//-------------------------------------------------------
// COLOR COORDINATES --------------------------------
//-------------------------------------------------------
sprintf (frameBasename,"data/map/video_map_%d.coord",frameIndex);
fid = fopen(frameBasename,"wb");
if(!fid){
printf("ERROR abriendo el archivo %d\n",frameBasename);
exit(-1);
}
fprintf(fid, "%d %d\n", m_depthWidth, m_depthHeight);
fwrite(colorCoordinates,1,m_depthWidth*m_depthHeight*2*sizeof(LONG),fid);
fclose(fid);
}
void SimpleKinect::UpdateOpticFlow()
{
USHORT player;
Vector4 prevWorld, curWorld;
LONG* pOpticFlow = m_pOpticFlow;
// end pixel is start + width*height - 1
int x = cOpticalFlowWindowSize / 2, y = cOpticalFlowWindowSize / 2, i = 0;
int xx = 0, yy = 0, dx = 0, dy = 0, dz = 0, mindx = 0, mindy = 0, mindz =0 ;
USHORT* pCurrentDepth = m_pCurrentDepth + (y * cDepthWidth) + x;
const USHORT * pBufferEnd = pCurrentDepth + (cDepthWidth * cDepthHeight);
// TODO: Use world coordiantes!
while ( pCurrentDepth < pBufferEnd )
{
player = NuiDepthPixelToPlayerIndex(*pCurrentDepth);
curWorld = m_pCurrentWorld[y * cDepthWidth + x];
dx=0, dy=0, dz=0, mindx=0,mindy=0,mindz=0;
// to do: change backto 0 when done debugging
// find coordinate of pixel that has smallest distance to current pixel
double minDistance = LONG_MAX;
for(yy = y - cOpticalFlowWindowSize / 2; yy < y + cOpticalFlowWindowSize / 2; yy++)
{
for (xx = x - cOpticalFlowWindowSize / 2; xx < x + cOpticalFlowWindowSize / 2; xx++)
{
prevWorld = m_pPrevWorld[(yy) * cDepthWidth + xx];
dy = curWorld.y - prevWorld.y;
dx = curWorld.x - prevWorld.x;
dz = curWorld.z - prevWorld.z;
double distance = sqrtl(dx * dx + dy * dy + dz * dz);
if(distance < minDistance)
{
// make sure that we are not accidentally recording giant jumps in z/x/y, whihc represent just noise
minDistance = distance;
mindx = dx;
mindy = dy;
mindz = dz;
}
}
}
pOpticFlow[0] = mindx;
pOpticFlow[1] = mindy;
pOpticFlow[2] = mindz;
// update indices
x+= cOpticalFlowWindowSize;
if(x >= cDepthWidth){
x = cOpticalFlowWindowSize / 2;
y+= cOpticalFlowWindowSize;
pCurrentDepth = m_pCurrentDepth + y * cDepthWidth + x;
} else
{
pCurrentDepth += cOpticalFlowWindowSize;
}
pOpticFlow += 3;
}
}
USHORT Kinect::GetPlayerIndexAt(UINT depthX, UINT depthY)
{
return NuiDepthPixelToPlayerIndex((USHORT)(depthY * DepthStream().Width() + depthX));
}
void SimpleKinect::UpdateWorld()
{
// copy current world coords to prev world coords
memcpy(m_pPrevWorld, m_pCurrentWorld, sizeof(Vector4) * cDepthWidth * cDepthHeight);
// convert depth coords to world coords in depth fram
int x=0,y=0;
USHORT* pBufferRun = m_pCurrentDepth;
Vector4* pWorldRun = m_pCurrentWorld;
const USHORT* pBufferEnd = pBufferRun + (cDepthWidth * cDepthHeight);
float fSkeletonX = 0, fSkeletonY = 0;
USHORT depth, player;
while(pBufferRun < pBufferEnd)
{
if(*pBufferRun < NUI_IMAGE_DEPTH_MAXIMUM && *pBufferRun > NUI_IMAGE_DEPTH_MINIMUM){
depth = NuiDepthPixelToDepth(*pBufferRun);
player = NuiDepthPixelToPlayerIndex(*pBufferRun);
float depthM = depth;
//
// Center of depth sensor is at (0,0,0) in skeleton space, and
// and (width/2,height/2) in depth image coordinates. Note that positive Y
// is up in skeleton space and down in image coordinates.
//
fSkeletonX = (x/2.0 - 160) * NUI_CAMERA_DEPTH_IMAGE_TO_SKELETON_MULTIPLIER_320x240 * depthM;
fSkeletonY = -(y/2.0 - 120) * NUI_CAMERA_DEPTH_IMAGE_TO_SKELETON_MULTIPLIER_320x240 * depthM;
(*pWorldRun).x = fSkeletonX;
(*pWorldRun).y = fSkeletonY;
(*pWorldRun).z = depthM;
// update min/max
if(depthM < m_minDepthPerPlayer[player])
{
m_minDepthPerPlayer[player] = depthM;
}
if(depthM > m_maxDepthPerPlayer[player])
{
m_maxDepthPerPlayer[player] = depthM;
}
} else
{
(*pWorldRun).x = 0;
(*pWorldRun).y = 0;
(*pWorldRun).z = 0;
}
x++;
if(x >= cDepthWidth)
{
y++;
x = 0;
}
pBufferRun++;
pWorldRun++;
}
}
