void idle()
{
// Read color data
IColorFrame* pCFrame = nullptr;
if (pColorFrameReader->AcquireLatestFrame(&pCFrame) == S_OK)
{
pCFrame->CopyConvertedFrameDataToArray(uColorBufferSize, pColorBuffer, ColorImageFormat_Rgba);
pCFrame->Release();
pCFrame = nullptr;
}
// Read depth data
IDepthFrame* pDFrame = nullptr;
if (pDepthFrameReader->AcquireLatestFrame(&pDFrame) == S_OK)
{
pDFrame->CopyFrameDataToArray(uDepthPointNum, pDepthBuffer);
pDFrame->Release();
pDFrame = nullptr;
// map to camera space
pCoordinateMapper->MapColorFrameToCameraSpace(uDepthPointNum, pDepthBuffer, uColorPointNum, pCSPoints);
}
}
// デプスデータを取得する
GLuint KinectV2::getDepth() const
{
// デプスのテクスチャを指定する
glBindTexture(GL_TEXTURE_2D, depthTexture);
// 次のデプスのフレームデータが到着していれば
IDepthFrame *depthFrame;
if (depthReader->AcquireLatestFrame(&depthFrame) == S_OK)
{
// デプスデータのサイズと格納場所を得る
UINT depthSize;
UINT16 *depthBuffer;
depthFrame->AccessUnderlyingBuffer(&depthSize, &depthBuffer);
// カラーのテクスチャ座標を求めてバッファオブジェクトに転送する
glBindBuffer(GL_ARRAY_BUFFER, coordBuffer);
ColorSpacePoint *const texcoord(static_cast<ColorSpacePoint *>(glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY)));
coordinateMapper->MapDepthFrameToColorSpace(depthCount, depthBuffer, depthCount, texcoord);
glUnmapBuffer(GL_ARRAY_BUFFER);
// デプスデータをテクスチャに転送する
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, depthWidth, depthHeight, GL_RED, GL_UNSIGNED_SHORT, depthBuffer);
// デプスフレームを開放する
depthFrame->Release();
}
return depthTexture;
}
// カメラ座標を取得する
GLuint KinectV2::getPoint() const
{
// カメラ座標のテクスチャを指定する
glBindTexture(GL_TEXTURE_2D, pointTexture);
// 次のデプスのフレームデータが到着していれば
IDepthFrame *depthFrame;
if (depthReader->AcquireLatestFrame(&depthFrame) == S_OK)
{
// デプスデータのサイズと格納場所を得る
UINT depthSize;
UINT16 *depthBuffer;
depthFrame->AccessUnderlyingBuffer(&depthSize, &depthBuffer);
// カラーのテクスチャ座標を求めてバッファオブジェクトに転送する
glBindBuffer(GL_ARRAY_BUFFER, coordBuffer);
ColorSpacePoint *const texcoord(static_cast<ColorSpacePoint *>(glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY)));
coordinateMapper->MapDepthFrameToColorSpace(depthCount, depthBuffer, depthCount, texcoord);
glUnmapBuffer(GL_ARRAY_BUFFER);
// カメラ座標への変換テーブルを得る
UINT32 entry;
PointF *table;
coordinateMapper->GetDepthFrameToCameraSpaceTable(&entry, &table);
// すべての点について
for (unsigned int i = 0; i < entry; ++i)
{
// デプス値の単位をメートルに換算する係数
static const GLfloat zScale(-0.001f);
// その点のデプス値を得る
const unsigned short d(depthBuffer[i]);
// デプス値の単位をメートルに換算する (計測不能点は maxDepth にする)
const GLfloat z(d == 0 ? -maxDepth : GLfloat(d) * zScale);
// その点のスクリーン上の位置を求める
const GLfloat x(table[i].X);
const GLfloat y(-table[i].Y);
// その点のカメラ座標を求める
position[i][0] = x * z;
position[i][1] = y * z;
position[i][2] = z;
}
// カメラ座標を転送する
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, depthWidth, depthHeight, GL_RGB, GL_FLOAT, position);
// テーブルを開放する
CoTaskMemFree(table);
// デプスフレームを開放する
depthFrame->Release();
}
return pointTexture;
}
bool KinectInterface::getFrameData(IMultiSourceFrame* frame, cv::Mat& intensity_mat, cv::Mat& depth_mat, cv::Mat& pos_mat) {
//Obtain depth frame
IDepthFrame* depthframe = nullptr;
if (FAILED(depthFrameReader->AcquireLatestFrame(&depthframe))) return false;
if (!depthframe) return false;
// Get data from frame
unsigned int sz;
unsigned short* buf;
if (FAILED(depthframe->AccessUnderlyingBuffer(&sz, &buf))) return false;
//get depth -> xyz mapping
if (FAILED(mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz))) return false;
//get depth -> rgb image mapping
if (FAILED(mapper->MapDepthFrameToColorSpace(width*height, buf, width*height, depth2rgb))) return false;
//save depth
if (FAILED(depthframe->CopyFrameDataToArray(height * width, depth_data)));
if (depthframe) depthframe->Release();
//Obtain RGB frame
IColorFrame* colorframe;
if (FAILED(colorFrameReader->AcquireLatestFrame(&colorframe))) return false;
if (!colorframe) return false;
// Get data from frame
if (FAILED(colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight * 4, rgbimage, ColorImageFormat_Rgba))) return false;
cv::Mat tmp_depth = cv::Mat::zeros(colorheight, colorwidth, CV_16UC1);
cv::Mat tmp_pos = cv::Mat::zeros(colorheight, colorwidth, CV_32FC3);
cv::Mat depth_org(height, width, CV_16UC1, depth_data);
cv::Mat tmp_rgb(colorheight, colorwidth, CV_8UC4, rgbimage);
// Write color array for vertices
for (int i = 0; i < width*height; i++) {
ColorSpacePoint p = depth2rgb[i];
int iY = (int)(p.Y + 0.5);
int iX = (int)(p.X + 0.5);
if (iX >= 0 && iY >= 0 && iX < colorwidth && iY < colorheight) {
// Check if color pixel coordinates are in bounds
tmp_depth.at<unsigned short>(iY, iX) = depth_data[i];
//tmp_pos.at<float>(iY, iX, 0) = depth2xyz[i].X;
//tmp_pos.at<float>(iY, iX, 1) = depth2xyz[i].Y;
//tmp_pos.at<float>(iY, iX, 2) = depth2xyz[i].Z;
}
}
if (colorframe) colorframe->Release();
cv::resize(tmp_rgb(cv::Rect(240, 0, 1440, 1080)), intensity_mat, cv::Size(640, 480));
cv::resize(tmp_depth(cv::Rect(240, 0, 1440, 1080)), depth_mat, cv::Size(640, 480));
cv::resize(tmp_pos(cv::Rect(240, 0, 1440, 1080)), pos_mat, cv::Size(640, 480));
cv::cvtColor(intensity_mat, intensity_mat, CV_RGBA2GRAY);
return true;
}
void MKinect::getDepthData(IMultiSourceFrame* frame, float* dest) {
IDepthFrame* depthframe;
IDepthFrameReference* frameref = NULL;
frame->get_DepthFrameReference(&frameref);
frameref->AcquireFrame(&depthframe);
if (frameref) frameref->Release();
if (!depthframe) return;
// Get data from frame
unsigned int sz;
UINT16 * buf;
while (!SUCCEEDED(depthframe->AccessUnderlyingBuffer(&sz, &buf))) {
}
HRESULT res = S_OK;
res = mapper->MapDepthFrameToCameraSpace(
KinectColorWidth*KinectColorHeight, buf, // Depth frame data and size of depth frame
KinectColorWidth*KinectColorHeight, depth2xyz); // Output CameraSpacePoint array and size
// Process depth frame data...
if (depthframe) depthframe->Release();
}
int main(int argc, char** argv)
{
// 1a. Get default Sensor
cout << "Try to get default sensor" << endl;
IKinectSensor* pSensor = nullptr;
if (GetDefaultKinectSensor(&pSensor) != S_OK)
{
cerr << "Get Sensor failed" << endl;
}
else
{
// 1b. Open sensor
cout << "Try to open sensor" << endl;
if (pSensor->Open() != S_OK)
{
cerr << "Can't open sensor" << endl;
}
else
{
// 2a. Get frame source
cout << "Try to get source" << endl;
IDepthFrameSource* pFrameSource = nullptr;
if (pSensor->get_DepthFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get frame source" << endl;
}
else
{
// 2b. Get frame description
int iWidth = 0;
int iHeight = 0;
IFrameDescription* pFrameDescription = nullptr;
if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK)
{
pFrameDescription->get_Width(&iWidth);
pFrameDescription->get_Height(&iHeight);
pFrameDescription->Release();
pFrameDescription = nullptr;
}
// 2c. get some dpeth only meta
UINT16 uDepthMin = 0, uDepthMax = 0;
pFrameSource->get_DepthMinReliableDistance(&uDepthMin);
pFrameSource->get_DepthMaxReliableDistance(&uDepthMax);
cout << "Reliable Distance: " << uDepthMin << " - " << uDepthMax << endl;
// perpare OpenCV
cv::Mat mDepthImg(iHeight, iWidth, CV_16UC1);
cv::Mat mImg8bit(iHeight, iWidth, CV_8UC1);
cv::namedWindow( "Depth Map" );
// 3a. get frame reader
cout << "Try to get frame reader" << endl;
IDepthFrameReader* pFrameReader = nullptr;
if (pFrameSource->OpenReader(&pFrameReader) != S_OK)
{
cerr << "Can't get frame reader" << endl;
}
else
{
// Enter main loop
cout << "Enter main loop" << endl;
while (true)
{
// 4a. Get last frame
IDepthFrame* pFrame = nullptr;
if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK)
{
// 4c. copy the depth map to image
if (pFrame->CopyFrameDataToArray(iWidth * iHeight, reinterpret_cast<UINT16*>(mDepthImg.data)) == S_OK)
{
// 4d. convert from 16bit to 8bit
mDepthImg.convertTo(mImg8bit, CV_8U, 255.0f / uDepthMax);
cv::imshow("Depth Map", mImg8bit);
}
else
{
cerr << "Data copy error" << endl;
}
// 4e. release frame
pFrame->Release();
}
// 4f. check keyboard input
if (cv::waitKey(30) == VK_ESCAPE){
break;
}
}
// 3b. release frame reader
cout << "Release frame reader" << endl;
pFrameReader->Release();
pFrameReader = nullptr;
}
// 2d. release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
}
// 1c. Close Sensor
cout << "close sensor" << endl;
pSensor->Close();
}
// 1d. Release Sensor
cout << "Release sensor" << endl;
pSensor->Release();
pSensor = nullptr;
}
return 0;
}
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;
}
}
int main(int argc, char* argv[])
{
int similarity = 0;
string line;
ifstream myfile ("source_config.txt");
if (myfile.is_open())
{
getline (myfile,line);
hauteurCamera = stoi(line);
getline (myfile,line);
fountainXPosition = stoi(line);
getline (myfile,line);
fountainYPosition = stoi(line);
getline (myfile,line);
fountainWidth = stoi(line);
getline (myfile,line);
blasterWidth = stoi(line);
getline (myfile,line);
int numberOfBlaster = stoi(line);
for(int i = 0;i<numberOfBlaster;i++){
getline (myfile,line);
blasterXPosition.push_back(stoi(line));
getline (myfile,line);
blasterYPosition.push_back(stoi(line));
}
myfile.close();
}
else
{
cout << "Unable to open file";
exit(-1);
}
IKinectSensor* m_pKinectSensor;
IDepthFrameReader* pDepthReader;
IDepthFrameSource* pDepthFrameSource = NULL; // Depth image
IColorFrameReader* pColorReader;
IColorFrameSource* pColorFrameSource = NULL;
HRESULT hr;
hr = GetDefaultKinectSensor(&m_pKinectSensor);
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : GetDefaultKinectSensor failed." << endl;
return false;
}
hr = m_pKinectSensor->Open();
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : Open failed." << endl;
return false;
}
hr = m_pKinectSensor->get_DepthFrameSource( &pDepthFrameSource );
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : get_DepthFrameSource failed." << endl;
return false;
}
hr = pDepthFrameSource->OpenReader( &pDepthReader );
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : OpenReader failed." << endl;
return false;
}
IFrameDescription* pDepthDescription;
hr = pDepthFrameSource->get_FrameDescription( &pDepthDescription );
if( FAILED( hr ) ){
std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
w = 0,h = 0;
pDepthDescription->get_Width( &w ); // 512
pDepthDescription->get_Height( &h ); // 424
//unsigned int irBufferSize = w * h * sizeof( unsigned short );
hr = m_pKinectSensor->get_ColorFrameSource( &pColorFrameSource );
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : get_ColorFrameSource failed." << endl;
return false;
}
hr = pColorFrameSource->OpenReader( &pColorReader );
if (FAILED(hr)){
cout << "ColorTrackerv2 Error : OpenReader failed." << endl;
return false;
}
// Description
IFrameDescription* pColorDescription;
hr = pColorFrameSource->get_FrameDescription( &pColorDescription );
if( FAILED( hr ) ){
std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
int colorWidth = 0;
int colorHeight = 0;
pColorDescription->get_Width( &colorWidth ); // 1920
pColorDescription->get_Height( &colorHeight ); // 1080
unsigned int colorBufferSize = colorWidth * colorHeight * 4 * sizeof( unsigned char );
Mat colorBufferMat = Mat::zeros( cvSize(colorWidth,colorHeight), CV_8UC4 );
//colorMat = Mat::zeros( cvSize(colorWidth/2,colorHeight/2), CV_8UC4 );
ICoordinateMapper* pCoordinateMapper;
hr = m_pKinectSensor->get_CoordinateMapper( &pCoordinateMapper );
// open a opencv window
cv::namedWindow("source_config", CV_WINDOW_AUTOSIZE );
setMouseCallback("source_config", MouseCallBackFunc, NULL);
Mat frame(h,w, CV_8UC3, Scalar(255,255,255));
Mat display;
//Mat img;
char k = 0;
while(k!=27){
HRESULT hResult = S_OK;
if(displayColor){
IColorFrame* pColorFrame = nullptr;
hResult = pColorReader->AcquireLatestFrame( &pColorFrame );
while(!SUCCEEDED(hResult)){
Sleep(50);
hResult = pColorReader->AcquireLatestFrame(&pColorFrame);
}
if( SUCCEEDED( hResult ) ){
hResult = pColorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<BYTE*>( colorBufferMat.data ), ColorImageFormat::ColorImageFormat_Bgra );
if( !SUCCEEDED( hResult ) ){
return false;
}
resize(colorBufferMat,display,Size(displaySize*w,displaySize*h));
flip(display,display,1);
cv::line(display,Point(displaySize*w/2,0),Point(displaySize*w/2,displaySize*h),Scalar(0,0,255),2);
cv::line(display,Point(0,displaySize*h/2),Point(displaySize*w,displaySize*h/2),Scalar(0,0,255),2);
if (pColorFrame )
{
pColorFrame ->Release();
pColorFrame = NULL;
}
}
else
return false;
}
else
{
IDepthFrame* pDepthFrame = nullptr;
hResult = pDepthReader->AcquireLatestFrame( &pDepthFrame );
while(!SUCCEEDED(hResult)){
Sleep(10);
hResult = pDepthReader->AcquireLatestFrame( &pDepthFrame );
}
if( SUCCEEDED( hResult ) ){
unsigned int bufferSize = 0;
unsigned short* buffer = nullptr;
hResult = pDepthFrame->AccessUnderlyingBuffer( &bufferSize, &buffer );
if( SUCCEEDED( hResult ) ){
for( int y = 0; y < h; y++ ){
for( int x = 0; x < w; x++ ){
Vec3b intensity = frame.at<Vec3b>(y, x);
if(buffer[ y * w + (w - x - 1) ] < hauteurCamera){
int d = buffer[ y * w + (w - x - 1) ];
intensity.val[0] = 2.55*(d % 100);
intensity.val[1] = 1.22*(d % 200);
intensity.val[2] = 256.0*d/hauteurCamera;
}
else
{
intensity.val[0] = 255;
intensity.val[1] = 255;
intensity.val[2] = 255;
}
/*intensity.val[0] = buffer[ y * w + x ] >> 8;
intensity.val[1] = buffer[ y * w + x ] >> 8;
intensity.val[2] = buffer[ y * w + x ] >> 8;*/
frame.at<Vec3b>(y, x) = intensity;
}
}
// changer la couleur du rectangle en fonction de la hauteur des coins (similaire ou non) ( moins de 4cm)
float d1 = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))];
float d2 = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))];
float d3 = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))];
float d4 = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))];
if((d1 < 0)||(d1>3500)||(d2 < 0)||(d2>3500)||(d3 < 0)||(d3>3500)||(d4 < 0)||(d4>3500)){
similarity = 0;
}
else
{
int mn = 100;
if((abs(d1-d2) < mn)
&&(abs(d1-d3) < mn)
&&(abs(d1-d4) < mn)
&&(abs(d2-d3) < mn)
&&(abs(d2-d4) < mn)
&&(abs(d3-d4) < mn))
similarity = 255;
else{
int md = abs(d1-d2);
md = MAX(md,abs(d1-d3));
md = MAX(md,abs(d1-d4));
md = MAX(md,abs(d2-d3));
md = MAX(md,abs(d2-d4));
md = MAX(md,abs(d3-d4));
if(md-mn>128)
similarity = 0;
else
similarity = 128 - (md - mn);
}
}
if(k=='s'){
// get hauteur camera
// Depthframe get 3D position of 1m20 jets et les enregistrer dans un autre fichier pour etre charger par un Observer
CameraSpacePoint cameraPoint = { 0 };
DepthSpacePoint depthPoint = { 0 };
UINT16 depth;
// Compute hauteur camera, by average of four points
float h = 0;
int cptH = 0;
float d;
d = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))];
if((d>500)&&(d<3500)){h+=d; cptH++;}
d = buffer[(int)(fountainYPosition-fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))];
if((d>500)&&(d<3500)){h+=d; cptH++;}
d = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition-fountainWidth/2.0))];
if((d>500)&&(d<3500)){h+=d; cptH++;}
d = buffer[(int)(fountainYPosition+fountainWidth/2.0)*w + (w-1-(int)(fountainXPosition+fountainWidth/2.0))];
if((d>500)&&(d<3500)){h+=d; cptH++;}
if(cptH>0){
//hauteurCamera = h/cptH;
cout << "H = " << hauteurCamera << endl;
}
// Compute real size of blaster
/*depthPoint.X = static_cast<float>(blasterXPosition[0] - blasterWidth);
depthPoint.Y = static_cast<float>(blasterYPosition[0] - blasterWidth);
depth = hauteurCamera - 1000.0f; // TODO change
pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint);
float corner1X = static_cast<float>(cameraPoint.X);
float corner1Y = static_cast<float>(cameraPoint.Y);
depthPoint.X = static_cast<float>(blasterXPosition[0] + blasterWidth);
depthPoint.Y = static_cast<float>(blasterYPosition[0] + blasterWidth);
pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint);
float corner2X = static_cast<float>(cameraPoint.X);
float corner2Y = static_cast<float>(cameraPoint.Y);*/
//float realBlasterWidth = 1000.0*(abs(corner2X-corner1X)+abs(corner2Y-corner1Y))/2.0;
ofstream myfile;
myfile.open ("source_config.txt");
myfile << hauteurCamera << "\n";
myfile << fountainXPosition << "\n";
myfile << fountainYPosition << "\n";
myfile << fountainWidth << "\n";
myfile << blasterWidth << "\n";
myfile << blasterXPosition.size() << "\n";
for(int i = 0;i<blasterXPosition.size();i++){
myfile << blasterXPosition[i] << "\n";
myfile << blasterYPosition[i] << "\n";
}
myfile.close();
// save real positions to file
myfile.open ("source3D.txt");
myfile << hauteurCamera << "\n";
myfile << blasterWidth << "\n";
myfile << blasterXPosition.size() << "\n";
for(int i = 0;i<blasterXPosition.size();i++){
depthPoint.X = static_cast<float>(blasterXPosition[i]);
depthPoint.Y = static_cast<float>(blasterYPosition[i]);
depth = hauteurCamera;
pCoordinateMapper->MapDepthPointToCameraSpace(depthPoint,depth,&cameraPoint);
cout << depthPoint.X << " - " << depthPoint.Y << endl;
cout << static_cast<float>(cameraPoint.X) << " - " << static_cast<float>(cameraPoint.Y) << endl;
pCoordinateMapper->MapCameraPointToDepthSpace(cameraPoint, &depthPoint);
cout << depthPoint.X << " - " << depthPoint.Y << endl;
cout << static_cast<float>(cameraPoint.X) << " - " << static_cast<float>(cameraPoint.Y) << endl;
myfile << 1000.0*static_cast<float>(cameraPoint.X) << "\n";
myfile << 1000.0*static_cast<float>(cameraPoint.Y) << "\n";
}
myfile.close();
}
}
else{
return false;
}
}
else
return false;
if( pDepthFrame != NULL ){
pDepthFrame->Release();
pDepthFrame = NULL;
}
rectangle(frame,
Rect(fountainXPosition-fountainWidth/2.0,
fountainYPosition-fountainWidth/2.0,
fountainWidth ,
fountainWidth),
Scalar(0,similarity,255-similarity),
3);
for(int i = -2; i <= 2; i++)
{
rectangle(frame,
Rect(fountainXPosition - (i*blasterWidth) - (blasterWidth/2.0),
fountainYPosition-fountainWidth/2.0,
blasterWidth,
fountainWidth),
Scalar(0,255,0),
1);
rectangle(frame,
Rect(fountainXPosition-fountainWidth/2.0,
fountainYPosition - (i*blasterWidth) - (blasterWidth/2.0),
fountainWidth,
blasterWidth),
Scalar(0,255,0),
1);
}
char textbuffer [33];
for(int i = 0;i<blasterXPosition.size();i++){
sprintf(textbuffer,"%i",i+1);
putText(frame,textbuffer, Point2f(blasterXPosition[i]-blasterWidth/2.0,blasterYPosition[i]), FONT_HERSHEY_PLAIN, 1, Scalar(0,0,255,255), 2);
//rectangle(frame,Rect(blasterXPosition[i]-blasterWidth/2.0,blasterYPosition[i]-blasterWidth/2.0,blasterWidth,blasterWidth),Scalar(255,0,0));
circle(frame,Point(blasterXPosition[i],blasterYPosition[i]),blasterWidth/2.0,Scalar(255,0,0));
}
resize(frame,display,Size(displaySize*w,displaySize*h));
}
cv::imshow("source_config", display);
k=cv::waitKey(1);
if(k == 32) // Space
displayColor = ! displayColor;
if(k=='a')
alignBuseFromLayout();
if(k=='r')
resetFountainPosition();
}
if (pDepthReader)
{
pDepthReader->Release();
pDepthReader = NULL;
}
if (pCoordinateMapper)
{
pCoordinateMapper->Release();
pCoordinateMapper = NULL;
}
m_pKinectSensor->Close();
if (m_pKinectSensor)
{
m_pKinectSensor->Release();
m_pKinectSensor = NULL;
}
//system("pause");
return 0;
}
void* Kinect2StreamImpl::populateFrameBuffer(int& buffWidth, int& buffHeight)
{
buffWidth = 0;
buffHeight = 0;
if (m_sensorType == ONI_SENSOR_COLOR) {
if (m_pFrameReader.color && m_pFrameBuffer.color) {
buffWidth = 1920;
buffHeight = 1080;
IColorFrame* frame = NULL;
HRESULT hr = m_pFrameReader.color->AcquireLatestFrame(&frame);
if (SUCCEEDED(hr)) {
ColorImageFormat imageFormat = ColorImageFormat_None;
hr = frame->get_RawColorImageFormat(&imageFormat);
if (SUCCEEDED(hr)) {
if (imageFormat == ColorImageFormat_Bgra) {
RGBQUAD* data;
UINT bufferSize;
frame->AccessRawUnderlyingBuffer(&bufferSize, reinterpret_cast<BYTE**>(&data));
memcpy(m_pFrameBuffer.color, data, 1920*1080*sizeof(RGBQUAD));
}
else {
frame->CopyConvertedFrameDataToArray(1920*1080*sizeof(RGBQUAD), reinterpret_cast<BYTE*>(m_pFrameBuffer.color), ColorImageFormat_Bgra);
}
}
}
if (frame) {
frame->Release();
}
return reinterpret_cast<void*>(m_pFrameBuffer.color);
}
}
else if (m_sensorType == ONI_SENSOR_DEPTH) {
if (m_pFrameReader.depth && m_pFrameBuffer.depth) {
buffWidth = 512;
buffHeight = 424;
IDepthFrame* frame = NULL;
HRESULT hr = m_pFrameReader.depth->AcquireLatestFrame(&frame);
if (SUCCEEDED(hr)) {
UINT16* data;
UINT bufferSize;
frame->AccessUnderlyingBuffer(&bufferSize, &data);
memcpy(m_pFrameBuffer.depth, data, 512*424*sizeof(UINT16));
}
if (frame) {
frame->Release();
}
return reinterpret_cast<void*>(m_pFrameBuffer.depth);
}
}
else { // ONI_SENSOR_IR
if (m_pFrameReader.infrared && m_pFrameBuffer.infrared) {
buffWidth = 512;
buffHeight = 424;
IInfraredFrame* frame = NULL;
HRESULT hr = m_pFrameReader.infrared->AcquireLatestFrame(&frame);
if (SUCCEEDED(hr)) {
UINT16* data;
UINT bufferSize;
frame->AccessUnderlyingBuffer(&bufferSize, &data);
memcpy(m_pFrameBuffer.infrared, data, 512*424*sizeof(UINT16));
}
if (frame) {
frame->Release();
}
return reinterpret_cast<void*>(m_pFrameBuffer.infrared);
}
}
return NULL;
}
void KinectDevice::listen() {
if (_listening) throw std::exception("Already listening for new frames");
_listening = true;
while (_listening) {
int idx = WaitForSingleObject((HANDLE) _frameEvent, 100);
switch (idx) {
case WAIT_TIMEOUT:
std::cout << ".";
continue;
case WAIT_OBJECT_0:
IMultiSourceFrameArrivedEventArgs *frameArgs = nullptr;
IMultiSourceFrameReference *frameRef = nullptr;
HRESULT hr = _reader->GetMultiSourceFrameArrivedEventData(_frameEvent, &frameArgs);
if (hr == S_OK) {
hr = frameArgs->get_FrameReference(&frameRef);
frameArgs->Release();
}
if (hr == S_OK) {
//if (_lastFrame) _lastFrame->Release();
hr = frameRef->AcquireFrame(&_lastFrame);
frameRef->Release();
}
if (hr == S_OK) {
// Store frame data
//std::cout << "Got a frame YEAH" << std::endl;
IDepthFrameReference *depthRef = nullptr;
IColorFrameReference *colorRef = nullptr;
IInfraredFrameReference *irRef = nullptr;
ILongExposureInfraredFrameReference *hdirRef = nullptr;
IBodyIndexFrameReference *indexRef = nullptr;
IDepthFrame *depth = nullptr;
IColorFrame *color = nullptr;
IInfraredFrame *ir = nullptr;
ILongExposureInfraredFrame *hdir = nullptr;
IBodyIndexFrame *index = nullptr;
size_t size;
uint16_t *buff;
BYTE *cbuff;
frameLock.lock();
if (_streams & Streams::DEPTH_STREAM) {
_lastFrame->get_DepthFrameReference(&depthRef);
depthRef->AcquireFrame(&depth);
if (depth) {
depthSwap();
depth->AccessUnderlyingBuffer(&size, &buff);
memcpy(depthData.get(), buff, size * sizeof(uint16_t));
depth->Release();
}
depthRef->Release();
}
if (_streams & Streams::COLOR_STREAM) {
_lastFrame->get_ColorFrameReference(&colorRef);
colorRef->AcquireFrame(&color);
//color->AccessUnderlyingBuffer(&size, &buff);
//memcpy(_colorData.get(), buff, size);
color->Release();
colorRef->Release();
}
if (_streams & Streams::IR_STREAM) {
_lastFrame->get_InfraredFrameReference(&irRef);
irRef->AcquireFrame(&ir);
ir->AccessUnderlyingBuffer(&size, &buff);
memcpy(irData.get(), buff, size);
ir->Release();
irRef->Release();
}
if (_streams & Streams::HDIR_STREAM) {
_lastFrame->get_LongExposureInfraredFrameReference(&hdirRef);
hdirRef->AcquireFrame(&hdir);
hdir->AccessUnderlyingBuffer(&size, &buff);
memcpy(hdirData.get(), buff, size);
hdir->Release();
hdirRef->Release();
}
if (_streams & Streams::INDEX_STREAM) {
_lastFrame->get_BodyIndexFrameReference(&indexRef);
indexRef->AcquireFrame(&index);
index->AccessUnderlyingBuffer(&size, &cbuff);
memcpy(indexData.get(), cbuff, size);
index->Release();
indexRef->Release();
}
frameLock.unlock();
_lastFrame->Release();
}
}
}
}
int TextureManager::CreateSensorTexture(char *errorString, const char *name) {
if (!depth_frame_reader_) {
sprintf_s(errorString, MAX_ERROR_LENGTH,
"No depth sensor exists for texture creation");
return -1;
}
glGenTextures(1, textureID + numTextures);
strcpy_s(textureName[numTextures], TM_MAX_FILENAME_LENGTH, name);
glBindTexture(GL_TEXTURE_2D, textureID[numTextures]);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
//gluBuild2DMipmaps(GL_TEXTURE_2D, GL_RGBA,
// TM_NOISE_TEXTURE_SIZE, TM_NOISE_TEXTURE_SIZE,
// GL_BGRA, GL_UNSIGNED_BYTE, noiseIntData);
IDepthFrame* pDepthFrame = NULL;
HRESULT hr;
bool hasSucceeded = false;
for (int tries = 0; tries < 20 && !hasSucceeded; tries++) {
Sleep(100);
hr = depth_frame_reader_->AcquireLatestFrame(&pDepthFrame);
if (SUCCEEDED(hr)) hasSucceeded = true;
}
if (!hasSucceeded) {
sprintf_s(errorString, MAX_ERROR_LENGTH,
"Could not acquire last depth sensor frame");
return -1;
}
pDepthFrame->get_RelativeTime(&last_frame_time_);
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
hr = pDepthFrame->get_FrameDescription(&pFrameDescription);
if (FAILED(hr)) {
pDepthFrame->Release();
sprintf_s(errorString, MAX_ERROR_LENGTH,
"Could not get Depth Frame description");
return -1;
}
pFrameDescription->get_Width(&nWidth);
pFrameDescription->get_Height(&nHeight);
depth_sensor_width_ = nWidth;
depth_sensor_height_ = nHeight;
if (cpu_depth_sensor_buffer_) delete[] cpu_depth_sensor_buffer_;
cpu_depth_sensor_buffer_ = new float[nWidth * nHeight];
memset(cpu_depth_sensor_buffer_, 0, nWidth * nHeight);
if (smoothed_depth_sensor_buffer_[0]) {
delete[] smoothed_depth_sensor_buffer_[0];
delete[] smoothed_depth_sensor_buffer_[1];
}
smoothed_depth_sensor_buffer_[0] = new float[nWidth * nHeight];
smoothed_depth_sensor_buffer_[1] = new float[nWidth * nHeight];
memset(smoothed_depth_sensor_buffer_[0], 0,
nWidth*nHeight*sizeof(smoothed_depth_sensor_buffer_[0][0]));
memset(smoothed_depth_sensor_buffer_[1], 0,
nWidth*nHeight*sizeof(smoothed_depth_sensor_buffer_[1][0]));
glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F,
nWidth, nHeight,
0, GL_RED, GL_FLOAT, smoothed_depth_sensor_buffer_[0]);
textureWidth[numTextures] = nWidth;
textureHeight[numTextures] = nHeight;
numTextures++;
pFrameDescription->Release();
pDepthFrame->Release();
return 0;
}
int main(int argc, char** argv)
{
int first_time = 0;
Size screen_size(1440, 900);//the dst image size,e.g.100x100
Scalar text_color = Scalar(0, 255, 0);
Scalar text_color2 = Scalar(0, 255, 255);
Scalar text_color3 = Scalar(0, 0, 255);
inhaler_coach coach;
coach.control = 0;
thread mThread(test_func, &coach);
// 1a. Get Kinect Sensor
cout << "Try to get default sensor" << endl;
IKinectSensor* pSensor = nullptr;
if (GetDefaultKinectSensor(&pSensor) != S_OK)
{
cerr << "Get Sensor failed" << endl;
return -1;
}
// 1b. Open sensor
cout << "Try to open sensor" << endl;
if (pSensor->Open() != S_OK)
{
cerr << "Can't open sensor" << endl;
return -1;
}
// 2. Color Related code
IColorFrameReader* pColorFrameReader = nullptr;
cv::Mat mColorImg;
UINT uBufferSize = 0;
UINT uColorPointNum = 0;
int iWidth = 0;
int iHeight = 0;
{
// 2a. Get color frame source
cout << "Try to get color source" << endl;
IColorFrameSource* pFrameSource = nullptr;
if (pSensor->get_ColorFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get color frame source" << endl;
return -1;
}
// 2b. Get frame description
cout << "get color frame description" << endl;
IFrameDescription* pFrameDescription = nullptr;
if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK)
{
pFrameDescription->get_Width(&iWidth);
pFrameDescription->get_Height(&iHeight);
}
pFrameDescription->Release();
pFrameDescription = nullptr;
// 2c. get frame reader
cout << "Try to get color frame reader" << endl;
if (pFrameSource->OpenReader(&pColorFrameReader) != S_OK)
{
cerr << "Can't get color frame reader" << endl;
return -1;
}
// 2d. release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
// Prepare OpenCV data
mColorImg = cv::Mat(iHeight, iWidth, CV_8UC4);
uBufferSize = iHeight * iWidth * 4 * sizeof(BYTE);
uColorPointNum = iHeight * iWidth;
}
// 3. Depth related code
IDepthFrameReader* pDepthFrameReader = nullptr;
UINT uDepthPointNum = 0;
int iDepthWidth = 0, iDepthHeight = 0;
cout << "Try to get depth source" << endl;
{
// Get frame source
IDepthFrameSource* pFrameSource = nullptr;
if (pSensor->get_DepthFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get depth frame source" << endl;
return -1;
}
// Get frame description
cout << "get depth frame description" << endl;
IFrameDescription* pFrameDescription = nullptr;
if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK)
{
pFrameDescription->get_Width(&iDepthWidth);
pFrameDescription->get_Height(&iDepthHeight);
uDepthPointNum = iDepthWidth * iDepthHeight;
}
pFrameDescription->Release();
pFrameDescription = nullptr;
// get frame reader
cout << "Try to get depth frame reader" << endl;
if (pFrameSource->OpenReader(&pDepthFrameReader) != S_OK)
{
cerr << "Can't get depth frame reader" << endl;
return -1;
}
// release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
}
// 4. Body related code
IBodyFrameReader* pBodyFrameReader = nullptr;
IBody** aBodyData = nullptr;
INT32 iBodyCount = 0;
{
// 3a. Get frame source
cout << "Try to get body source" << endl;
IBodyFrameSource* pFrameSource = nullptr;
if (pSensor->get_BodyFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get body frame source" << endl;
return -1;
}
// 3b. Get the number of body
if (pFrameSource->get_BodyCount(&iBodyCount) != S_OK)
{
cerr << "Can't get body count" << endl;
return -1;
}
cout << " > Can trace " << iBodyCount << " bodies" << endl;
aBodyData = new IBody*[iBodyCount];
for (int i = 0; i < iBodyCount; ++i)
aBodyData[i] = nullptr;
// 3c. get frame reader
cout << "Try to get body frame reader" << endl;
if (pFrameSource->OpenReader(&pBodyFrameReader) != S_OK)
{
cerr << "Can't get body frame reader" << endl;
return -1;
}
// 3d. release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
}
// 4. Body Index releated code
IBodyIndexFrameReader* pBIFrameReader = nullptr;
cout << "Try to get body index source" << endl;
{
// Get frame source
IBodyIndexFrameSource* pFrameSource = nullptr;
if (pSensor->get_BodyIndexFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get body index frame source" << endl;
return -1;
}
// get frame reader
cout << "Try to get body index frame reader" << endl;
if (pFrameSource->OpenReader(&pBIFrameReader) != S_OK)
{
cerr << "Can't get depth frame reader" << endl;
return -1;
}
// release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
}
// 5. background
cv::Mat imgBG(iHeight, iWidth, CV_8UC3);
imgBG.setTo(0);
// 4. get CoordinateMapper
ICoordinateMapper* pCoordinateMapper = nullptr;
if (pSensor->get_CoordinateMapper(&pCoordinateMapper) != S_OK)
{
cout << "Can't get coordinate mapper" << endl;
return -1;
}
// Enter main loop
UINT16* pDepthPoints = new UINT16[uDepthPointNum];
BYTE* pBodyIndex = new BYTE[uDepthPointNum];
DepthSpacePoint* pPointArray = new DepthSpacePoint[uColorPointNum];
cv::namedWindow("Inhaler Coach");
while (true)
{
// 4a. Get last frame
IColorFrame* pColorFrame = nullptr;
if (pColorFrameReader->AcquireLatestFrame(&pColorFrame) == S_OK)
{
pColorFrame->CopyConvertedFrameDataToArray(uBufferSize, mColorImg.data, ColorImageFormat_Bgra);
pColorFrame->Release();
pColorFrame = nullptr;
}
cv::Mat mImg = mColorImg.clone();
// 8b. read depth frame
IDepthFrame* pDepthFrame = nullptr;
if (pDepthFrameReader->AcquireLatestFrame(&pDepthFrame) == S_OK)
{
pDepthFrame->CopyFrameDataToArray(uDepthPointNum, pDepthPoints);
pDepthFrame->Release();
pDepthFrame = nullptr;
}
// 8c. read body index frame
IBodyIndexFrame* pBIFrame = nullptr;
if (pBIFrameReader->AcquireLatestFrame(&pBIFrame) == S_OK)
{
pBIFrame->CopyFrameDataToArray(uDepthPointNum, pBodyIndex);
pBIFrame->Release();
pBIFrame = nullptr;
}
#ifdef COACH_DEBUG
cv::Mat imgTarget = imgBG.clone();
// 9b. map color to depth
if (pCoordinateMapper->MapColorFrameToDepthSpace(uDepthPointNum, pDepthPoints, uColorPointNum, pPointArray) == S_OK)
{
for (int y = 0; y < imgTarget.rows; ++y)
{
for (int x = 0; x < imgTarget.cols; ++x)
{
// ( x, y ) in color frame = rPoint in depth frame
const DepthSpacePoint& rPoint = pPointArray[y * imgTarget.cols + x];
// check if rPoint is in range
if (rPoint.X >= 0 && rPoint.X < iDepthWidth && rPoint.Y >= 0 && rPoint.Y < iDepthHeight)
{
// fill color from color frame if this pixel is user
int iIdx = (int)rPoint.X + iDepthWidth * (int)rPoint.Y;
if (pBodyIndex[iIdx] < 6)
{
cv::Vec4b& rPixel = mImg.at<cv::Vec4b>(y, x);
imgTarget.at<cv::Vec3b>(y, x) = cv::Vec3b(rPixel[0], rPixel[1], rPixel[2]);
}
}
}
}
}
#else
cv::Mat imgTarget = mImg.clone();
#endif
// 4b. Get body data
IBodyFrame* pBodyFrame = nullptr;
if (pBodyFrameReader->AcquireLatestFrame(&pBodyFrame) == S_OK)
{
// 4b. get Body data
if (pBodyFrame->GetAndRefreshBodyData(iBodyCount, aBodyData) == S_OK)
{
// 4c. for each body
for (int i = 0; i < iBodyCount; ++i)
{
IBody* pBody = aBodyData[i];
// check if is tracked
BOOLEAN bTracked = false;
if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked)
{
// get joint position
Joint aJoints[JointType::JointType_Count];
if (pBody->GetJoints(JointType::JointType_Count, aJoints) == S_OK)
{
if (coach.state == 0){
coach.state = 1;
if (first_time == 0){
first_time = 1;
PlaySound(TEXT("welcome.wav"), NULL, SND_FILENAME);
}
}
#ifdef COACH_DEBUG
DrawLine(imgTarget, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper);
//DrawLine(imgTarget, aJoints[JointType_HandLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper);
DrawLine(imgTarget, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper);
//DrawLine(imgTarget, aJoints[JointType_HandRight], aJoints[JointType_ThumbRight], pCoordinateMapper);
#endif
ColorSpacePoint q;
ColorSpacePoint head;
//ColorSpacePoint w;
pCoordinateMapper->MapCameraPointToColorSpace(aJoints[JointType_Head].Position, &head);
// check shaking
coach.shaking_detection(aJoints, pCoordinateMapper);
q = coach.position_checking(aJoints, pCoordinateMapper);
#ifdef COACH_DEBUG
circle(imgTarget, cv::Point(q.X, q.Y), 10, Scalar(0, 255, 255), 10, 8, 0);
//circle(imgTarget, cv::Point(q.X, q.Y), 10, Scalar(0, 255, 255), 10, 8, 0);
rectangle(imgTarget, Point(head.X - 50, head.Y - 40), Point(head.X + 50, head.Y + 90), Scalar(0, 255, 255), 1, 8, 0);
//circle(imgTarget, cv::Point(w.X, w.Y), 10, Scalar(255, 0, 255), 10, 8, 0);
#endif
coach.state_change_rule();
}
}
}
}
else
{
cerr << "Can't read body data" << endl;
}
// 4e. release frame
pBodyFrame->Release();
}
switch (coach.state){
case 0: putText(imgTarget, "CMU Inhaler Coaching System", Point(120, 120), FONT_HERSHEY_DUPLEX, 2, text_color); break;
case 1: putText(imgTarget, "Please shake the inhaler", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break;
case 2: putText(imgTarget, "Shaking detected", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break;
case 3: putText(imgTarget, "Please put the inhaler in front of your mouth", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break;
case 4: putText(imgTarget, "Position check OK", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color2); break;
case 5: putText(imgTarget, "You forget to shake the inhaler first!!!", Point(20, 120), FONT_HERSHEY_DUPLEX, 2, text_color3); break;
}
// show image
Mat dst;
resize(imgTarget, dst, screen_size);
imshow("Coach", dst);
// 4c. check keyboard input
if (cv::waitKey(30) == VK_ESCAPE){
break;
}
}
mThread.join();
// 3. delete body data array
delete[] aBodyData;
// 3. release frame reader
cout << "Release body frame reader" << endl;
pBodyFrameReader->Release();
pBodyFrameReader = nullptr;
// 2. release color frame reader
cout << "Release color frame reader" << endl;
pColorFrameReader->Release();
pColorFrameReader = nullptr;
// 1c. Close Sensor
cout << "close sensor" << endl;
pSensor->Close();
// 1d. Release Sensor
cout << "Release sensor" << endl;
pSensor->Release();
pSensor = nullptr;
return 0;
}
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;
}
}
