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 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 );
}
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);
}
//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 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);
}
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);
}
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;
}
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;
}
/// <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 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);
}
/// <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;
}
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>
///
/// </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 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>
/// 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);
}
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 KinectStreamImpl::mainLoop()
{
m_running = TRUE;
while (m_running)
{
if ( WAIT_OBJECT_0 == WaitForSingleObject(m_hNextFrameEvent, LOOP_TIMEOUT) && m_running)
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
OniDriverFrame* pFrame = NULL;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_hStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
continue;
}
INuiFrameTexture * pTexture;
if (m_sensorType == ONI_SENSOR_DEPTH)
{
BOOL nearMode;
// Get the depth image pixel texture
hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(
m_hStreamHandle, &imageFrame, &nearMode, &pTexture);
}
else
{
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);
if (LockedRect.Pitch != 0)
{
List<BaseKinectStream*>::ConstIterator iter = m_streamList.Begin();
while( iter != m_streamList.End())
{
if (((BaseKinectStream*)(*iter))->isRunning())
((BaseKinectStream*)(*iter))->frameReceived(imageFrame, LockedRect);
++iter;
}
}
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_hStreamHandle, &imageFrame);
}
}
return;
}
/* 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);
}
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);
}
bool KinectCapture::GetDepthImage(cv::Mat & image)
{
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT lockedRect;
HRESULT hr;
image = cv::Mat::zeros(m_ColorHeight, m_ColorWidth, CV_16U);
// Get a depth frame from Kinect
hr = m_NuiSensor->NuiImageStreamGetNextFrame(m_DepthStreamHandle, 30, &imageFrame);
if (FAILED(hr))
{
if (hr == E_NUI_FRAME_NO_DATA)
{
return false;
}
else
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed." << std::endl;
}
return false;
}
INuiFrameTexture* depthTexture = imageFrame.pFrameTexture;
// Lock the frame data to access depth data
hr = depthTexture->LockRect(0, &lockedRect, NULL, 0);
if (FAILED(hr) || lockedRect.Pitch == 0)
{
std::cout << "Error getting depth texture pixels." << std::endl;
return false;
}
// Copy the depth pixels so we can return the image frame
errno_t err = memcpy_s(m_depthImagePixelBuffer, m_DepthImagePixels * sizeof(USHORT), lockedRect.pBits, depthTexture->BufferLen());
depthTexture->UnlockRect(0);
if (0 != err)
{
std::cout << "Error copying depth texture pixels." << std::endl;
return false;
}
// Release the Kinect camera frame
m_NuiSensor->NuiImageStreamReleaseFrame(m_DepthStreamHandle, &imageFrame);
if (FAILED(hr))
{
std::cout << "Kinect Depth stream NuiImageStreamReleaseFrame call failed." << std::endl;
return false;
}
cv::Mat depthImg(m_DepthHeight,m_DepthWidth,CV_16U, m_depthImagePixelBuffer);
image = depthImg;
return true;
}
void SimpleKinect::UpdateDepth()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 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_pPrevDepth, m_pCurrentDepth, LockedRect.size);
// copy in data to current
memcpy(m_pCurrentDepth, LockedRect.pBits, LockedRect.size);
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pDepthStreamHandle, &imageFrame);
// update the mapping from depth to color
hr = m_pNuiSensor->NuiImageGetColorPixelCoordinateFrameFromDepthPixelFrameAtResolution(
NUI_IMAGE_RESOLUTION_640x480,
NUI_IMAGE_RESOLUTION_640x480,
cDepthWidth * cDepthHeight,
m_pCurrentDepth,
cDepthWidth * cDepthHeight * 2,
m_pDepthToColorCoordinates
);
if(FAILED(hr))
{
return;
}
}
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 KinectImageSource::next() {
#ifdef WITH_MSKINECT_SDK
// Attempt to get the color frame
HRESULT hr;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return false;
}
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);
frame = Mat(480, 640, CV_8UC4, static_cast<BYTE *>(LockedRect.pBits));
// Write out the bitmap to disk
//static_cast<BYTE *>(LockedRect.pBits), cColorWidth, cColorHeight, 32
//BYTE* pBitmapBits, LONG lWidth, LONG lHeight, WORD wBitsPerPixel
/// <param name="pBitmapBits">image data to save</param>
/// <param name="lWidth">width (in pixels) of input image data</param>
/// <param name="lHeight">height (in pixels) of input image data</param>
/// <param name="wBitsPerPixel">bits per pixel of image data</param>
/// Standard RGB, no compression
}
// We're done with the texture so unlock it
pTexture->UnlockRect(0);
// Release the frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
return true;
#else
std::cerr << "Error! This is the Microsoft Kinect SDK interface and not available under Linux." << std::endl;
return false;
#endif
}
void Helper::depthFloatToPointCloud(::NUI_FUSION_IMAGE_FRAME * pDepthFloatImage, PCloud &outCloud)
{
INuiFrameTexture *imageFrameTexture = pDepthFloatImage->pFrameTexture;
NUI_LOCKED_RECT LockedRect;
imageFrameTexture->LockRect(0, &LockedRect, nullptr, 0);
if (LockedRect.Pitch) {
assert(NUI_FUSION_IMAGE_TYPE_FLOAT == pDepthFloatImage->imageType);
outCloud.width = pDepthFloatImage->width * pDepthFloatImage->height;
outCloud.height = 1;
outCloud.is_dense = true;
outCloud.points.reserve(outCloud.width * outCloud.height);
const float *pFloatBuffer = reinterpret_cast<float *>(LockedRect.pBits);
const float cx = 319.5f;// pDepthFloatImage->pCameraParameters->principalPointX;
const float cy = 239.5f;// pDepthFloatImage->pCameraParameters->principalPointY;
const float fx = 1.0 / 571.4;// pDepthFloatImage->pCameraParameters->focalLengthX;
const float fy = 1.0 / 571.4;// pDepthFloatImage->pCameraParameters->focalLengthY;
//Concurrency::parallel_for(0u, pDepthFloatImage->height, [&](unsigned int y)
for (unsigned int y = 0; y < pDepthFloatImage->height; ++y)
{
const float* pFloatRow = reinterpret_cast<const float*>(reinterpret_cast<const unsigned char*>(pFloatBuffer)+(y * LockedRect.Pitch));
for (unsigned int x = 0; x < pDepthFloatImage->width; ++x)
{
float depth = pFloatRow[x];
if (depth < 1e-6 || depth > 10.) {
continue;
}
float vx = (1.f * x - cx) * fx * depth;
float vy = (1.f * y - cy) * fy * depth;
float vz = depth;
outCloud.push_back(PCloud::PointType(vx, vy, vz));
}
}
//);
}
imageFrameTexture->UnlockRect(0);
outCloud.width = outCloud.points.size();
}
char* KTM::KinectWrapper::getColorFromKinectStream(){
HRESULT hr;
/* Get the next frame from the Kinect stream */
NUI_IMAGE_FRAME imageFrame;
hr = pKinectSensor->NuiImageStreamGetNextFrame(hColorStreamHandle, 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);
/* Use the assigned data heap to avoid memory leaks and avoid */
/* needlessly assigning new memory */
char* mFrameData = mRGBDataHeap;
/* Make sure we have valid data */
if (lockedRect.Pitch != 0){
/* Pointer to the top left corner of rectangle, and pointer */
/* to the frame end */
const char* pBufferRun = (const char*)lockedRect.pBits;
const char* pBufferEnd = pBufferRun + (RGBFrameWidth * RGBFrameHeight * 4);
if(OUT_FRAME_CHANNELS == 4){
memcpy(mFrameData, pBufferRun, pBufferEnd - pBufferRun);
}else{
int s = pBufferEnd - pBufferRun;
for(int i = 0, j = 0; i < s; i++){
mFrameData[j++] = pBufferRun[i++];
mFrameData[j++] = pBufferRun[i++];
mFrameData[j++] = pBufferRun[i++];
}
}
}
/* Unlock the rectangle and release the frame */
pTexture->UnlockRect(0);
pKinectSensor->NuiImageStreamReleaseFrame(hColorStreamHandle, &imageFrame);
return mFrameData;
}
/// <summary>
/// Handle new depth data
/// </summary>
/// <returns>S_OK on success, otherwise failure code</returns>
HRESULT CBackgroundRemovalBasics::ProcessDepth()
{
HRESULT hr;
HRESULT bghr = S_OK;
NUI_IMAGE_FRAME imageFrame;
// Attempt to get the depth frame
LARGE_INTEGER depthTimeStamp;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
return hr;
}
depthTimeStamp = imageFrame.liTimeStamp;
INuiFrameTexture* pTexture;
// Attempt to get the extended depth texture
hr = m_pNuiSensor->NuiImageFrameGetDepthImagePixelFrameTexture(m_pDepthStreamHandle, &imageFrame, &m_bNearMode, &pTexture);
if (FAILED(hr))
{
return hr;
}
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, and then present it to the background removed color stream.
if (LockedRect.Pitch != 0)
{
bghr = m_pBackgroundRemovalStream->ProcessDepth(m_depthWidth * m_depthHeight * cBytesPerPixel, LockedRect.pBits, depthTimeStamp);
}
// We're done with the texture so unlock it. Even if above process failed, we still need to unlock and release.
pTexture->UnlockRect(0);
pTexture->Release();
// Release the frame
hr = m_pNuiSensor->NuiImageStreamReleaseFrame(m_pDepthStreamHandle, &imageFrame);
if (FAILED(bghr))
{
return bghr;
}
return hr;
}
void nextColorFrame ()
{
NUI_IMAGE_FRAME colorFrame;
WinRet ret = sensor->NuiImageStreamGetNextFrame(colorStreamHandle, 0, &colorFrame);
if (FAILED(ret))
return;
INuiFrameTexture* texture = colorFrame.pFrameTexture;
NUI_LOCKED_RECT lockedRect;
texture->LockRect(0, &lockedRect, NULL, 0);
if (lockedRect.Pitch != 0)
{
NUI_SURFACE_DESC surfaceDesc;
texture->GetLevelDesc(0, &surfaceDesc);
const int width = surfaceDesc.Width;
const int height = surfaceDesc.Height;
ntk_assert( width == that->m_current_image.rawRgb().cols, "Bad width");
ntk_assert(height == that->m_current_image.rawRgb().rows, "Bad height");
uint8_t* buf = static_cast<uint8_t*>(lockedRect.pBits);
// size_t size = size_t(lockedRect.size);
{
QWriteLocker locker(&that->m_lock);
cvtColor(cv::Mat4b(height, width, (cv::Vec4b*)buf), that->m_current_image.rawRgbRef(), CV_RGBA2RGB);
}
// std::copy(buf, buf + 3 * width * height, (uint8_t*) that->m_current_image.rawRgbRef().ptr());
// cvtColor(that->m_current_image.rawRgb(), that->m_current_image.rawRgbRef(), CV_RGB2BGR);
// m_pDrawColor->Draw( static_cast<BYTE *>(LockedRect.pBits), LockedRect.size );
}
else
{
debug(L"Buffer length of received texture is bogus\r\n");
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(colorStreamHandle, &colorFrame);
dirtyColor = false;
}
