KinectDepth2PointCloudBuilder::KinectDepth2PointCloudBuilder(const USHORT *depthBuffer, UINT width, UINT height)
: m_pointCloud(new PointCloudRegistrator::PointCloud)
{
int imageWidth = width;
int imageHeight = height;
int imageHalfWidth = imageWidth / 2;
int imageHalfHeight = imageHeight / 2;
float pixelToMeterScale = tanf(NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV * M_PI / 180.f * 0.5f) / (width * .5f);
size_t size = width * height;
m_pointCloud->width = size;
m_pointCloud->height = 1;
m_pointCloud->is_dense = true;
m_pointCloud->points.reserve(width * height);
for (size_t i = 0; i < size; ++i)
{
int w = i % width;
int h = i / height;
float depth = 0.001f * NuiDepthPixelToDepth(*(depthBuffer + i));
if (depth == 0.f)
continue;
float x = 1.0f * (w - imageHalfWidth) * pixelToMeterScale * depth;
float y = -1.0f * (h - imageHalfHeight) * pixelToMeterScale * depth;
m_pointCloud->points.push_back(PointXYZ(x, y, depth));
}
}
void getDepthData(GLubyte* dest) {
float* fdest = (float*) dest;
long* depth2rgb = (long*) depthToRgbMap;
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT LockedRect;
if (sensor->NuiImageStreamGetNextFrame(depthStream, 0, &imageFrame) < 0) return;
INuiFrameTexture* texture = imageFrame.pFrameTexture;
texture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch != 0) {
const USHORT* curr = (const USHORT*) LockedRect.pBits;
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
// Get depth of pixel in millimeters
USHORT depth = NuiDepthPixelToDepth(*curr++);
// Store coordinates of the point corresponding to this pixel
Vector4 pos = NuiTransformDepthImageToSkeleton(i, j, depth<<3, NUI_IMAGE_RESOLUTION_640x480);
*fdest++ = pos.x/pos.w;
*fdest++ = pos.y/pos.w;
*fdest++ = pos.z/pos.w;
// Store the index into the color array corresponding to this pixel
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
NUI_IMAGE_RESOLUTION_640x480, NUI_IMAGE_RESOLUTION_640x480, NULL,
i, j, depth<<3, depth2rgb, depth2rgb+1);
depth2rgb += 2;
}
}
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(depthStream, &imageFrame);
}
UINT16 * Kinect1::getDepthBuffer()
{
int result;
// Grab an image frame
NUI_IMAGE_FRAME imageFrame;
result = sensor->NuiImageStreamGetNextFrame(depthStream, 0, &imageFrame);
// Make sure the frame is not empty
if (result >= 0)
{
// Get frame texture
NUI_LOCKED_RECT lockedRect;
INuiFrameTexture * texture = imageFrame.pFrameTexture;
texture->LockRect(0, &lockedRect, NULL, 0);
UINT16 * bufferIndex = depthBuffer;
UINT16 * frameIndex = (UINT16 *) lockedRect.pBits;
UINT16 * end = frameIndex + (depthWidth * depthHeight);
// Copy data to buffer
while (frameIndex < end)
{
// 4 byte BGRA to 1 int ARGB
*bufferIndex = NuiDepthPixelToDepth(*frameIndex);
bufferIndex++;
frameIndex++;
}
// Release
texture->LockRect(0, &lockedRect, NULL, 0);
sensor->NuiImageStreamReleaseFrame(depthStream, &imageFrame);
}
return depthBuffer;
}
void kinectView::customDraw()
{
ofPushStyle();
ofSetColor(255, 100, 100);
USHORT maxDepth = NuiDepthPixelToDepth(NUI_IMAGE_DEPTH_MAXIMUM);
USHORT minDepth = NuiDepthPixelToDepth(NUI_IMAGE_DEPTH_MINIMUM);
// get far plane
// draw rectangle at far plane
// max depth: maxDepth
float verticalFovRad = NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * 2 * PI / 360;
float horizFovRad = NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV * 2 * PI / 360;
float halfBackHeight = atan( verticalFovRad) * maxDepth;
float halfBackWidth = atan( horizFovRad) * maxDepth;
float halfFrontHeight = atan( verticalFovRad) * minDepth;
float halfFrontWidth = atan( horizFovRad) * minDepth;
ofPoint origin = ofPoint();
ofPoint leftTop = ofPoint(-halfBackWidth, halfBackHeight, maxDepth);
ofPoint rightTop = ofPoint(halfBackWidth, halfBackHeight, maxDepth);
ofPoint leftBottom = ofPoint(-halfBackWidth, -halfBackHeight, maxDepth);
ofPoint rightBottom = ofPoint(halfBackWidth, -halfBackHeight, maxDepth);
ofLine(origin, leftTop);
ofLine(origin, rightTop);
ofLine(origin, leftBottom);
ofLine(origin, rightBottom);
ofLine(leftTop, rightTop);
ofLine(rightTop, rightBottom);
ofLine(rightBottom, leftBottom);
ofLine(leftBottom, leftTop);
ofPoint frontleftTop = ofPoint(-halfFrontWidth, halfFrontHeight, minDepth);
ofPoint frontrightTop = ofPoint(halfFrontWidth, halfFrontHeight, minDepth);
ofPoint frontleftBottom = ofPoint(-halfFrontWidth, -halfFrontHeight, minDepth);
ofPoint frontrightBottom = ofPoint(halfFrontWidth, -halfFrontHeight, minDepth);
ofLine(frontleftTop, frontrightTop);
ofLine(frontrightTop, frontrightBottom);
ofLine(frontrightBottom, frontleftBottom);
ofLine(frontleftBottom, frontleftTop);
ofPopStyle();
}
void ConvertDepthFrameToArray(const NUI_IMAGE_FRAME& frame, DataTypes::DepthImage& image)
{
assert(frame.eImageType == NUI_IMAGE_TYPE_DEPTH);
assert(frame.eResolution == NUI_IMAGE_RESOLUTION_640x480);
INuiFrameTexture* texture = frame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
texture->LockRect(0, &lockedRect, NULL, 0);
{
// Make sure we've received valid data
if (lockedRect.Pitch == 0)
{
texture->UnlockRect(0);
throw -1;
}
//////////////////////////////
assert((lockedRect.size % sizeof(short)) == 0);
assert((lockedRect.Pitch % sizeof(short)) == 0);
int cols = lockedRect.Pitch / sizeof(short);
int rows = lockedRect.size / lockedRect.Pitch;
memcpy(depthData, lockedRect.pBits, lockedRect.size);
int byteNum = 0;
for(int y=0; y<rows; y++)
{
for(int x=0; x<cols; x++)
{
const unsigned short& pixel = (unsigned short&)lockedRect.pBits[byteNum];
DepthPixel& depth = image.data[y][x];
// discard the portion of the depth that contains only the player index
depth = NuiDepthPixelToDepth(pixel);
byteNum += sizeof(short);
}
}
image.rows = rows;
image.cols = cols;
//////////////////////////////
}
// We're done with the texture so unlock it
texture->UnlockRect(0);
return;
}
USHORT* KTM::KinectWrapper::getDepthFromKinectStream(){
HRESULT hr;
/* Get the next frame from the Kinect stream */
NUI_IMAGE_FRAME imageFrame;
hr = pKinectSensor->NuiImageStreamGetNextFrame(hDepthStreamHandle, 0, &imageFrame);
/* Quit if the frame couldn't be recieved */
if (FAILED(hr))
return NULL;
/* Get the frame as a texture so we can get the frame rectangle */
/* and lock the handle on the frame so it's not overwritten by */
/* the Kinect while we're using it */
INuiFrameTexture* pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
pTexture->LockRect(0, &lockedRect, NULL, 0);
/* Make sure we have valid data */
if (lockedRect.Pitch != 0){
/* Use the assigned data heap to avoid memory leaks and avoid */
/* needlessly assigning new memory */
USHORT* mFrameData = mDepthDataHeap;
/* Pointer to the top left corner of rectangle, and pointer */
/* to the frame end */
const USHORT* pBufferRun = (const unsigned short*)lockedRect.pBits;
const USHORT* pBufferEnd = pBufferRun + (depthFrameWidth * depthFrameHeight);
while ( pBufferRun < pBufferEnd ){
/* discard the portion of the depth that contains only the player index */
USHORT depth = NuiDepthPixelToDepth(*pBufferRun);
/* Convert the returned depth to a char, discarding the most significant */
/* bits in favor of the least significant */
USHORT intensity = depth;
/* Save the depth data */
*(mFrameData++) = intensity;
// Increment our index into the Kinect's depth buffer
++pBufferRun;
}
}
/* Unlock the rectangle and release the frame */
pTexture->UnlockRect(0);
pKinectSensor->NuiImageStreamReleaseFrame(hDepthStreamHandle, &imageFrame);
return mDepthDataHeap;
}
void storeNuiDepth(void)
{
NUI_IMAGE_FRAME depthFrame;
if(WAIT_OBJECT_0 != WaitForSingleObject(hNextDepthFrameEvent, 0)) return;
HRESULT hr = pNuiSensor->NuiImageStreamGetNextFrame(
pDepthStreamHandle,
0,
&depthFrame );
if( FAILED( hr ) ){
return;
}
if(depthFrame.eImageType != NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX)
STDERR("Depth type is not match with the depth and players\r\n");
INuiFrameTexture *pTexture = depthFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 ){
unsigned short *pBuffer = (unsigned short *)LockedRect.pBits;
memcpy(depth, LockedRect.pBits, pTexture->BufferLen());
#if defined(USE_FACETRACKER)
setDepthImage(LockedRect.pBits, LockedRect.size);
#endif
NUI_SURFACE_DESC pDesc;
pTexture->GetLevelDesc(0, &pDesc);
//printf("w: %d, h: %d, byte/pixel: %d\r\n", pDesc.Width, pDesc.Height, LockedRect.Pitch/pDesc.Width);
unsigned short *p = (unsigned short *)pBuffer;
for(int i=0;i<pTexture->BufferLen()/2;i++){
//*p = (unsigned short)((*p & 0xff00)>>8) | ((*p & 0x00ff)<<8); // for test
//*p = (unsigned short)((*p & 0xfff8)>>3);
*p = (unsigned short)(NuiDepthPixelToDepth(*pBuffer));
p++;
}
glBindTexture(GL_TEXTURE_2D, bg_texture[DEPTH_TEXTURE]);
glPixelStorei(GL_UNPACK_ALIGNMENT, 2);
glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE,
pDesc.Width, pDesc.Height,
0, GL_LUMINANCE, GL_UNSIGNED_SHORT, pBuffer);
pTexture->UnlockRect(0);
}
else{
STDERR("Buffer length of received texture is bogus\r\n");
}
pNuiSensor->NuiImageStreamReleaseFrame( pDepthStreamHandle, &depthFrame );
}
bool KinectHandler::handleImages( NUI_IMAGE_FRAME& imageFrame )
{
INuiFrameTexture * nuiTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
nuiTexture->LockRect( 0, &lockedRect, NULL, 0 );
if ( lockedRect.Pitch!=NULL )
{
const USHORT* current = (const USHORT*)lockedRect.pBits;
const USHORT* bufferEnd = current + (KINECT_IMAGE_WIDTH * KINECT_IMAGE_HEIGHT);
unsigned char* ptr = _depthBuffer;
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);
while ( current<bufferEnd )
{
// Note that for depth stream, realDepth = depth & 0x0fff
// But for depth with player index, realDepth = (depth & 0xfff8) >> 3
USHORT depth = NuiDepthPixelToDepth(*current);
*ptr = static_cast<unsigned char>(depth % 256);
++current; ++ptr;
}
}
nuiTexture->UnlockRect( 0 );
return true;
}
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::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++;
}
}
