void DepthSensor::processDepth()
{
HRESULT hr;
NUI_IMAGE_FRAME imageFrame;
//get the depth frame
hr = mNuiSensor->NuiImageStreamGetNextFrame(mDepthStreamHandle, 500, &imageFrame);
if (FAILED(hr))
{
throw std::runtime_error("NuiImageStreamGetNextFrame failed");
}
hr = CopyExtendedDepth(imageFrame);
LARGE_INTEGER currentDepthFrameTime = imageFrame.liTimeStamp;
// Release the Kinect camera frame
mNuiSensor->NuiImageStreamReleaseFrame(mDepthStreamHandle, &imageFrame);
// To enable playback of a .xed file through Kinect Studio and reset of the reconstruction
// if the .xed loops, we test for when the frame timestamp has skipped a large number.
// Note: this will potentially continually reset live reconstructions on slow machines which
// cannot process a live frame in less time than the reset threshold. Increase the number of
// milliseconds in cResetOnTimeStampSkippedMilliseconds if this is a problem.
if (m_bAutoResetReconstructionOnTimeout && m_cFrameCounter != 0
&& abs(currentDepthFrameTime.QuadPart - m_cLastDepthFrameTimeStamp.QuadPart) > cResetOnTimeStampSkippedMilliseconds)
{
ResetReconstruction();
if (FAILED(hr))
{
return;
}
}
m_cLastDepthFrameTimeStamp = currentDepthFrameTime;
// Return if the volume is not initialized
if (nullptr == m_pVolume)
{
throw std::runtime_error("Kinect Fusion reconstruction volume not initialized. Please try reducing volume size or restarting.");
return;
}
////////////////////////////////////////////////////////
// Depth to DepthFloat
// Convert the pixels describing extended depth as unsigned short type in millimeters to depth
// as floating point type in meters.
hr = m_pVolume->DepthToDepthFloatFrame(m_pDepthImagePixelBuffer, cDepthImagePixels * sizeof(NUI_DEPTH_IMAGE_PIXEL), m_pDepthFloatImage, m_fMinDepthThreshold, m_fMaxDepthThreshold, m_bMirrorDepthFrame);
if (FAILED(hr))
{
throw std::runtime_error("Kinect Fusion NuiFusionDepthToDepthFloatFrame call failed.");
return;
}
////////////////////////////////////////////////////////
// ProcessFrame
// Perform the camera tracking and update the Kinect Fusion Volume
// This will create memory on the GPU, upload the image, run camera tracking and integrate the
// data into the Reconstruction Volume if successful. Note that passing nullptr as the final
// parameter will use and update the internal camera pose.
hr = m_pVolume->ProcessFrame(m_pDepthFloatImage, NUI_FUSION_DEFAULT_ALIGN_ITERATION_COUNT, m_cMaxIntegrationWeight, &m_worldToCameraTransform);
if (SUCCEEDED(hr))
{
Matrix4 calculatedCameraPose;
hr = m_pVolume->GetCurrentWorldToCameraTransform(&calculatedCameraPose);
if (SUCCEEDED(hr))
{
// Set the pose
m_worldToCameraTransform = calculatedCameraPose;
m_cLostFrameCounter = 0;
m_bTrackingFailed = false;
}
}
else
{
if (hr == E_NUI_FUSION_TRACKING_ERROR)
{
m_cLostFrameCounter++;
m_bTrackingFailed = true;
std::cout << "Kinect Fusion camera tracking failed! Align the camera to the last tracked position. " << std::endl;
}
else
{
throw std::runtime_error("Kinect Fusion ProcessFrame call failed!");
return;
}
}
if (m_bAutoResetReconstructionWhenLost && m_bTrackingFailed && m_cLostFrameCounter >= cResetOnNumberOfLostFrames)
{
// Automatically clear volume and reset tracking if tracking fails
hr = ResetReconstruction();
if (FAILED(hr))
{
return;
}
// Set bad tracking message
std::cout << "Kinect Fusion camera tracking failed, automatically reset volume" << std::endl;
}
////////////////////////////////////////////////////////
// CalculatePointCloud
// Raycast all the time, even if we camera tracking failed, to enable us to visualize what is happening with the system
hr = m_pVolume->CalculatePointCloud(m_pPointCloud, &m_worldToCameraTransform);
if (FAILED(hr))
{
throw std::runtime_error("Kinect Fusion CalculatePointCloud call failed.");
return;
}
////////////////////////////////////////////////////////
// ShadePointCloud and render
hr = NuiFusionShadePointCloud(m_pPointCloud, &m_worldToCameraTransform, nullptr, m_pShadedSurface, nullptr);
if (FAILED(hr))
{
throw std::runtime_error("Kinect Fusion NuiFusionShadePointCloud call failed.");
return;
}
// Draw the shaded raycast volume image
INuiFrameTexture * pShadedImageTexture = m_pShadedSurface->pFrameTexture;
NUI_LOCKED_RECT ShadedLockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
hr = pShadedImageTexture->LockRect(0, &ShadedLockedRect, nullptr, 0);
if (FAILED(hr))
{
return;
}
// Make sure we've received valid data
if (ShadedLockedRect.Pitch != 0)
{
BYTE * pBuffer = (BYTE *)ShadedLockedRect.pBits;
// Draw the data with vtk
mDrawDepth->Draw(pBuffer, cDepthWidth , cDepthHeight , cBytesPerPixel);
if (!m_isInit)
{
mDrawDepth->Actor->GetMapper()->SetInputData(mDrawDepth->image);
mDrawDepth->renderer->AddActor(mDrawDepth->Actor);
m_isInit = true;
}
mDrawDepth->renWin->Render();
}
// We're done with the texture so unlock it
pShadedImageTexture->UnlockRect(0);
//////////////////////////////////////////////////////////
//// Periodically Display Fps
//// Update frame counter
//m_cFrameCounter++;
//// Display fps count approximately every cTimeDisplayInterval seconds
//double elapsed = m_timer.AbsoluteTime() - m_fStartTime;
//if ((int)elapsed >= cTimeDisplayInterval)
//{
// double fps = (double)m_cFrameCounter / elapsed;
// // Update status display
// if (!m_bTrackingFailed)
// {
// WCHAR str[MAX_PATH];
// swprintf_s(str, ARRAYSIZE(str), L"Fps: %5.2f", fps);
//
// cout<<str<<endl;
// }
// m_cFrameCounter = 0;
// m_fStartTime = m_timer.AbsoluteTime();
//}
}
/// <summary>
/// Handle new depth data and perform Kinect Fusion processing
/// </summary>
void CKinectFusion::ProcessDepth()
{
if (m_bInitializeError)
{
return;
}
HRESULT hr = S_OK;
NUI_IMAGE_FRAME imageFrame;
////////////////////////////////////////////////////////
// Get an extended depth frame from Kinect
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle, 0, &imageFrame);
if (FAILED(hr))
{
std::cout << "Kinect NuiImageStreamGetNextFrame call failed." << std::endl;
return;
}
hr = CopyExtendedDepth(imageFrame);
LARGE_INTEGER currentFrameTime = imageFrame.liTimeStamp;
// Release the Kinect camera frame
m_pNuiSensor->NuiImageStreamReleaseFrame(m_pDepthStreamHandle, &imageFrame);
if (FAILED(hr))
{
return;
}
// To enable playback of a .xed file through Kinect Studio and reset of the reconstruction
// if the .xed loops, we test for when the frame timestamp has skipped a large number.
// Note: this will potentially continually reset live reconstructions on slow machines which
// cannot process a live frame in less time than the reset threshold. Increase the number of
// milliseconds in cResetOnTimeStampSkippedMilliseconds if this is a problem.
if (m_cFrameCounter != 0 && abs(currentFrameTime.QuadPart - m_cLastFrameTimeStamp.QuadPart) > cResetOnTimeStampSkippedMilliseconds)
{
ResetReconstruction();
if (FAILED(hr))
{
return;
}
}
m_cLastFrameTimeStamp = currentFrameTime;
////////////////////////////////////////////////////////
// Depth to DepthFloat
// Convert the pixels describing extended depth as unsigned short type in millimeters to depth
// as floating point type in meters.
hr = NuiFusionDepthToDepthFloatFrame(m_pDepthImagePixelBuffer, m_cDepthWidth, m_cDepthHeight, m_pDepthFloatImage, m_fMinDepthThreshold, m_fMaxDepthThreshold, m_bMirrorDepthFrame);
if (FAILED(hr))
{
std::cout << "Kinect Fusion NuiFusionDepthToDepthFloatFrame call failed." << std::endl;
return;
}
// Return if the volume is not initialized
if (nullptr == m_pVolume)
{
std::cout << "Kinect Fusion reconstruction volume not initialized. Please try reducing volume size or restarting." << std::endl;
return;
}
////////////////////////////////////////////////////////
// ProcessFrame
// Perform the camera tracking and update the Kinect Fusion Volume
// This will create memory on the GPU, upload the image, run camera tracking and integrate the
// data into the Reconstruction Volume if successful. Note that passing nullptr as the final
// parameter will use and update the internal camera pose.
//hr = m_pVolume->IntegrateFrame(m_pDepthFloatImage, 1, &m_cameraTransform);
hr = m_pVolume->ProcessFrame(m_pDepthFloatImage, NUI_FUSION_DEFAULT_ALIGN_ITERATION_COUNT, m_cMaxIntegrationWeight, &m_worldToCameraTransform);
// Test to see if camera tracking failed.
// If it did fail, no data integration or raycast for reference points and normals will have taken
// place, and the internal camera pose will be unchanged.
if (FAILED(hr))
{
if (hr == E_NUI_FUSION_TRACKING_ERROR)
{
m_cLostFrameCounter++;
m_bTrackingFailed = true;
std::cout << "Kinect Fusion camera tracking failed! Align the camera to the last tracked position. " << std::endl;
}
else
{
std::cout << "Kinect Fusion ProcessFrame call failed!" << std::endl;
return;
}
}
else
{
Matrix4 calculatedCameraPose;
hr = m_pVolume->GetCurrentWorldToCameraTransform(&calculatedCameraPose);
if (SUCCEEDED(hr))
{
// Set the pose
m_worldToCameraTransform = calculatedCameraPose;
m_cLostFrameCounter = 0;
m_bTrackingFailed = false;
}
}
if (m_bAutoResetReconstructionWhenLost && m_bTrackingFailed && m_cLostFrameCounter >= cResetOnNumberOfLostFrames)
{
// Automatically clear volume and reset tracking if tracking fails
hr = ResetReconstruction();
if (FAILED(hr))
{
return;
}
// Set bad tracking message
std::cout << "Kinect Fusion camera tracking failed, automatically reset volume." << std::endl;
}
////////////////////////////////////////////////////////
// CalculatePointCloud
// Raycast all the time, even if we camera tracking failed, to enable us to visualize what is happening with the system
hr = m_pVolume->CalculatePointCloud(m_pPointCloud, &m_cameraTransform);
if (FAILED(hr))
{
std::cout << "Kinect Fusion CalculatePointCloud call failed." << std::endl;
return;
}
////////////////////////////////////////////////////////
// ShadePointCloud and render
hr = NuiFusionShadePointCloud(m_pPointCloud, &m_cameraTransform, nullptr, m_pShadedSurface, nullptr);
if (FAILED(hr))
{
std::cout << "Kinect Fusion NuiFusionShadePointCloud call failed." << std::endl;
return;
}
// Draw the shaded raycast volume image
INuiFrameTexture * pShadedImageTexture = m_pShadedSurface->pFrameTexture;
NUI_LOCKED_RECT ShadedLockedRect;
// Lock the frame data so the Kinect knows not to modify it while we're reading it
hr = pShadedImageTexture->LockRect(0, &ShadedLockedRect, nullptr, 0);
if (FAILED(hr))
{
return;
}
// Make sure we've received valid data
if (ShadedLockedRect.Pitch != 0)
{
if (m_pImageData != 0) {
BYTE * pBuffer = (BYTE *)ShadedLockedRect.pBits;
const BYTE* dataEnd = pBuffer + (640*480*4);
GLubyte* dest = m_pImageData;
while (pBuffer < dataEnd)
*dest++ = *pBuffer++;
}
}
// We're done with the texture so unlock it
pShadedImageTexture->UnlockRect(0);
////////////////////////////////////////////////////////
// Periodically Display Fps
// Update frame counter
m_cFrameCounter++;
// Display fps count approximately every cTimeDisplayInterval seconds
}
