bool KinectInterface::capture_frame() {
IMultiSourceFrame *frame = nullptr;
cv::Mat intensity, depth, pos;
if (getFrameData(frame, intensity, depth, pos)) {
cv::imshow("intensity", intensity);
cv::Mat depth_f;
depth.convertTo(depth_f, CV_32FC1);
cv::imshow("depth", depth_f / 6500);
if (frame) frame->Release();
return true;
}
return false;
}
void Microsoft2Grabber::FrameArrived(IMultiSourceFrameArrivedEventArgs *pArgs, bool depthOnly) {
HRESULT hr;
IMultiSourceFrameReference *pFrameReference = nullptr;
//cout << "got a valid frame" << endl;
hr = pArgs->get_FrameReference(&pFrameReference);
if (SUCCEEDED(hr))
{
IMultiSourceFrame *pFrame = nullptr;
hr = pFrameReference->AcquireFrame(&pFrame);
if (FAILED(hr)) {
cout << "fail on AcquireFrame" << endl;
}
IColorFrameReference* pColorFrameReference = nullptr;
IDepthFrameReference* pDepthFrameReference = nullptr;
IBodyIndexFrameReference* pBodyIndexFrameReference = nullptr;
hr = pFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
DepthFrameArrived(pDepthFrameReference);
SafeRelease(pDepthFrameReference);
if (!depthOnly) {
hr = pFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
ColorFrameArrived(pColorFrameReference);
SafeRelease(pColorFrameReference);
hr = pFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
if (SUCCEEDED(hr))
BodyIndexFrameArrived(pBodyIndexFrameReference);
SafeRelease(pBodyIndexFrameReference);
}
pFrameReference->Release();
}
}
bool KinectV2Controller::UpdateCamera()
{
if(InitializeMultiFrameReader())
{
IMultiSourceFrame* pMultiSourceFrame = NULL;
IDepthFrame* pDepthFrame = NULL;
IColorFrame* pColorFrame = NULL;
IInfraredFrame* pInfraRedFrame = NULL;
HRESULT hr = -1;
static DWORD lastTime = 0;
DWORD currentTime = GetTickCount();
//Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS.
if( unsigned int(currentTime - lastTime) > 33 )
{
hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
lastTime = currentTime;
}
if (SUCCEEDED(hr))
{
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pDepthFrameReference);
}
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
SafeRelease(pColorFrameReference);
}
if (SUCCEEDED(hr))
{
IInfraredFrameReference* pInfraredFrameReference = NULL;
hr = pMultiSourceFrame->get_InfraredFrameReference(&pInfraredFrameReference);
if (SUCCEEDED(hr))
{
hr = pInfraredFrameReference->AcquireFrame(&pInfraRedFrame);
}
SafeRelease(pInfraredFrameReference);
}
if (SUCCEEDED(hr))
{
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = NULL;
UINT16 *pIntraRedBuffer = NULL;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
if (SUCCEEDED(hr))
{
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
}
if (SUCCEEDED(hr))
{
hr = pInfraRedFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pIntraRedBuffer);
}
if (SUCCEEDED(hr))
{
for(int i = 0; i < d->m_DepthCaptureHeight*d->m_DepthCaptureWidth; ++i)
{
d->m_Distances[i] = static_cast<float>(*pDepthBuffer);
d->m_Amplitudes[i] = static_cast<float>(*pIntraRedBuffer);
++pDepthBuffer;
++pIntraRedBuffer;
}
}
else
{
MITK_ERROR << "AccessUnderlyingBuffer";
}
// get color frame data
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (d->m_pColorRGBX)
{
pColorBuffer = d->m_pColorRGBX;
nColorBufferSize = d->m_RGBCaptureWidth * d->m_RGBCaptureHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
if (SUCCEEDED(hr))
{
for(int i = 0; i < d->m_RGBBufferSize; i+=3)
{
//convert from BGR to RGB
d->m_Colors[i+0] = pColorBuffer->rgbRed;
d->m_Colors[i+1] = pColorBuffer->rgbGreen;
d->m_Colors[i+2] = pColorBuffer->rgbBlue;
++pColorBuffer;
}
}
}
}
SafeRelease(pDepthFrame);
SafeRelease(pColorFrame);
SafeRelease(pInfraRedFrame);
SafeRelease(pMultiSourceFrame);
if( hr != -1 && !SUCCEEDED(hr) )
{
//The thread gets here, if the data is requested faster than the device can deliver it.
//This may happen from time to time.
return false;
}
return true;
}
MITK_ERROR << "Unable to initialize MultiFrameReader";
return false;
}
void capture(Image::Ptr& pImage)
{
HRESULT hr;
if (m_pMultiSourceFrameReader==nullptr)
{
camera->getContext().error("CameraKinectDevice::capture: m_pMultiSourceFrameReader is nullptr\n");
// this is bad news - perhaps throw?
return; // @@@
}
IMultiSourceFrame* pMultiSourceFrame = nullptr;
IDepthFrame* pDepthFrame = nullptr;
IColorFrame* pColorFrame = nullptr;
const golem::MSecTmU32 waitStep = 1;
golem::MSecTmU32 timeWaited = 0;
golem::Sleep timer;
while (FAILED(hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame)))
{
// this is in CameraOpenNI, but suspect may be causing problem here
// if (camera->isTerminating()) return;
timer.msleep(waitStep);
timeWaited += waitStep;
if (timeWaited >= timeout)
{
camera->getContext().error("CameraKinectDevice::capture: failed to acquire frame within %d ms\n", timeout);
// keep going - don't return with nothing; reset stopwatch @@@
timeWaited = 0;
}
}
const golem::SecTmReal systemTime1 = camera->getContext().getTimer().elapsed();
if (SUCCEEDED(hr))
{
IDepthFrameReference* pDepthFrameReference = nullptr;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
RELEASE_PTR(pDepthFrameReference);
}
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = nullptr;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
RELEASE_PTR(pColorFrameReference);
}
if (SUCCEEDED(hr))
{
INT64 nDepthTime = 0;
IFrameDescription* pDepthFrameDescription = nullptr;
int nDepthWidth = 0;
int nDepthHeight = 0;
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = nullptr;
IFrameDescription* pColorFrameDescription = nullptr;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = nullptr;
// get depth frame data
hr = pDepthFrame->get_RelativeTime(&nDepthTime);
if (SUCCEEDED(hr))
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
if (SUCCEEDED(hr))
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
if (SUCCEEDED(hr))
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
if (SUCCEEDED(hr))
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
// get color frame data
if (SUCCEEDED(hr))
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
if (SUCCEEDED(hr))
hr = pColorFrameDescription->get_Width(&nColorWidth);
if (SUCCEEDED(hr))
hr = pColorFrameDescription->get_Height(&nColorHeight);
if (SUCCEEDED(hr))
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (m_pColorRGBX)
{
pColorBuffer = m_pColorRGBX;
nColorBufferSize = nColorWidth * nColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
cv::Mat colorFrame;
colorFrame.create(cColorHeight, cColorWidth, CV_8UC4);
colorFrame.setTo(cv::Scalar(0, 0, 0, 0));
//copying color buffer into cv::mat
memcpy(colorFrame.data, pColorBuffer, (nColorWidth * nColorHeight) * 4);
//originally kinect has mirrored image
cv::flip(colorFrame, colorFrame, 1);
//converstion to pImage which is BGR
cv::Mat colorFrameT;
cv::cvtColor(colorFrame, colorFrameT, CV_BGRA2BGR);
if (mode & CameraKinect::MODE_COLOUR)
{
// initialize buffer
if (pImage == nullptr)
pImage.reset(new Image());
pImage->reserve((int)1920, (int)1080);
//printf("Before clonning frame\n");
IplImage * img = cvCloneImage(&(IplImage)colorFrameT);
cvCopy(img, pImage->image);
cvReleaseImage(&img);
//printf("After clonning frame\n");
const golem::SecTmReal systemTime2 = camera->getContext().getTimer().elapsed();
//seting timestamp
pImage->timeStamp = REAL_HALF*(systemTime1 + systemTime2);
}
//camera->getContext().debug("After colour\n");
//needed for retain information of the begining of the buffer
const UINT16* pBufferStart = pDepthBuffer;
const UINT16* pBufferEnd = pDepthBuffer + (nDepthWidth * nDepthHeight);
//copying depth buffer into cv::Mat
long depthSizeP = (pBufferEnd - pBufferStart) * 2;
cv::Mat depthFrame;
depthFrame.create(cDepthHeight, cDepthWidth, CV_16UC1);
depthFrame.setTo(cv::Scalar(0));
memcpy(depthFrame.data, pBufferStart, depthSizeP);
//originally kinect has mirrored image
cv::flip(depthFrame, depthFrame, 1);
//camera->getContext().debug("After getting depth data\n");
//depth mode color data mapped into camera space (remember high resolution 1920x1080)
if (mode & CameraKinect::MODE_DEPTH && (std::stoi(this->property.format.c_str()) == 101))
{
//camera->getContext().debug("In depth mode\n");
if (pImage == nullptr)
pImage.reset(new Image());
pImage->reserve((int)1920, (int)1080);
pImage->cloud.reset(new PointSeq((uint32_t)property.width, (uint32_t)property.height));
//Obtaining the pointcloud
hasImageStream = false;
float missingPoint = std::numeric_limits<float>::quiet_NaN();
hr = m_pCoordinateMapper->MapColorFrameToCameraSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, cColorHeight * cColorWidth, m_pCameraCoordinatesColor);
golem::Point* pOutWidth = &pImage->cloud->front();
CameraSpacePoint* pCamCWidth = m_pCameraCoordinatesColor;
for (int y = 0; y < cColorHeight; ++y)
{
golem::Point* pOut = pOutWidth;
CameraSpacePoint* pCamC = pCamCWidth;
for (int x = 0; x < cColorWidth; ++x, pCamC++, ++pOut) {
cv::Vec4b color;
int abc = pCamC->Z;
if (abc != 0) {
pOut->x = pCamC->X;
pOut->y = pCamC->Y;
pOut->z = pCamC->Z;
}
else {
pOut->x = pOut->y = pOut->z = missingPoint;
//printf("Getting to this point4\n");
}
pOut->normal_x = pOut->normal_y = pOut->normal_z = golem::numeric_const<float>::ZERO;
color = colorFrame.at<cv::Vec4b>(y, x);
if (hasImageStream) {
pOut->b = color[0];
pOut->g = color[1];
pOut->r = color[2];
pOut->a = colour._rgba.a;
}
else {
pOut->r = colour._rgba.r;
pOut->g = colour._rgba.g;
pOut->b = colour._rgba.b;
pOut->a = colour._rgba.a;
}
}
colorFrame += cColorWidth;
pCamCWidth += cColorWidth;
pOutWidth += cColorWidth;
}
}
//depth mode depth data mapped into camera space
if (mode & CameraKinect::MODE_DEPTH && (std::stoi(this->property.format.c_str()) == 102))
{
if (pImage == nullptr)
pImage.reset(new Image());
//camera->getContext().debug("In depth mode\n");
pImage->cloud.reset(new PointSeq((uint32_t)property.width, (uint32_t)property.height));
//Obtaining the pointcloud
hasImageStream = false;
float missingPoint = std::numeric_limits<float>::quiet_NaN();
hr = m_pCoordinateMapper->MapDepthFrameToCameraSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, cDepthHeight * cDepthWidth, m_pCameraCoordinatesDepth);
golem::Point* pOutWidth = &pImage->cloud->front();
CameraSpacePoint* pCamDWidth = m_pCameraCoordinatesDepth;
for (int y = 0; y < cDepthHeight; ++y)
{
golem::Point* pOut = pOutWidth;
CameraSpacePoint* pCamC = pCamDWidth;
for (int x = 0; x < cDepthWidth; ++x, pCamC++, ++pOut) {
cv::Vec4b color;
//int abc = pCamC->Z;
if (pCamC->Z != 0) {
pOut->x = pCamC->X;
pOut->y = pCamC->Y;
pOut->z = pCamC->Z;
}
else {
pOut->x = missingPoint;
pOut->y = missingPoint;
pOut->z = missingPoint;
// //printf("Getting to this point4\n");
}
pOut->normal_x = pOut->normal_y = pOut->normal_z = golem::numeric_const<float>::ZERO;
/*color = colorframe.at<cv::vec4b>(y, x);
if (hasimagestream) {
pout->b = color[0];
pout->g = color[1];
pout->r = color[2];
pout->a = colour._rgba.a;
}*/
//else {
pOut->r = colour._rgba.r;
pOut->g = colour._rgba.g;
pOut->b = colour._rgba.b;
pOut->a = colour._rgba.a;
//}
}
//colorFrame += cDepthWidth;
pCamDWidth += cDepthWidth;
pOutWidth += cDepthWidth;
}
golem::Mat33 aMatrix;
aMatrix.m11 = golem::Real(-1);
aMatrix.m12 = golem::Real(0);
aMatrix.m13 = golem::Real(0);
aMatrix.m21 = golem::Real(0);
aMatrix.m22 = golem::Real(-1);
aMatrix.m23 = golem::Real(0);
aMatrix.m31 = golem::Real(0);
aMatrix.m32 = golem::Real(0);
aMatrix.m33 = golem::Real(1);
golem::Vec3 aVector;
aVector.v1 = 0;
aVector.v2 = 0;
aVector.v3 = 0;
golem::Mat34 aMatrix34;
aMatrix34.R = aMatrix;
aMatrix34.p = aVector;
Cloud::PointSeqPtr pCloud = pImage->cloud;
const Mat34 frame = Cloud::getSensorFrame(*pCloud);
Cloud::transform(aMatrix34, *pCloud, *pCloud);
Cloud::setSensorFrame(frame, *pCloud); // don't change the camera frame !!!!!!!!!!!!!!!!!!!!!!!!!!!!
const golem::SecTmReal systemTime2 = camera->getContext().getTimer().elapsed();
//seting timestamp
pImage->timeStamp = REAL_HALF*(systemTime1 + systemTime2);
}
RELEASE_PTR(pDepthFrameDescription);
RELEASE_PTR(pColorFrameDescription);
}
RELEASE_PTR(pDepthFrame);
RELEASE_PTR(pColorFrame);
RELEASE_PTR(pMultiSourceFrame);
}
/// <summary>
/// Main processing function
/// </summary>
void CCoordinateMappingBasics::Update()
{
if (!m_pMultiSourceFrameReader)
{
return;
}
IMultiSourceFrame* pMultiSourceFrame = NULL;
IDepthFrame* pDepthFrame = NULL;
IColorFrame* pColorFrame = NULL;
IBodyIndexFrame* pBodyIndexFrame = NULL;
IBodyFrame* pBodyFrame = NULL;
HRESULT hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
if (SUCCEEDED(hr))
{
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pDepthFrameReference);
}
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
SafeRelease(pColorFrameReference);
}
if (SUCCEEDED(hr))
{
IBodyIndexFrameReference* pBodyIndexFrameReference = NULL;
hr = pMultiSourceFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame);
}
SafeRelease(pBodyIndexFrameReference);
}
if (SUCCEEDED(hr))
{
IBodyFrameReference* pBodyFrameReference = NULL;
hr = pMultiSourceFrame->get_BodyFrameReference(&pBodyFrameReference);
if (SUCCEEDED(hr))
{
hr = pBodyFrameReference->AcquireFrame(&pBodyFrame);
}
SafeRelease(pBodyFrameReference);
}
if (SUCCEEDED(hr))
{
// Depth
INT64 nDepthTime = 0;
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = NULL;
// Color
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
// BodyIndex
IFrameDescription* pBodyIndexFrameDescription = NULL;
int nBodyIndexWidth = 0;
int nBodyIndexHeight = 0;
UINT nBodyIndexBufferSize = 0;
BYTE *pBodyIndexBuffer = NULL;
// Body
IBody* ppBodies[BODY_COUNT] = { 0 };
// get depth frame data
hr = pDepthFrame->get_RelativeTime(&nDepthTime);
// Depth
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
}
// get color frame data
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Width(&nColorWidth);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Height(&nColorHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (m_pColorRGBX)
{
pColorBuffer = m_pColorRGBX;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
// get body index frame data
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrame->get_FrameDescription(&pBodyIndexFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameDescription->get_Width(&nBodyIndexWidth);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameDescription->get_Height(&nBodyIndexHeight);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrame->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer);
}
// get body frame data
if (SUCCEEDED(hr))
{
hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
}
if (SUCCEEDED(hr))
{
ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
pColorBuffer, nColorWidth, nColorHeight,
pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight, BODY_COUNT, ppBodies);
}
SafeRelease(pDepthFrameDescription);
SafeRelease(pColorFrameDescription);
SafeRelease(pBodyIndexFrameDescription);
for (int i = 0; i < _countof(ppBodies); ++i)
{
SafeRelease(ppBodies[i]);
}
}
SafeRelease(pDepthFrame);
SafeRelease(pColorFrame);
SafeRelease(pBodyIndexFrame);
SafeRelease(pBodyFrame);
SafeRelease(pMultiSourceFrame);
}
void Device::update()
{
if ( mSensor != 0 ) {
mSensor->get_Status( &mStatus );
}
if ( mFrameReader == 0 ) {
return;
}
IAudioBeamFrame* audioFrame = 0;
IBodyFrame* bodyFrame = 0;
IBodyIndexFrame* bodyIndexFrame = 0;
IColorFrame* colorFrame = 0;
IDepthFrame* depthFrame = 0;
IMultiSourceFrame* frame = 0;
IInfraredFrame* infraredFrame = 0;
ILongExposureInfraredFrame* infraredLongExposureFrame = 0;
HRESULT hr = mFrameReader->AcquireLatestFrame( &frame );
// TODO audio
if ( SUCCEEDED( hr ) ) {
console() << "SUCCEEDED " << getElapsedFrames() << endl;
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyEnabled() ) {
IBodyFrameReference* frameRef = 0;
hr = frame->get_BodyFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &bodyFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyIndexEnabled() ) {
IBodyIndexFrameReference* frameRef = 0;
hr = frame->get_BodyIndexFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &bodyIndexFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isColorEnabled() ) {
IColorFrameReference* frameRef = 0;
hr = frame->get_ColorFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &colorFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isDepthEnabled() ) {
IDepthFrameReference* frameRef = 0;
hr = frame->get_DepthFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &depthFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredEnabled() ) {
IInfraredFrameReference* frameRef = 0;
hr = frame->get_InfraredFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &infraredFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredLongExposureEnabled() ) {
ILongExposureInfraredFrameReference* frameRef = 0;
hr = frame->get_LongExposureInfraredFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &infraredLongExposureFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) ) {
long long time = 0L;
// TODO audio
IFrameDescription* bodyFrameDescription = 0;
int32_t bodyWidth = 0;
int32_t bodyHeight = 0;
uint32_t bodyBufferSize = 0;
uint8_t* bodyBuffer = 0;
IFrameDescription* bodyIndexFrameDescription = 0;
int32_t bodyIndexWidth = 0;
int32_t bodyIndexHeight = 0;
uint32_t bodyIndexBufferSize = 0;
uint8_t* bodyIndexBuffer = 0;
IFrameDescription* colorFrameDescription = 0;
int32_t colorWidth = 0;
int32_t colorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
uint32_t colorBufferSize = 0;
uint8_t* colorBuffer = 0;
IFrameDescription* depthFrameDescription = 0;
int32_t depthWidth = 0;
int32_t depthHeight = 0;
uint16_t depthMinReliableDistance = 0;
uint16_t depthMaxReliableDistance = 0;
uint32_t depthBufferSize = 0;
uint16_t* depthBuffer = 0;
IFrameDescription* infraredFrameDescription = 0;
int32_t infraredWidth = 0;
int32_t infraredHeight = 0;
uint32_t infraredBufferSize = 0;
uint16_t* infraredBuffer = 0;
IFrameDescription* infraredLongExposureFrameDescription = 0;
int32_t infraredLongExposureWidth = 0;
int32_t infraredLongExposureHeight = 0;
uint32_t infraredLongExposureBufferSize = 0;
uint16_t* infraredLongExposureBuffer = 0;
hr = depthFrame->get_RelativeTime( &time );
// TODO audio
if ( mDeviceOptions.isAudioEnabled() ) {
}
// TODO body
if ( mDeviceOptions.isBodyEnabled() ) {
}
if ( mDeviceOptions.isBodyIndexEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrame->get_FrameDescription( &bodyIndexFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrameDescription->get_Width( &bodyIndexWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrameDescription->get_Height( &bodyIndexHeight );
}
if ( SUCCEEDED( hr ) ) {
//hr = bodyIndexFrame->AccessUnderlyingBuffer( &bodyIndexBufferSize, &bodyIndexBuffer );
}
}
if ( mDeviceOptions.isColorEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = colorFrame->get_FrameDescription( &colorFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrameDescription->get_Width( &colorWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrameDescription->get_Height( &colorHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrame->get_RawColorImageFormat( &imageFormat );
}
if ( SUCCEEDED( hr ) ) {
bool isAllocated = false;
SurfaceChannelOrder channelOrder = SurfaceChannelOrder::BGRA;
if ( imageFormat == ColorImageFormat_Bgra ) {
hr = colorFrame->AccessRawUnderlyingBuffer( &colorBufferSize, reinterpret_cast<uint8_t**>( &colorBuffer ) );
channelOrder = SurfaceChannelOrder::BGRA;
} else if ( imageFormat == ColorImageFormat_Rgba ) {
hr = colorFrame->AccessRawUnderlyingBuffer( &colorBufferSize, reinterpret_cast<uint8_t**>( &colorBuffer ) );
channelOrder = SurfaceChannelOrder::RGBA;
} else {
isAllocated = true;
colorBufferSize = colorWidth * colorHeight * sizeof( uint8_t ) * 4;
colorBuffer = new uint8_t[ colorBufferSize ];
hr = colorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<uint8_t*>( colorBuffer ), ColorImageFormat_Rgba );
channelOrder = SurfaceChannelOrder::RGBA;
}
if ( SUCCEEDED( hr ) ) {
colorFrame->get_RelativeTime( &time );
Surface8u colorSurface = Surface8u( colorBuffer, colorWidth, colorHeight, colorWidth * sizeof( uint8_t ) * 4, channelOrder );
mFrame.mSurfaceColor = Surface8u( colorWidth, colorHeight, false, channelOrder );
mFrame.mSurfaceColor.copyFrom( colorSurface, colorSurface.getBounds() );
console() << "Color\n\twidth: " << colorWidth << "\n\theight: " << colorHeight
<< "\n\tbuffer size: " << colorBufferSize << "\n\ttime: " << time << endl;
}
if ( isAllocated && colorBuffer != 0 ) {
delete[] colorBuffer;
colorBuffer = 0;
}
}
}
if ( mDeviceOptions.isDepthEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_FrameDescription( &depthFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrameDescription->get_Width( &depthWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrameDescription->get_Height( &depthHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_DepthMinReliableDistance( &depthMinReliableDistance );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_DepthMaxReliableDistance( &depthMaxReliableDistance );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->AccessUnderlyingBuffer( &depthBufferSize, &depthBuffer );
}
if ( SUCCEEDED( hr ) ) {
Channel16u depthChannel = Channel16u( depthWidth, depthHeight, depthWidth * sizeof( uint16_t ), 1, depthBuffer );
mFrame.mChannelDepth = Channel16u( depthWidth, depthHeight );
mFrame.mChannelDepth.copyFrom( depthChannel, depthChannel.getBounds() );
console( ) << "Depth\n\twidth: " << depthWidth << "\n\theight: " << depthHeight << endl;
}
}
if ( mDeviceOptions.isInfraredEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = infraredFrame->get_FrameDescription( &infraredFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrameDescription->get_Width( &infraredWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrameDescription->get_Height( &infraredHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrame->AccessUnderlyingBuffer( &infraredBufferSize, &infraredBuffer );
}
if ( SUCCEEDED( hr ) ) {
Channel16u infraredChannel = Channel16u( infraredWidth, infraredHeight, infraredWidth * sizeof( uint16_t ), 1, infraredBuffer );
mFrame.mChannelInfrared = Channel16u( infraredWidth, infraredHeight );
mFrame.mChannelInfrared.copyFrom( infraredChannel, infraredChannel.getBounds() );
console( ) << "Infrared\n\twidth: " << infraredWidth << "\n\theight: " << infraredHeight << endl;
}
}
if ( mDeviceOptions.isInfraredLongExposureEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrame->get_FrameDescription( &infraredLongExposureFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrameDescription->get_Width( &infraredLongExposureWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrameDescription->get_Height( &infraredLongExposureHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrame->AccessUnderlyingBuffer( &infraredLongExposureBufferSize, &infraredLongExposureBuffer );
}
if ( SUCCEEDED( hr ) ) {
Channel16u infraredLongExposureChannel = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight, infraredLongExposureWidth * sizeof( uint16_t ), 1, infraredLongExposureBuffer );
mFrame.mChannelInfraredLongExposure = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight );
mFrame.mChannelInfraredLongExposure.copyFrom( infraredLongExposureChannel, infraredLongExposureChannel.getBounds() );
int64_t irLongExpTime = 0;
hr = infraredLongExposureFrame->get_RelativeTime( &irLongExpTime );
console( ) << "Infrared Long Exposure\n\twidth: " << infraredLongExposureWidth << "\n\theight: " << infraredLongExposureHeight;
if ( SUCCEEDED( hr ) ) {
console() << "\n\ttimestamp: " << irLongExpTime;
}
console() << endl;
}
}
if ( SUCCEEDED( hr ) ) {
// TODO build Kinect2::Frame from buffers, data
mFrame.mTimeStamp = time;
}
if ( bodyFrameDescription != 0 ) {
bodyFrameDescription->Release();
bodyFrameDescription = 0;
}
if ( bodyIndexFrameDescription != 0 ) {
bodyIndexFrameDescription->Release();
bodyIndexFrameDescription = 0;
}
if ( colorFrameDescription != 0 ) {
colorFrameDescription->Release();
colorFrameDescription = 0;
}
if ( depthFrameDescription != 0 ) {
depthFrameDescription->Release();
depthFrameDescription = 0;
}
if ( infraredFrameDescription != 0 ) {
infraredFrameDescription->Release();
infraredFrameDescription = 0;
}
if ( infraredLongExposureFrameDescription != 0 ) {
infraredLongExposureFrameDescription->Release();
infraredLongExposureFrameDescription = 0;
}
}
if ( audioFrame != 0 ) {
audioFrame->Release();
audioFrame = 0;
}
if ( bodyFrame != 0 ) {
bodyFrame->Release();
bodyFrame = 0;
}
if ( bodyIndexFrame != 0 ) {
bodyIndexFrame->Release();
bodyIndexFrame = 0;
}
if ( colorFrame != 0 ) {
colorFrame->Release();
colorFrame = 0;
}
if ( depthFrame != 0 ) {
depthFrame->Release();
depthFrame = 0;
}
if ( frame != 0 ) {
frame->Release();
frame = 0;
}
if ( infraredFrame != 0 ) {
infraredFrame->Release();
infraredFrame = 0;
}
if ( infraredLongExposureFrame != 0 ) {
infraredLongExposureFrame->Release();
infraredLongExposureFrame = 0;
}
}
void Kinect2Manager::Update(unsigned int options) {
#ifdef _USE_KINECT
IColorFrame * pColorFrame = NULL;
IDepthFrame * pDepthFrame = NULL;
IBodyFrame * pBodyFrame = NULL;
IBodyIndexFrame * pBodyIndexFrame = NULL;
IMultiSourceFrame * pMultiSourceFrame = NULL;
m_nColorWidth = 0;
m_nColorHeight = 0;
m_nDepthWidth = 0;
m_nDepthHeight = 0;
m_bCalculateDepthRGBX = options & Update::DepthRGBX;
int numUpdate = countUpdate(options);
HRESULT hr = 1;
if (numUpdate > 1) {
hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
}
if (SUCCEEDED(hr)) {
if (options & Update::Color) {
IColorFrameReference* pColorFrameReference = NULL;
if (numUpdate > 1) {
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
}
else {
hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
}
if (SUCCEEDED(hr)) {
m_bMapColorToDepth = options & Update::MapColorToDepth;
}
else {
options &= ~Update::Color;
}
SafeRelease(pColorFrameReference);
}
if (options & Update::Depth) {
IDepthFrameReference* pDepthFrameReference = NULL;
if (numUpdate > 1) {
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
}
else {
hr = m_pDepthFrameReader->AcquireLatestFrame(&pDepthFrame);
}
if (SUCCEEDED(hr)) {
m_bMapDepthToColor = options & Update::MapDepthToColor;
}
else {
options &= ~Update::Depth;
}
SafeRelease(pDepthFrameReference);
}
if (options & Update::Body) {
IBodyFrameReference* pBodyFrameReference = NULL;
if (numUpdate > 1) {
hr = pMultiSourceFrame->get_BodyFrameReference(&pBodyFrameReference);
if (SUCCEEDED(hr))
{
hr = pBodyFrameReference->AcquireFrame(&pBodyFrame);
}
}
else {
hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame);
}
SafeRelease(pBodyFrameReference);
}
if (options & Update::Color && options & Update::Depth) {
if (options & Update::BodyIndex && options & Update::MapColorToDepth) {
if (m_bDepthColorMapCalculated) {
IBodyIndexFrameReference* pBodyIndexFrameReference = NULL;
hr = pMultiSourceFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame);
}
SafeRelease(pBodyIndexFrameReference);
}
}
}
}
if (pBodyFrame) {
UpdateBody(pBodyFrame);
}
if (pColorFrame) {
UpdateColor(pColorFrame);
}
if (pDepthFrame) {
UpdateDepth(pDepthFrame);
if (options & Update::MapDepthToColor) {
CalculateColorDepthMap();
if (m_bColorDepthMapCalculated)
ProcessDepthToColor(m_pDepth, m_nDepthWidth, m_nDepthHeight, m_pColorDepthMap, m_nColorWidth, m_nColorHeight);
}
if (options & Update::MapColorToDepth) {
CalculateDepthColorMap();
if (m_bDepthColorMapCalculated) {
ProcessColorToDepth(m_pColorRGBX, m_nColorWidth, m_nColorHeight, m_pDepthColorMap, m_nDepthWidth, m_nDepthHeight);
if (pBodyIndexFrame) {
UpdateBodyIndex(pBodyIndexFrame);
}
}
}
}
SafeRelease(pColorFrame);
SafeRelease(pDepthFrame);
SafeRelease(pBodyFrame);
SafeRelease(pBodyIndexFrame);
SafeRelease(pMultiSourceFrame);
#else
m_nDepthWidth = CAPTURE_SIZE_X_DEPTH;
m_nDepthHeight = CAPTURE_SIZE_Y_DEPTH;
m_nColorWidth = CAPTURE_SIZE_X_COLOR;
m_nColorHeight = CAPTURE_SIZE_Y_COLOR;
if (options & Update::MapDepthToColor) {
CalculateColorDepthMap();
if (m_bColorDepthMapCalculated)
ProcessDepthToColor(m_pDepth, m_nDepthWidth, m_nDepthHeight, m_pColorDepthMap, m_nColorWidth, m_nColorHeight);
}
if (options & Update::MapColorToDepth) {
CalculateDepthColorMap();
if (m_bDepthColorMapCalculated) {
ProcessColorToDepth(m_pColorRGBX, m_nColorWidth, m_nColorHeight, m_pDepthColorMap, m_nDepthWidth, m_nDepthHeight);
}
}
UpdateBodyIndex(NULL);
#endif
}
HRESULT KinectHandler::GetColorData(RGBQUAD* &dest)
{
if (!m_pMultiFrameReader)
{
cout << "No frame reader!" << endl;
return E_FAIL;
}
IColorFrame* pColorFrame = NULL;
IMultiSourceFrame* pMultiSourceFrame = NULL;
HRESULT hr = m_pMultiFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
SafeRelease(pColorFrameReference);
}
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Width(&nColorWidth);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Height(&nColorHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
cout << "Acessando o raw format!!!" << endl;
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (m_pColorRGBX)
{
cout << "Acessando o convert format" << endl;
pColorBuffer = m_pColorRGBX;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
cout << "FAILED" << endl;
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
{
dest = pColorBuffer;
}
}
else
{
cout << "Acquire last frame FAILED " << endl;
hr = E_FAIL;
SafeRelease(pColorFrame);
return hr;
}
SafeRelease(pColorFrame);
SafeRelease(pMultiSourceFrame);
return hr;
}
HRESULT KinectHandler::GetColorAndDepth(RGBQUAD* &color, RGBQUAD* &depth, UINT16*& depthBuffer)
{
if (!m_pMultiFrameReader)
{
cout << "No frame reader!" << endl;
return E_FAIL;
}
IColorFrame* pColorFrame = NULL;
IDepthFrame* pDepthFrame = NULL;
IMultiSourceFrame* pMultiSourceFrame = NULL;
HRESULT hr = m_pMultiFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pColorFrameReference);
SafeRelease(pDepthFrameReference);
}
if (SUCCEEDED(hr) && pColorFrame != NULL && pDepthFrame != NULL)
{
INT64 nTime = 0;
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Width(&nColorWidth);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Height(&nColorHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (m_pColorRGBX)
{
pColorBuffer = m_pColorRGBX;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
cout << "FAILED" << endl;
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
{
color = pColorBuffer;
}
///===========================================////
nTime = 0;
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
USHORT nDepthMinReliableDistance = 0;
USHORT nDepthMaxDistance = 0;
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = NULL;
hr = pDepthFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_DepthMinReliableDistance(&nDepthMinReliableDistance);
}
if (SUCCEEDED(hr))
{
// In order to see the full range of depth (including the less reliable far field depth)
// we are setting nDepthMaxDistance to the extreme potential depth threshold
nDepthMaxDistance = USHRT_MAX;
// Note: If you wish to filter by reliable depth distance, uncomment the following line.
//// hr = pDepthFrame->get_DepthMaxReliableDistance(&nDepthMaxDistance);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
}
if (SUCCEEDED(hr))
{
//RGBQUAD* pRGBX = new RGBQUAD[cDepthWidth * cDepthHeight];
// end pixel is start + width*height - 1
const UINT16* pBufferEnd = pDepthBuffer + (nDepthWidth * nDepthHeight);
RGBQUAD* auxiliar = m_pDepthRGBX;
//const UINT16* pBufferEnd = pDepthBuffer + (640 * 480);
int counter = 0;
while (pDepthBuffer < pBufferEnd)
{
//cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl;
USHORT depth = *pDepthBuffer;
//cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl;
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
// Note: Using conditionals in this loop could degrade performance.
// Consider using a lookup table instead when writing production code.
//BYTE intensity = static_cast<BYTE>((depth >= nDepthMinReliableDistance) && (depth <= nDepthMaxDistance) ? (depth % 256) : 0);
BYTE intensity = static_cast<BYTE>((depth >= nDepthMinReliableDistance) && (depth <= nDepthMaxDistance) ? ((depth - nDepthMinReliableDistance) * (0 - 255) / (nDepthMaxDistance / 50 - nDepthMinReliableDistance) + 255) : 0);
auxiliar->rgbBlue = intensity;
auxiliar->rgbGreen = intensity;
auxiliar->rgbRed = intensity;
auxiliar->rgbReserved = (BYTE)255;
counter++;
++auxiliar;
++pDepthBuffer;
}
depth = m_pDepthRGBX;
}
if (m_pDepthRawBuffer)
{
hr = pDepthFrame->CopyFrameDataToArray((cDepthWidth * cDepthHeight), m_pDepthRawBuffer);
if(SUCCEEDED(hr)) depthBuffer = m_pDepthRawBuffer;
}
SafeRelease(pDepthFrameDescription);
}
else
{
cout << "Acquire last frame FAILED " << endl;
hr = E_FAIL;
SafeRelease(pColorFrame);
SafeRelease(pDepthFrame);
return hr;
}
SafeRelease(pColorFrame);
SafeRelease(pDepthFrame);
SafeRelease(pMultiSourceFrame);
return hr;
}
HRESULT KinectHandler::GetDepthImageData(RGBQUAD* &dest)
{
if (!m_pMultiFrameReader)
{
cout << "No frame reader!" << endl;
return E_FAIL;
}
IDepthFrame* pDepthFrame = NULL;
IMultiSourceFrame* pMultiSourceFrame = NULL;
HRESULT hr = m_pMultiFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
if (SUCCEEDED(hr))
{
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pDepthFrameReference);
}
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
USHORT nDepthMinReliableDistance = 0;
USHORT nDepthMaxDistance = 0;
UINT nDepthBufferSize = 0;
UINT16 *pDepthBuffer = NULL;
hr = pDepthFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_DepthMinReliableDistance(&nDepthMinReliableDistance);
}
if (SUCCEEDED(hr))
{
// In order to see the full range of depth (including the less reliable far field depth)
// we are setting nDepthMaxDistance to the extreme potential depth threshold
nDepthMaxDistance = USHRT_MAX;
// Note: If you wish to filter by reliable depth distance, uncomment the following line.
//// hr = pDepthFrame->get_DepthMaxReliableDistance(&nDepthMaxDistance);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
}
if (SUCCEEDED(hr))
{
//RGBQUAD* pRGBX = new RGBQUAD[cDepthWidth * cDepthHeight];
// end pixel is start + width*height - 1
cout << "w:" << nDepthWidth << " h:" << nDepthHeight << endl;
cout << "buffersize:" << nDepthBufferSize << endl;
const UINT16* pBufferEnd = pDepthBuffer + (nDepthWidth * nDepthHeight);
RGBQUAD* auxiliar = m_pDepthRGBX;
//const UINT16* pBufferEnd = pDepthBuffer + (640 * 480);
cout << "bufferLocation:" << pDepthBuffer << endl;
cout << "bufferend:" << pBufferEnd << endl;
int counter = 0;
while (pDepthBuffer < pBufferEnd)
{
//cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl;
USHORT depth = *pDepthBuffer;
//cout << "now:" << pDepthBuffer << " end:" << pBufferEnd << endl;
// To convert to a byte, we're discarding the most-significant
// rather than least-significant bits.
// We're preserving detail, although the intensity will "wrap."
// Values outside the reliable depth range are mapped to 0 (black).
// Note: Using conditionals in this loop could degrade performance.
// Consider using a lookup table instead when writing production code.
BYTE intensity = static_cast<BYTE>((depth >= nDepthMinReliableDistance) && (depth <= nDepthMaxDistance) ? (depth % 256) : 0);
auxiliar->rgbBlue = intensity;
auxiliar->rgbGreen = intensity;
auxiliar->rgbRed = intensity;
auxiliar->rgbReserved = (BYTE)255;
counter++;
++auxiliar;
++pDepthBuffer;
}
dest = m_pDepthRGBX;
cout << "total struct:" << counter << endl;
}
SafeRelease(pDepthFrameDescription);
}
else
{
cout << "Acquire last frame FAILED " << endl;
hr = E_FAIL;
SafeRelease(pDepthFrame);
return hr;
}
SafeRelease(pDepthFrame);
SafeRelease(pMultiSourceFrame);
return hr;
}
void Device::update()
{
if ( mFrameReader == 0 ) {
return;
}
IAudioBeamFrame* audioFrame = 0;
IBodyFrame* bodyFrame = 0;
IBodyIndexFrame* bodyIndexFrame = 0;
IColorFrame* colorFrame = 0;
IDepthFrame* depthFrame = 0;
IMultiSourceFrame* frame = 0;
IInfraredFrame* infraredFrame = 0;
ILongExposureInfraredFrame* infraredLongExposureFrame = 0;
HRESULT hr = mFrameReader->AcquireLatestFrame( &frame );
if ( SUCCEEDED( hr ) && mDeviceOptions.isAudioEnabled() ) {
// TODO audio
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyEnabled() ) {
IBodyFrameReference* frameRef = 0;
hr = frame->get_BodyFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &bodyFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyIndexEnabled() ) {
IBodyIndexFrameReference* frameRef = 0;
hr = frame->get_BodyIndexFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &bodyIndexFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isColorEnabled() ) {
IColorFrameReference* frameRef = 0;
hr = frame->get_ColorFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &colorFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isDepthEnabled() ) {
IDepthFrameReference* frameRef = 0;
hr = frame->get_DepthFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &depthFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredEnabled() ) {
IInfraredFrameReference* frameRef = 0;
hr = frame->get_InfraredFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &infraredFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredLongExposureEnabled() ) {
ILongExposureInfraredFrameReference* frameRef = 0;
hr = frame->get_LongExposureInfraredFrameReference( &frameRef );
if ( SUCCEEDED( hr ) ) {
hr = frameRef->AcquireFrame( &infraredLongExposureFrame );
}
if ( frameRef != 0 ) {
frameRef->Release();
frameRef = 0;
}
}
if ( SUCCEEDED( hr ) ) {
long long timeStamp = 0L;
// TODO audio
std::vector<Body> bodies;
int64_t bodyTime = 0L;
IBody* kinectBodies[ BODY_COUNT ] = { 0 };
Vec4f floorClipPlane = Vec4f::zero();
Channel8u bodyIndexChannel;
IFrameDescription* bodyIndexFrameDescription = 0;
int32_t bodyIndexWidth = 0;
int32_t bodyIndexHeight = 0;
uint32_t bodyIndexBufferSize = 0;
uint8_t* bodyIndexBuffer = 0;
int64_t bodyIndexTime = 0L;
Surface8u colorSurface;
IFrameDescription* colorFrameDescription = 0;
int32_t colorWidth = 0;
int32_t colorHeight = 0;
ColorImageFormat colorImageFormat = ColorImageFormat_None;
uint32_t colorBufferSize = 0;
uint8_t* colorBuffer = 0;
Channel16u depthChannel;
IFrameDescription* depthFrameDescription = 0;
int32_t depthWidth = 0;
int32_t depthHeight = 0;
uint16_t depthMinReliableDistance = 0;
uint16_t depthMaxReliableDistance = 0;
uint32_t depthBufferSize = 0;
uint16_t* depthBuffer = 0;
Channel16u infraredChannel;
IFrameDescription* infraredFrameDescription = 0;
int32_t infraredWidth = 0;
int32_t infraredHeight = 0;
uint32_t infraredBufferSize = 0;
uint16_t* infraredBuffer = 0;
Channel16u infraredLongExposureChannel;
IFrameDescription* infraredLongExposureFrameDescription = 0;
int32_t infraredLongExposureWidth = 0;
int32_t infraredLongExposureHeight = 0;
uint32_t infraredLongExposureBufferSize = 0;
uint16_t* infraredLongExposureBuffer = 0;
hr = depthFrame->get_RelativeTime( &timeStamp );
// TODO audio
if ( mDeviceOptions.isAudioEnabled() ) {
}
if ( mDeviceOptions.isBodyEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = bodyFrame->get_RelativeTime( &bodyTime );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyFrame->GetAndRefreshBodyData( BODY_COUNT, kinectBodies );
}
if ( SUCCEEDED( hr ) ) {
Vector4 v;
hr = bodyFrame->get_FloorClipPlane( &v );
floorClipPlane = toVec4f( v );
}
if ( SUCCEEDED( hr ) ) {
for ( uint8_t i = 0; i < BODY_COUNT; ++i ) {
IBody* kinectBody = kinectBodies[ i ];
if ( kinectBody != 0 ) {
uint8_t isTracked = false;
hr = kinectBody->get_IsTracked( &isTracked );
if ( SUCCEEDED( hr ) && isTracked ) {
Joint joints[ JointType_Count ];
kinectBody->GetJoints( JointType_Count, joints );
JointOrientation jointOrientations[ JointType_Count ];
kinectBody->GetJointOrientations( JointType_Count, jointOrientations );
uint64_t id = 0;
kinectBody->get_TrackingId( &id );
std::map<JointType, Body::Joint> jointMap;
for ( int32_t j = 0; j < JointType_Count; ++j ) {
Body::Joint joint(
toVec3f( joints[ j ].Position ),
toQuatf( jointOrientations[ j ].Orientation ),
joints[ j ].TrackingState
);
jointMap.insert( pair<JointType, Body::Joint>( static_cast<JointType>( j ), joint ) );
}
Body body( id, i, jointMap );
bodies.push_back( body );
}
}
}
}
}
if ( mDeviceOptions.isBodyIndexEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrame->get_RelativeTime( &bodyIndexTime );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrame->get_FrameDescription( &bodyIndexFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrameDescription->get_Width( &bodyIndexWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrameDescription->get_Height( &bodyIndexHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = bodyIndexFrame->AccessUnderlyingBuffer( &bodyIndexBufferSize, &bodyIndexBuffer );
}
if ( SUCCEEDED( hr ) ) {
bodyIndexChannel = Channel8u( bodyIndexWidth, bodyIndexHeight );
memcpy( bodyIndexChannel.getData(), bodyIndexBuffer, bodyIndexWidth * bodyIndexHeight * sizeof( uint8_t ) );
}
}
if ( mDeviceOptions.isColorEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = colorFrame->get_FrameDescription( &colorFrameDescription );
if ( SUCCEEDED( hr ) ) {
float vFov = 0.0f;
float hFov = 0.0f;
float dFov = 0.0f;
colorFrameDescription->get_VerticalFieldOfView( &vFov );
colorFrameDescription->get_HorizontalFieldOfView( &hFov );
colorFrameDescription->get_DiagonalFieldOfView( &dFov );
}
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrameDescription->get_Width( &colorWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrameDescription->get_Height( &colorHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = colorFrame->get_RawColorImageFormat( &colorImageFormat );
}
if ( SUCCEEDED( hr ) ) {
colorBufferSize = colorWidth * colorHeight * sizeof( uint8_t ) * 4;
colorBuffer = new uint8_t[ colorBufferSize ];
hr = colorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<uint8_t*>( colorBuffer ), ColorImageFormat_Rgba );
if ( SUCCEEDED( hr ) ) {
colorSurface = Surface8u( colorWidth, colorHeight, false, SurfaceChannelOrder::RGBA );
memcpy( colorSurface.getData(), colorBuffer, colorWidth * colorHeight * sizeof( uint8_t ) * 4 );
}
delete [] colorBuffer;
colorBuffer = 0;
}
}
if ( mDeviceOptions.isDepthEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_FrameDescription( &depthFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrameDescription->get_Width( &depthWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrameDescription->get_Height( &depthHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_DepthMinReliableDistance( &depthMinReliableDistance );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->get_DepthMaxReliableDistance( &depthMaxReliableDistance );
}
if ( SUCCEEDED( hr ) ) {
hr = depthFrame->AccessUnderlyingBuffer( &depthBufferSize, &depthBuffer );
}
if ( SUCCEEDED( hr ) ) {
depthChannel = Channel16u( depthWidth, depthHeight );
memcpy( depthChannel.getData(), depthBuffer, depthWidth * depthHeight * sizeof( uint16_t ) );
}
}
if ( mDeviceOptions.isInfraredEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = infraredFrame->get_FrameDescription( &infraredFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrameDescription->get_Width( &infraredWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrameDescription->get_Height( &infraredHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredFrame->AccessUnderlyingBuffer( &infraredBufferSize, &infraredBuffer );
}
if ( SUCCEEDED( hr ) ) {
infraredChannel = Channel16u( infraredWidth, infraredHeight );
memcpy( infraredChannel.getData(), infraredBuffer, infraredWidth * infraredHeight * sizeof( uint16_t ) );
}
}
if ( mDeviceOptions.isInfraredLongExposureEnabled() ) {
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrame->get_FrameDescription( &infraredLongExposureFrameDescription );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrameDescription->get_Width( &infraredLongExposureWidth );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrameDescription->get_Height( &infraredLongExposureHeight );
}
if ( SUCCEEDED( hr ) ) {
hr = infraredLongExposureFrame->AccessUnderlyingBuffer( &infraredLongExposureBufferSize, &infraredLongExposureBuffer );
}
if ( SUCCEEDED( hr ) ) {
infraredLongExposureChannel = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight );
memcpy( infraredLongExposureChannel.getData(), infraredLongExposureBuffer, infraredLongExposureWidth * infraredLongExposureHeight * sizeof( uint16_t ) );
}
}
if ( SUCCEEDED( hr ) ) {
mFrame.mBodies = bodies;
mFrame.mChannelBodyIndex = bodyIndexChannel;
mFrame.mChannelDepth = depthChannel;
mFrame.mChannelInfrared = infraredChannel;
mFrame.mChannelInfraredLongExposure = infraredLongExposureChannel;
mFrame.mDeviceId = mDeviceOptions.getDeviceId();
mFrame.mSurfaceColor = colorSurface;
mFrame.mTimeStamp = timeStamp;
mFrame.mFloorClipPlane = floorClipPlane;
}
if ( bodyIndexFrameDescription != 0 ) {
bodyIndexFrameDescription->Release();
bodyIndexFrameDescription = 0;
}
if ( colorFrameDescription != 0 ) {
colorFrameDescription->Release();
colorFrameDescription = 0;
}
if ( depthFrameDescription != 0 ) {
depthFrameDescription->Release();
depthFrameDescription = 0;
}
if ( infraredFrameDescription != 0 ) {
infraredFrameDescription->Release();
infraredFrameDescription = 0;
}
if ( infraredLongExposureFrameDescription != 0 ) {
infraredLongExposureFrameDescription->Release();
infraredLongExposureFrameDescription = 0;
}
}
if ( audioFrame != 0 ) {
audioFrame->Release();
audioFrame = 0;
}
if ( bodyFrame != 0 ) {
bodyFrame->Release();
bodyFrame = 0;
}
if ( bodyIndexFrame != 0 ) {
bodyIndexFrame->Release();
bodyIndexFrame = 0;
}
if ( colorFrame != 0 ) {
colorFrame->Release();
colorFrame = 0;
}
if ( depthFrame != 0 ) {
depthFrame->Release();
depthFrame = 0;
}
if ( frame != 0 ) {
frame->Release();
frame = 0;
}
if ( infraredFrame != 0 ) {
infraredFrame->Release();
infraredFrame = 0;
}
if ( infraredLongExposureFrame != 0 ) {
infraredLongExposureFrame->Release();
infraredLongExposureFrame = 0;
}
}
void capture()
{
IMultiSourceFrame *multiFrame = NULL;
IColorFrame *colorFrame = NULL;
IColorFrameReference *colorFrameReference = NULL;
UINT colorBufferSize = 0;
RGBQUAD *colorBuffer = NULL;
IDepthFrame *depthFrame = NULL;
IDepthFrameReference *depthFrameReference = NULL;
UINT bufferSize = 0;
// UINT16 *depthBuffer = NULL;
IBodyIndexFrame *bodyIndexFrame = NULL;
IBodyIndexFrameReference *bodyIndexFrameReference = NULL;
UINT bodyIndexBufferSize = 0;
static int lastTime = 0;
static int currentTime = 0;
HRESULT hr = -1;
//フレームリーダーが読み込み可能になるのを待つループ(各Frameしか取らない)
while(1) {
if((currentTime = GetTickCount()) > 33)
{
hr = multiFrameReader->AcquireLatestFrame(&multiFrame);
lastTime = currentTime;
}else continue;
if(FAILED(hr)) {
//fprintf(stderr, "AcquireLatestFrame(&multiFrame)\n");
Sleep(1);
continue;
}
hr = multiFrame->get_ColorFrameReference(&colorFrameReference);
if(FAILED(hr)) {
Sleep(1);
fprintf(stderr, "ColorFrameReference(&colorFrameReference)\n");
SafeRelease(multiFrame);
continue;
}
hr = colorFrameReference->AcquireFrame(&colorFrame);
if(FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&colorFrame)\n");
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = multiFrame->get_DepthFrameReference(&depthFrameReference);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "DepthFrameReference(&depthFrameReference)\n");
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = depthFrameReference->AcquireFrame(&depthFrame);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&depthFrame)\n");
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
// hr = depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer);
hr = depthFrame->CopyFrameDataToArray( dPixels, &depthBuffer[0] );
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AccessUnderlyingBuffer(&bufferSize, &depthBuffer\n");
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = multiFrame->get_BodyIndexFrameReference(&bodyIndexFrameReference);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "BodyIndexReference(&colorFrameReference)\n");
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = bodyIndexFrameReference->AcquireFrame(&bodyIndexFrame);
if (FAILED(hr)) {
Sleep(1);
fprintf(stderr, "AcquireFrame(&bodyIndexFrame)\n");
SafeRelease(bodyIndexFrameReference);
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
hr = bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer);
if(FAILED(hr))
{
Sleep(1);
fprintf(stderr, "bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer)");
SafeRelease(bodyIndexFrame);
SafeRelease(bodyIndexFrameReference);
free(depthBuffer);
SafeRelease(depthFrame);
SafeRelease(depthFrameReference);
SafeRelease(colorFrame);
SafeRelease(colorFrameReference);
SafeRelease(multiFrame);
continue;
}
SafeRelease(colorFrameReference);
SafeRelease(bodyIndexFrameReference);
SafeRelease(depthFrameReference);
break;
}
//深度値をBufferへ格納
// ERROR_CHECK(depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer));
//カラーマップの設定データの読み込みと、colorBufferメモリの確保
if(colorRGBX == NULL)
{
IFrameDescription *colorFrameDescription = NULL;
ERROR_CHECK2(colorFrame->get_FrameDescription(&colorFrameDescription), "FrameDescription");
ERROR_CHECK2(colorFrameDescription->get_Width(&colorWidth), "get_Width");
ERROR_CHECK2(colorFrameDescription->get_Height(&colorHeight), "get_Height");
colorRGBX = new RGBQUAD[colorWidth * colorHeight];
glutReshapeWindow(width, height);
ERROR_CHECK2(colorFrame->get_RawColorImageFormat(&imageFormat), "get_RawColorImageFormat");
SafeRelease(colorFrameDescription);
}
//カラーイメージをcolorBufferへコピー
if(imageFormat == ColorImageFormat_Bgra)
{
ERROR_CHECK2(colorFrame->AccessRawUnderlyingBuffer(&colorBufferSize,
reinterpret_cast<BYTE**>(&colorBuffer)), "AccessRawUnderlyingBuffer");
}else if(colorRGBX)
{
colorBuffer = colorRGBX;
colorBufferSize = colorWidth * colorHeight * sizeof(RGBQUAD);
ERROR_CHECK2(colorFrame->CopyConvertedFrameDataToArray(colorBufferSize,
reinterpret_cast<BYTE*>(colorBuffer),
ColorImageFormat_Bgra), "CopyConvertedFrameDataToArray");
}else
{
//Error
}
//colorMapの初期化 一度だけ実行される
if(colorMap == NULL)
{
colorMap = new float[colorWidth * colorHeight][3];
}
if (colorCoordinates == NULL)
{
colorCoordinates = new ColorSpacePoint[width * height];
}
if (cameraSpacePoints == NULL)
{
cameraSpacePoints = new CameraSpacePoint[width * height];
}
if (cameraSpacePoints_ave == NULL)
{
cameraSpacePoints_ave = new CameraSpacePoint[width * height];
}
ERROR_CHECK2(coordinateMapper->MapDepthFrameToColorSpace(
width * height, (UINT16*)depthBuffer,
width * height, colorCoordinates), "MapDepthFrameToColorSpace");
ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace(
width * height, (UINT16*)depthBuffer,
width * height, cameraSpacePoints),"MapDepthFrameToCameraSpace");
//colorBufferのデータを、colorMapにコピー
for(int i = 0; i < height; i++){
for(int j = 0; j < width; j++){
int index = i * width + j;
ColorSpacePoint colorPoint = colorCoordinates[index];
int colorX = (int)(floor(colorPoint.X + 0.5));
int colorY = (int)(floor(colorPoint.Y + 0.5));
if(colorX >= 0 && colorX < colorWidth && colorY >= 0 && colorY < colorHeight)
{
int colorIndex = colorX + colorY * colorWidth;
//格納先はミラーモードを解除
float* colorp = colorMap[index];
UCHAR* data = (UCHAR*)(colorBuffer + colorIndex);
colorp[0] = (float)data[2] / 255.0f;
colorp[1] = (float)data[1] / 255.0f;
colorp[2] = (float)data[0] / 255.0f;
}else
{
float* colorp = colorMap[index];
colorp[0] = 0;
colorp[1] = 0;
colorp[2] = 0;
}
}
}
//フレームリソースを解放
SafeRelease(colorFrame);
SafeRelease(multiFrame);
SafeRelease(bodyIndexFrame);
SafeRelease(depthFrame);
}
void KinectGrabber::GetNextFrame() {
if (!m_pMultiSourceFrameReader)
{
return;
}
IMultiSourceFrame* pMultiSourceFrame = NULL;
IDepthFrame* pDepthFrame = NULL;
IColorFrame* pColorFrame = NULL;
IBodyIndexFrame* pBodyIndexFrame = NULL;
HRESULT hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
if (SUCCEEDED(hr))
{
IDepthFrameReference* pDepthFrameReference = NULL;
hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
if (SUCCEEDED(hr))
{
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
}
SafeRelease(pDepthFrameReference);
}
if (SUCCEEDED(hr))
{
IColorFrameReference* pColorFrameReference = NULL;
hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
if (SUCCEEDED(hr))
{
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
}
SafeRelease(pColorFrameReference);
}
if (SUCCEEDED(hr))
{
IBodyIndexFrameReference* pBodyIndexFrameReference = NULL;
hr = pMultiSourceFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame);
}
SafeRelease(pBodyIndexFrameReference);
}
if (SUCCEEDED(hr))
{
INT64 nDepthTime = 0;
IFrameDescription* pDepthFrameDescription = NULL;
int nDepthWidth = 0;
int nDepthHeight = 0;
UINT nDepthBufferSize = 0;
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
IFrameDescription* pBodyIndexFrameDescription = NULL;
int nBodyIndexWidth = 0;
int nBodyIndexHeight = 0;
UINT nBodyIndexBufferSize = 0;
BYTE *pBodyIndexBuffer = NULL;
// get depth frame data
hr = pDepthFrame->get_RelativeTime(&nDepthTime);
if (SUCCEEDED(hr))
{
hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Width(&nDepthWidth);
}
if (SUCCEEDED(hr))
{
hr = pDepthFrameDescription->get_Height(&nDepthHeight);
}
if (SUCCEEDED(hr))
{
//m_pDepthBuffer = new UINT16[cDepthWidth * cDepthHeight];
hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &m_pDepthBuffer);
//pDepthFrame->CopyFrameDataToArray(nDepthBufferSize,m_pDepthBuffer);
WaitForSingleObject(hDepthMutex,INFINITE);
m_depthImage.release();
Mat tmp = Mat(m_depthSize, DEPTH_PIXEL_TYPE, m_pDepthBuffer, Mat::AUTO_STEP);
m_depthImage = tmp.clone(); //need to deep copy because of the call to SafeRelease(pDepthFrame) to prevent access violation
ReleaseMutex(hDepthMutex);
}
// get color frame data
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Width(&nColorWidth);
}
if (SUCCEEDED(hr))
{
hr = pColorFrameDescription->get_Height(&nColorHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
}
else if (m_pColorRGBX)
{
pColorBuffer = m_pColorRGBX;
nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
if(SUCCEEDED(hr)) {
WaitForSingleObject(hColorMutex,INFINITE);
m_colorImage.release();
Mat tmp = Mat(m_colorSize, COLOR_PIXEL_TYPE, pColorBuffer, Mat::AUTO_STEP);
m_colorImage = tmp.clone();
ReleaseMutex(hColorMutex);
}
}
// get body index frame data
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrame->get_FrameDescription(&pBodyIndexFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameDescription->get_Width(&nBodyIndexWidth);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrameDescription->get_Height(&nBodyIndexHeight);
}
if (SUCCEEDED(hr))
{
hr = pBodyIndexFrame->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer);
}
SafeRelease(pDepthFrameDescription);
SafeRelease(pColorFrameDescription);
SafeRelease(pBodyIndexFrameDescription);
}
SafeRelease(pDepthFrame);
SafeRelease(pColorFrame);
SafeRelease(pBodyIndexFrame);
SafeRelease(pMultiSourceFrame);
}
