//Put the kinect imageframe data onto a Mat
Mat* KOCVStream::kFrameToMat(NUI_IMAGE_FRAME* imageFrame){
Mat* frame = new Mat(height, width, CV_8U);
NUI_LOCKED_RECT LockedRect;
//Lock the imageFrame such that kinnect cannot write on it
INuiFrameTexture* texture = imageFrame->pFrameTexture;
texture->LockRect(0, &LockedRect, NULL, 0);
//Get the kinect depth data
BYTE* imageData;
kinect->getDepthData(&LockedRect);
imageData = kinect->dataPix;
//If the data is not empty convert it to Mat
if (LockedRect.Pitch != 0){
/* //Do not do new above!
frame=new Mat(height, width, CV_8U, imageData);
*/
Mat tempMat(height, width, CV_8U, imageData);
tempMat.copyTo(*frame);
}
else{
return new Mat();
}
//Release the frame
texture->UnlockRect(0);
return frame;
};
void KinectCam::Nui_GetCamFrame(BYTE *frameBuffer, int frameSize)
{
const NUI_IMAGE_FRAME *pImageFrame = NULL;
WaitForSingleObject(m_hNextVideoFrameEvent, INFINITE);
HRESULT hr = NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
INuiFrameTexture *pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
memcpy(frameBuffer, pBuffer, frameSize);
}
NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
}
int createRGBImage(HANDLE h, IplImage* Color)
{
const NUI_IMAGE_FRAME *pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(h, 1000, &pImageFrame);
if(FAILED(hr))
{
cout << "Create RGB Image Failed\n";
return -1;
}
INuiFrameTexture *pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
if(LockedRect.Pitch != 0)
{
BYTE * pBuffer = (BYTE*)LockedRect.pBits;
cvSetData(Color, pBuffer, LockedRect.Pitch);
cvShowImage("Color Image", Color);
}
NuiImageStreamReleaseFrame(h, pImageFrame);
return 0;
}
void SimpleKinect::UpdateColor()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return;
}
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &LockedRect, NULL, 0);
// make sure we're receiving real data
if (LockedRect.Pitch == 0)
{
return;
}
// copy current to old
memcpy(m_pPrevRGB, m_pCurrentRGB, LockedRect.size);
// copy in data to current
memcpy(m_pCurrentRGB, LockedRect.pBits, LockedRect.size);
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
}
void KinectSensor::GotDepthAlert( )
{
const NUI_IMAGE_FRAME* pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &pImageFrame);
if (FAILED(hr))
{
return;
}
INuiFrameTexture* pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch)
{ // Copy depth frame to face tracking
memcpy(m_DepthBuffer->GetBuffer(), PBYTE(LockedRect.pBits), min(m_DepthBuffer->GetBufferSize(), UINT(pTexture->BufferLen())));
}
else
{
OutputDebugString( L"Buffer length of received depth texture is bogus\r\n" );
}
hr = NuiImageStreamReleaseFrame(m_pDepthStreamHandle, pImageFrame);
}
//Handles color data
bool Kinect::gotColorAlert()
{
NUI_IMAGE_FRAME frame;
bool processedFrame = true;
//get the next frame from the Kinect
HRESULT hr = globalNui->NuiImageStreamGetNextFrame( videoStreamHandle, 0, &frame);
if (FAILED( hr ) )
{
return false;
}
//get the data we need: frame now also contains information about the kinect it came from, etc. We do not need that.
INuiFrameTexture * texture = frame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
//lock the data we are going to use, so that other threads cant change it while we are using it.
texture->LockRect( 0, &lockedRect, NULL, 0);
if( lockedRect.Pitch != 0)
{
//draw it to the screen.
//mutex waiting, 5 ms timeout
bitmap->CopyFromMemory(NULL, static_cast<BYTE *>(lockedRect.pBits), 640 * 4);
DWORD result = WaitForSingleObject(mutex,5);
if (result == WAIT_OBJECT_0){
__try {
faceTracker->setColorVars(lockedRect, texture);
}
__finally {
ReleaseMutex(mutex);
}
}
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;
}
int Read_Kinect::drawColor(HANDLE h)
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = NuiImageStreamGetNextFrame(h, 0, &pImageFrame);
if (FAILED(hr))
{
cout << "Get Image Frame Failed" << endl;
return -1;
}
INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch != 0)
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
_color = Mat(COLOR_HIGHT,COLOR_WIDTH,CV_8UC4,pBuffer);
for (int i=0; i<COLOR_HIGHT; i++)
for(int j=0; j<COLOR_WIDTH; j++)
{
_bottom_left_img.at<Vec3b>(i,j).val[0] = _color.at<Vec4b>(i,j).val[0];
_bottom_left_img.at<Vec3b>(i,j).val[1] = _color.at<Vec4b>(i,j).val[1];
_bottom_left_img.at<Vec3b>(i,j).val[2] = _color.at<Vec4b>(i,j).val[2];
}
// imshow("color image", _color);
}
NuiImageStreamReleaseFrame(h, pImageFrame);
return 0;
}
void updateImageFrame( NUI_IMAGE_FRAME& imageFrame, bool isDepthFrame )
{
INuiFrameTexture* nuiTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
nuiTexture->LockRect( 0, &lockedRect, NULL, 0 );
if ( lockedRect.Pitch!=NULL )
{
const BYTE* buffer = (const BYTE*)lockedRect.pBits;
for ( int i=0; i<480; ++i )
{
const BYTE* line = buffer + i * lockedRect.Pitch;
const USHORT* bufferWord = (const USHORT*)line;
for ( int j=0; j<640; ++j )
{
if ( !isDepthFrame )
{
unsigned char* ptr = colorTexture->bits + 3 * (i * 640 + j);
*(ptr + 0) = line[4 * j + 2];
*(ptr + 1) = line[4 * j + 1];
*(ptr + 2) = line[4 * j + 0];
}
else
setPlayerColorPixel( bufferWord[j], j, i, 255 );
}
}
TextureObject* tobj = (isDepthFrame ? playerColorTexture : colorTexture);
glBindTexture( GL_TEXTURE_2D, tobj->id );
glTexImage2D( GL_TEXTURE_2D, 0, tobj->internalFormat, tobj->width, tobj->height,
0, tobj->imageFormat, GL_UNSIGNED_BYTE, tobj->bits );
}
nuiTexture->UnlockRect( 0 );
}
bool Nui_GotColorAlert( )
{
NUI_IMAGE_FRAME imageFrame;
bool processedFrame = true;
HRESULT hr = m_pNuiSensor->NuiImageStreamGetNextFrame( m_pVideoStreamHandle, 0, &imageFrame );
if ( FAILED( hr ) )
{
return false;
}
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if ( LockedRect.Pitch != 0 )
{
memcpy(g_ColorsData, LockedRect.pBits, LockedRect.size);
}
else
{
//OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
processedFrame = false;
}
pTexture->UnlockRect( 0 );
m_pNuiSensor->NuiImageStreamReleaseFrame( m_pVideoStreamHandle, &imageFrame );
return processedFrame;
}
void getRgbData(GLubyte* dest) {
float* fdest = (float*) dest;
long* depth2rgb = (long*) depthToRgbMap;
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT LockedRect;
if (sensor->NuiImageStreamGetNextFrame(rgbStream, 0, &imageFrame) < 0) return;
INuiFrameTexture* texture = imageFrame.pFrameTexture;
texture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch != 0) {
const BYTE* start = (const BYTE*) LockedRect.pBits;
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
// Determine rgb color for each depth pixel
long x = *depth2rgb++;
long y = *depth2rgb++;
// If out of bounds, then don't color it at all
if (x < 0 || y < 0 || x > width || y > height) {
for (int n = 0; n < 3; ++n) *(fdest++) = 0.0f;
}
else {
const BYTE* curr = start + (x + width*y)*4;
for (int n = 0; n < 3; ++n) *(fdest++) = curr[2-n]/255.0f;
}
}
}
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(rgbStream, &imageFrame);
}
void KinectDevice::Nui_GotRgbAlert()
{
NUI_IMAGE_FRAME imageFrame;
HRESULT hr = m_pNuiSensor->NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&imageFrame );
if( FAILED( hr ) )
{
return;
}
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
// TODO:
//cvSetData(iplColor, LockedRect.pBits, iplColor->widthStep);
Mat ref;//(iplColor);
if (_delegate)
_delegate->onRgbData(ref);
isNewFrames[FRAME_COLOR_U8C3] = true;
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
m_pNuiSensor->NuiImageStreamReleaseFrame( m_pVideoStreamHandle, &imageFrame );
}
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);
}
/// <summary>
/// Handle new color data
/// </summary>
/// <returns>S_OK for success or error code</returns>
HRESULT KinectEasyGrabber::ProcessColor()
{
HRESULT hr = S_OK;
NUI_IMAGE_FRAME imageFrame;
// Attempt to get the depth frame
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return hr;
}
m_colorTimeStamp = imageFrame.liTimeStamp;
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &LockedRect, NULL, 0);
// Make sure we've received valid data
if (LockedRect.Pitch != 0)
{
memcpy(m_colorRGBX, LockedRect.pBits, LockedRect.size);
}
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
return hr;
}
/// <summary>
/// Handle new color data
/// </summary>
/// <returns>indicates success or failure</returns>
void CDataCollection::ProcessColor()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
//DEBUG("Process color!\n");
// Attempt to get the color frame
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return;
}
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &LockedRect, NULL, 0);
// Make sure we've received valid data
if (LockedRect.Pitch != 0)
{
// Draw the data with Direct2D
m_pDrawColor->Draw(static_cast<BYTE *>(LockedRect.pBits), LockedRect.size);
}
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
}
void CSkeletalViewerApp::Nui_GotVideoAlert( )
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = m_pNuiInstance->NuiImageStreamGetNextFrame(
m_pVideoStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
m_DrawVideo.DrawFrame( (BYTE*) pBuffer );
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
m_pNuiInstance->NuiImageStreamReleaseFrame( m_pVideoStreamHandle, pImageFrame );
}
ci::gl::Texture *moduleCapture::getNextFrame()
{
HRESULT hr = sensor->NuiImageStreamGetNextFrame(colorStreamHandle, 0, pColorImageFrame); // Obtiene el siguiente frame del kinect
if (FAILED(hr)){
return NULL; // Si habia error, retorna NULL
}
INuiFrameTexture *colorTexture = pColorImageFrame->pFrameTexture;
NUI_LOCKED_RECT *colorRect = new NUI_LOCKED_RECT;
colorTexture->LockRect( 0, colorRect, 0, 0 );
if(colorRect->Pitch == 0){
return NULL;
}
// Crea un gl::Texture con los datos del frame
uint8_t *buffer = colorRect->pBits;
int size = resolution.width * resolution.height * 4;
ci::gl::Texture *texture = new ci::gl::Texture(buffer, GL_BGRA, resolution.width, resolution.height);
colorTexture->UnlockRect(0);
// Libera el frame del kinect
sensor->NuiImageStreamReleaseFrame(colorStreamHandle, pColorImageFrame);
// Retorna la textura
return texture;
}
void getKinectData(int* data) {
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;
const USHORT* dataEnd = curr + (width*height);
while (curr < dataEnd) {
//Get Depth in mm
USHORT depth = NuiDepthPixelToDepth(*curr++);
//Insert code to build a 640x480 matrix
for (int i = 0; i= height*width - 1; ++i){
*data++ = depth;
}
}
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(depthStream, &imageFrame);
}
void KinectDevice::Nui_GotDepthAlert()
{
FPS_CALC(fps_depth);
NUI_IMAGE_FRAME imageFrame;
HRESULT hr = m_pNuiSensor->NuiImageStreamGetNextFrame(
m_pDepthStreamHandle,
0,
&imageFrame );
if( FAILED( hr ) )
{
return;
}
//TODO: make depth image types optional
isNewFrames[FRAME_PLAYER_IDX_U8] = true;
isNewFrames[FRAME_DEPTH_U16] = true;
isNewFrames[FRAME_DEPTH_U8C3] = true;
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
BYTE * pixels = (BYTE*)renderedDepth.ptr();
USHORT * pBufferRun = (USHORT*) pBuffer;
ushort * pRaw = rawDepth.ptr<ushort>();
uchar * pIdx = playerIdx.ptr<uchar>();
for( int i =0; i<DEPTH_WIDTH*DEPTH_HEIGHT; i++,pBufferRun++,pRaw++,pIdx++)
{
//The low-order 3 bits (bits 0-2) contain the skeleton (player) ID.
//The high-order bits (bits 3¨C15) contain the depth value in millimeters.
Vec3b quad = Nui_ShortToQuad_Depth( *pBufferRun );
pixels[i*3+0] = quad.val[0];
pixels[i*3+1] = quad.val[1];
pixels[i*3+2] = quad.val[2];
*pRaw = NuiDepthPixelToDepth(*pBufferRun);//(*pBufferRun & 0xfff8) >> 3;
*pIdx = NuiDepthPixelToPlayerIndex(*pBufferRun);
}
char buf[10];
sprintf(buf,"fps: %d", fps_depth);
putText(renderedDepth, buf, Point(20,20), 0, 0.6, Scalar(255,255,255));
if (_delegate)
_delegate->onDepthData(rawDepth, renderedDepth, playerIdx);
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
m_pNuiSensor->NuiImageStreamReleaseFrame( m_pDepthStreamHandle, &imageFrame );
}
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;
}
void ConvertColorFrameToArray(const NUI_IMAGE_FRAME& frame, DataTypes::ColorImage& image)
{
assert(frame.eImageType == NUI_IMAGE_TYPE_COLOR);
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 % 4) == 0);
assert((lockedRect.Pitch % 4) == 0);
int cols = lockedRect.Pitch / 4;
int rows = lockedRect.size / lockedRect.Pitch;
int byteNum = 0;
for(int y=0; y<rows; y++)
{
for(int x=0; x<cols; x++)
{
const RgbPixel& pixel = (RgbPixel&)lockedRect.pBits[byteNum];
ColorPixel& color = image.data[y][x];
color.red = pixel.red();
color.green = pixel.green();
color.blue = pixel.blue();
byteNum += 4;
}
}
image.rows = rows;
image.cols = cols;
//////////////////////////////
}
// We're done with the texture so unlock it
texture->UnlockRect(0);
return;
}
void nextDepthFrame ()
{
NUI_IMAGE_FRAME depthFrame;
WinRet ret = sensor->NuiImageStreamGetNextFrame(depthStreamHandle, 0, &depthFrame);
if (FAILED(ret))
return;
BOOL nearModeOperational = false;
INuiFrameTexture* texture = 0;
ret = sensor->NuiImageFrameGetDepthImagePixelFrameTexture(depthStreamHandle, &depthFrame, &nearModeOperational, &texture);
if (FAILED(ret))
return;
NUI_LOCKED_RECT lockedRect;
texture->LockRect(0, &lockedRect, NULL, 0);
if (0 != lockedRect.Pitch)
{
NUI_SURFACE_DESC surfaceDesc;
texture->GetLevelDesc(0, &surfaceDesc);
const int width = surfaceDesc.Width;
const int height = surfaceDesc.Height;
NUI_DEPTH_IMAGE_PIXEL* extended_buf = reinterpret_cast<NUI_DEPTH_IMAGE_PIXEL*>(lockedRect.pBits);
ntk_assert(width == that->m_current_image.rawDepth16bits().cols, "Bad width");
ntk_assert(height == that->m_current_image.rawDepth16bits().rows, "Bad height");
if (that->m_align_depth_to_color)
{
QWriteLocker locker(&that->m_lock);
uint16_t* depth_buf = that->m_current_image.rawDepth16bitsRef().ptr<uint16_t>();
mapDepthFrameToRgbFrame(extended_buf, depth_buf);
}
else
{
QWriteLocker locker(&that->m_lock);
uint16_t* depth_buf = that->m_current_image.rawDepth16bitsRef().ptr<uint16_t>();
cv::Vec2w* depth_to_color_coords = that->m_current_image.depthToRgbCoordsRef().ptr<cv::Vec2w>();
extractDepthAndColorCoords (extended_buf, depth_buf, depth_to_color_coords);
}
}
else
{
debug(L"Buffer length of received texture is bogus\r\n");
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(depthStreamHandle, &depthFrame);
dirtyDepth = false;
}
/// <summary>
/// Process the incoming color frame
/// </summary>
void NuiColorStream::ProcessColor()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
// Attempt to get the color frame
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_hStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return;
}
if (m_paused)
{
// Stream paused. Skip frame process and release the frame.
goto ReleaseFrame;
}
INuiFrameTexture* pTexture = imageFrame.pFrameTexture;
// Lock the frame data so the Kinect knows not to modify it while we are reading it
NUI_LOCKED_RECT lockedRect;
pTexture->LockRect(0, &lockedRect, NULL, 0);
// Make sure we've received valid data
if (lockedRect.Pitch != 0)
{
switch (m_imageType)
{
case NUI_IMAGE_TYPE_COLOR_RAW_BAYER: // Convert raw bayer data to color image and copy to image buffer
m_imageBuffer.CopyBayer(lockedRect.pBits, lockedRect.size);
break;
case NUI_IMAGE_TYPE_COLOR_INFRARED: // Convert infrared data to color image and copy to image buffer
m_imageBuffer.CopyInfrared(lockedRect.pBits, lockedRect.size);
break;
default: // Copy color data to image buffer
m_imageBuffer.CopyRGB(lockedRect.pBits, lockedRect.size);
break;
}
if (m_pStreamViewer)
{
// Set image data to viewer
m_pStreamViewer->SetImage(&m_imageBuffer);
}
}
// Unlock frame data
pTexture->UnlockRect(0);
ReleaseFrame:
m_pNuiSensor->NuiImageStreamReleaseFrame(m_hStreamHandle, &imageFrame);
}
/// <summary>
/// Main processing function
/// </summary>
void CSkeletonBasics::Update()
{
if (NULL == m_pNuiSensor)
{
return;
}
// Wait for 0ms, just quickly test if it is time to process a skeleton
if ( WAIT_OBJECT_0 == WaitForSingleObject(m_hNextSkeletonEvent, 0) )
{
ProcessSkeleton();
}
if (WAIT_OBJECT_0 == WaitForSingleObject(hColorEvent, 0))
{
// Colorカメラからフレームを取得
hResult = m_pNuiSensor->NuiImageStreamGetNextFrame(hColorHandle, 0, &colorImageFrame);
if (FAILED(hResult)) {
std::cout << "Error : NuiImageStreamGetNextFrame( COLOR )" << std::endl;
return;
}
if (shutter == 1) {
// Color画像データの取得
INuiFrameTexture* pColorFrameTexture = colorImageFrame.pFrameTexture;
NUI_LOCKED_RECT colorLockedRect;
pColorFrameTexture->LockRect(0, &colorLockedRect, nullptr, 0);
// Retrieve the path to My Photos
WCHAR screenshotPath[MAX_PATH];
GetScreenshotFileName(screenshotPath, _countof(screenshotPath));
// Write out the bitmap to disk
hResult = SaveBitmapToFile(static_cast<BYTE *>(colorLockedRect.pBits), 640, 480, 32, screenshotPath);
if (SUCCEEDED(hResult))
{
std::cout << "saved" << std::endl;
}
else
{
std::cout << "unsave!!" << std::endl;
}
// フレームの解放
pColorFrameTexture->UnlockRect(0);
shutter = 0;
}
//リリース
pNuiSensor->NuiImageStreamReleaseFrame(hColorHandle, &colorImageFrame);
}
}
/// <summary>
/// Retrieve depth data from stream frame
/// </summary>
void NuiDepthStream::ProcessDepth()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
// Attempt to get the depth frame
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_hStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return;
}
if (m_paused)
{
// Stream paused. Skip frame process and release the frame.
goto ReleaseFrame;
}
BOOL nearMode;
INuiFrameTexture* pTexture;
// Get the depth image pixel texture
hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(m_hStreamHandle, &imageFrame, &nearMode, &pTexture);
if (FAILED(hr))
{
goto ReleaseFrame;
}
NUI_LOCKED_RECT lockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &lockedRect, NULL, 0);
// Make sure we've received valid data
if (lockedRect.Pitch != 0)
{
// Conver depth data to color image and copy to image buffer
m_imageBuffer.CopyDepth(lockedRect.pBits, lockedRect.size, nearMode, m_depthTreatment);
// Draw ou the data with Direct2D
if (m_pStreamViewer)
{
m_pStreamViewer->SetImage(&m_imageBuffer);
}
}
// Done with the texture. Unlock and release it
pTexture->UnlockRect(0);
pTexture->Release();
ReleaseFrame:
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_hStreamHandle, &imageFrame);
}
void storeNuiImage(void)
{
NUI_IMAGE_FRAME imageFrame;
if(WAIT_OBJECT_0 != WaitForSingleObject(hNextColorFrameEvent, 0)) return;
HRESULT hr = pNuiSensor->NuiImageStreamGetNextFrame(
pVideoStreamHandle,
0,
&imageFrame );
if( FAILED( hr ) ){
return;
}
if(imageFrame.eImageType != NUI_IMAGE_TYPE_COLOR)
STDERR("Image type is not match with the color\r\n");
INuiFrameTexture *pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 ){
byte * pBuffer = (byte *)LockedRect.pBits;
#if defined(USE_FACETRACKER)
setColorImage(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);
typedef struct t_RGBA{
byte r;
byte g;
byte b;
byte a;
};
t_RGBA *p = (t_RGBA *)pBuffer;
for(int i=0;i<pTexture->BufferLen()/4;i++){
byte b = p->b;
p->b = p->r;
p->r = b;
p->a = (byte)255;
p++;
}
glBindTexture(GL_TEXTURE_2D, bg_texture[IMAGE_TEXTURE]);
glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
pDesc.Width, pDesc.Height,
0, GL_RGBA, GL_UNSIGNED_BYTE, pBuffer);
pTexture->UnlockRect(0);
}else{
STDERR("Buffer length of received texture is bogus\r\n");
}
pNuiSensor->NuiImageStreamReleaseFrame( pVideoStreamHandle, &imageFrame );
}
/// <summary>
///
/// </summary>
void ProcessColor(){
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pStreamColorHandle, 0, &imageFrame);
if (FAILED(hr)) {
return;
}
INuiFrameTexture * pTexture = imageFrame.pFrameTexture;
NUI_LOCKED_RECT LockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
pTexture->LockRect(0, &LockedRect, NULL, 0);
// Make sure we've received valid data
if (LockedRect.Pitch != 0) {
cv::Mat colorFrame(cHeight, cWidth, CV_8UC3);
for(int i = 0; i < cHeight; i++) {
uchar *ptr = colorFrame.ptr<uchar>(i);
uchar *pBuffer = (uchar*)(LockedRect.pBits) + i * LockedRect.Pitch;
for(int j = 0; j < cWidth; j++) {
ptr[3*j] = pBuffer[4*j];
ptr[3*j+1] = pBuffer[4*j+1];
ptr[3*j+2] = pBuffer[4*j+2];
}
}
// Draw image
if (m_bShow) {
cv::imshow("Color", colorFrame);
cv::waitKey(1);
}
// If m_bRecord
if (m_bRecord) {
// Retrieve the path to My Photos
WCHAR screenshotPath[MAX_PATH];
// Write out the bitmap to disk
GetScreenshotFileName(screenshotPath, _countof(screenshotPath), COLOR);
std::wstring screenshotPathWStr(screenshotPath);
std::string screenshotPathStr(screenshotPathWStr.begin(), screenshotPathWStr.end());
cv::imwrite(screenshotPathStr, colorFrame);
}
}
pTexture->UnlockRect(0);
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pStreamColorHandle, &imageFrame);
}
void Nui_GotDepthAlert( )
{
const NUI_IMAGE_FRAME * pImageFrame = NULL;
HRESULT hr = m_pNuiInstance->NuiImageStreamGetNextFrame(
m_pDepthStreamHandle,
0,
&pImageFrame );
if( FAILED( hr ) )
{
return;
}
INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
pTexture->LockRect( 0, &LockedRect, NULL, 0 );
if( LockedRect.Pitch != 0 )
{
BYTE * pBuffer = (BYTE*) LockedRect.pBits;
BYTE * rgbrun = (BYTE*)kinect_depth_image->imageData;
// draw the bits to the bitmap
USHORT * pBufferRun = (USHORT*) pBuffer;
for( int y = 0 ; y < 240 ; y++ )
{
for( int x = 0 ; x < 320 ; x++ )
{
RGBQUAD quad = Nui_ShortToQuad_Depth( *pBufferRun );
pBufferRun++;
*rgbrun = quad.rgbBlue;
rgbrun++;
*rgbrun = quad.rgbGreen;
rgbrun++;
*rgbrun = quad.rgbRed;
rgbrun++;
}
}
}
else
{
OutputDebugString( L"Buffer length of received texture is bogus\r\n" );
}
m_pNuiInstance->NuiImageStreamReleaseFrame( m_pDepthStreamHandle, pImageFrame );
cvShowImage("Kinect depth image",kinect_depth_image);
cvWaitKey(10);
}
void storeNuiDepth(bool waitflag)
{
NUI_IMAGE_FRAME depthFrame;
if(waitflag) WaitForSingleObject(hNextDepthFrameEvent, INFINITE);
else 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 );
D3DLOCKED_RECT LPdest;
DepthTex->LockRect(0,&LPdest,NULL, 0);
if( LockedRect.Pitch != 0 ){
unsigned short *pBuffer = (unsigned short *)LockedRect.pBits;
unsigned char *pDestImage=(unsigned char*)LPdest.pBits;
NUI_SURFACE_DESC pDesc;
pTexture->GetLevelDesc(0, &pDesc);
unsigned short *p = (unsigned short *)pBuffer;
for(int y=0;y<60;y++){
for(int x=0;x<80;x++){
unsigned char depth = (unsigned char)((*pBuffer & 0xff00)>>8);
unsigned short playerID = NuiDepthPixelToPlayerIndex(*pBuffer);
*pDestImage = (unsigned char)(Colors[playerID][0] * depth);
pDestImage++;
*pDestImage = (unsigned char)(Colors[playerID][1] * depth);
pDestImage++;
*pDestImage = (unsigned char)(Colors[playerID][2] * depth);
pDestImage++;
*pDestImage = 255;
pDestImage++;
pBuffer++;
}
pDestImage += (128-80)*4;
}
DepthTex->UnlockRect(0);
pTexture->UnlockRect(0);
}
/* Function: getColorImage
* Inputs:
colorEvent - a reference to a handle for the color event
colorStreamHandle - a reference to a handle for the color stream
colorImage - a reference to the Mat for color image
* Outputs: void
*/
void getColorImage(HANDLE &colorEvent, HANDLE &colorStreamHandle, Mat &colorImage)
{
//initialize structure containing all metadata about the frame: number, resolution, etc.
const NUI_IMAGE_FRAME *colorFrame = NULL;
//gets the next fram of data from 'colorStreamHandle', waits 0 ms before returning
// and passes a reference to a NUI_IMAGE_FRAME 'colorFrame' that contains next image frame
//return value: HRESULT
NuiImageStreamGetNextFrame(colorStreamHandle, 0, &colorFrame);
//manages the frame data
INuiFrameTexture *pTexture = colorFrame->pFrameTexture;
//pointer to actual data
NUI_LOCKED_RECT LockedRect;
//lock frame data so Kinect knows not to modify it while we're reading
pTexture->LockRect(0, &LockedRect, NULL, 0);
//if # of bytes in 1 row of texture resource != 0, proceed
//make sure we've received valid data
if( LockedRect.Pitch != 0 )
{
//loop through the rows of the color image to receive byte/pixel data
for (int i=0; i<colorImage.rows; i++)
{
//unsigned char ptr returns pointer to specified row matrix
uchar *ptr = colorImage.ptr<uchar>(i);
//pbuffer = bits in lockedrect + currentrow# * bytes in pitch
uchar *pBuffer = (uchar*)(LockedRect.pBits) + i * LockedRect.Pitch;
for (int j=0; j<colorImage.cols; j++) //loop through the columns in Mat colorIMage
{
ptr[3*j] = pBuffer[4*j];
ptr[3*j+1] = pBuffer[4*j+1];
ptr[3*j+2] = pBuffer[4*j+2];
}
}
}//end of if statement
else
{
cout<<"Failed to receive valid data from LockedRect!"<<endl;
}
//unlocks the pTexture buffer
pTexture->UnlockRect(0);
//releases colorStream handle and color frame
NuiImageStreamReleaseFrame(colorStreamHandle, colorFrame);
}