void KinectManager::Update(void)
{
if (!m_pColorFrameReader)
{
return;
}
IColorFrame* pColorFrame = NULL;
HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_BytesPerPixel(&color_frame_bytes_per_pixel);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_LengthInPixels(&color_frame_length_in_pixels);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Rgba)
{
UINT nBufferSize = KINECT_STREAM_COLOR_WIDTH * KINECT_STREAM_COLOR_HEIGHT * color_frame_bytes_per_pixel;
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&m_pColorRGBX));
}
else if (m_pColorRGBX)
{
IFrameDescription* pRGBFrameDescription = NULL;
pColorFrame->CreateFrameDescription(ColorImageFormat_Rgba, &pRGBFrameDescription);
unsigned int clpp = 0;
pRGBFrameDescription->get_BytesPerPixel(&clpp);
unsigned int clip = 0;
pRGBFrameDescription->get_LengthInPixels(&clip);
color_frame_bytes_per_pixel = clpp;
color_frame_length_in_pixels = clip;
UINT nBufferSize = clip * clpp;
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, (reinterpret_cast<BYTE*>(&m_pColorRGBX)), ColorImageFormat_Rgba);
}
else
{
hr = E_FAIL;
}
}
SafeRelease(pFrameDescription);
}
SafeRelease(pColorFrame);
}
void processColor() {
if (!device) return;
if (!m_pColorFrameReader) return;
IColorFrame* pColorFrame = NULL;
HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr)) {
INT64 nTime = 0;
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nBufferSize = 0;
RGBQUAD *src = NULL;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr)) {
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr)) {
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr)) {
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr)) {
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (imageFormat != ColorImageFormat_Bgra)
{
if (!rgb_buffer) {
rgb_buffer = new RGBQUAD[nWidth * nHeight];
}
//post("image format %d", imageFormat);
//error("not brga");
nBufferSize = nWidth * nHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(rgb_buffer), ColorImageFormat_Rgba);
if (FAILED(hr)) {
error("failed to convert image");
return;
}
src = rgb_buffer;
}
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&src));
ARGB * dst = (ARGB *)rgb_mat.back;
int cells = nWidth * nHeight;
//if (align_depth_to_color) {
for (int i = 0; i < cells; ++i) {
dst[i].r = src[i].rgbRed;
dst[i].g = src[i].rgbGreen;
dst[i].b = src[i].rgbBlue;
}
/*}
else {
// align color to depth:
//std::fill(dst, dst + cells, RGB(0, 0, 0));
for (int i = 0; i < cells; ++i) {
int c = colorCoordinates[i * 2];
int r = colorCoordinates[i * 2 + 1];
if (c >= 0 && c < KINECT_DEPTH_WIDTH
&& r >= 0 && r < KINECT_DEPTH_HEIGHT) {
// valid location: depth value:
int idx = r*KINECT_DEPTH_WIDTH + c;
dst[i].r = src[idx].r;
dst[i].g = src[idx].g;
dst[i].b = src[idx].b;
}
}
}*/
new_rgb_data = 1;
}
}
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 MyKinect2::Update()
{
// récupération de l'image en 2D
if (!m_pColorFrameReader)
{
return;
}
IColorFrame* pColorFrame = NULL;
HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nBufferSize = 0;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr) && (nWidth == cColorWidth) && (nHeight == cColorHeight))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&webcam.data));
}
else if (m_pColorRGBX)
{
nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(webcam.data), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
SafeRelease(pFrameDescription);
}
// récupération du squelette
if (!m_pBodyFrameReader)
{
return;
}
IBodyFrame* pBodyFrame = NULL;
hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
hr = pBodyFrame->get_RelativeTime(&nTime);
IBody* ppBodies[BODY_COUNT] = {0};
if (SUCCEEDED(hr))
{
hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
}
if (SUCCEEDED(hr))
{
ProcessBody(BODY_COUNT, ppBodies); // BODY_COUNT est un define de Kinect.h égal à 6... peut etre mettre 1 plus tard afin déviter des problemes lors de la récupération des positions de joints
}
for (int i = 0; i < _countof(ppBodies); ++i)
{
SafeRelease(ppBodies[i]);
}
}
SafeRelease(pBodyFrame);
SafeRelease(pColorFrame);
}
bool ColorStream::readFrame(IMultiSourceFrame *multiFrame)
{
bool readed = false;
if (!m_StreamHandle.colorFrameReader) {
ofLogWarning("ofxKinect2::ColorStream") << "Stream is not open.";
return readed;
}
Stream::readFrame(multiFrame);
IColorFrame *colorFrame = nullptr;
HRESULT hr = E_FAIL;
if (!multiFrame) {
hr = m_StreamHandle.colorFrameReader->AcquireLatestFrame(&colorFrame);
}
else {
IColorFrameReference *colorFrameFeference = nullptr;
hr = multiFrame->get_ColorFrameReference(&colorFrameFeference);
if (SUCCEEDED(hr)) {
hr = colorFrameFeference->AcquireFrame(&colorFrame);
}
safeRelease(colorFrameFeference);
}
if (SUCCEEDED(hr)) {
IFrameDescription *colorFrameDescription = nullptr;
ColorImageFormat imageFormat = ColorImageFormat_None;
hr = colorFrame->get_RelativeTime((INT64 *)&m_Frame.timestamp);
if (SUCCEEDED(hr)) {
hr = colorFrame->get_FrameDescription(&colorFrameDescription);
}
if (SUCCEEDED(hr)) {
hr = colorFrameDescription->get_Width(&m_Frame.width);
}
if (SUCCEEDED(hr)) {
hr = colorFrameDescription->get_Height(&m_Frame.height);
}
if (SUCCEEDED(hr)) {
hr = colorFrameDescription->get_HorizontalFieldOfView(&m_Frame.horizontalFieldOfView);
}
if (SUCCEEDED(hr)) {
hr = colorFrameDescription->get_VerticalFieldOfView(&m_Frame.verticalFieldOfView);
}
if (SUCCEEDED(hr)) {
hr = colorFrameDescription->get_DiagonalFieldOfView(&m_Frame.diagonalFieldOfView);
}
if (SUCCEEDED(hr)) {
hr = colorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr)) {
if (m_Buffer == nullptr) {
m_Buffer = new unsigned char[m_Frame.width * m_Frame.height * 4];
}
if (imageFormat == ColorImageFormat_Rgba) {
hr = colorFrame->AccessRawUnderlyingBuffer((UINT *)&m_Frame.dataSize, reinterpret_cast<BYTE **>(&m_Frame.data));
}
else {
m_Frame.data = m_Buffer;
m_Frame.dataSize = m_Frame.width * m_Frame.height * 4 * sizeof(unsigned char);
hr = colorFrame->CopyConvertedFrameDataToArray((UINT)m_Frame.dataSize, reinterpret_cast<BYTE *>(m_Frame.data), ColorImageFormat_Rgba);
}
}
if (SUCCEEDED(hr)) {
readed = true;
setPixels(m_Frame);
}
safeRelease(colorFrameDescription);
}
safeRelease(colorFrame);
return readed;
}
void UpdateColor()
{
if (!m_pColorFrameReader)
{
return;
}
IColorFrame* pColorFrame = NULL;
HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nBufferSize = 0;
RGBQUAD *pBuffer = NULL;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr))
{
m_nColorWidth = nWidth;
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr))
{
m_nColorHeight = nHeight;
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer));
}
else if (m_pColorRGBX)
{
pBuffer = m_pColorRGBX;
nBufferSize = nWidth * nHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
{
{
ProcessColor(nTime, pBuffer, nWidth, nHeight);
}
}
SafeRelease(pFrameDescription);
}
else{
DumpHR(hr);
}
SafeRelease(pColorFrame);
}
void Microsoft2Grabber::ColorFrameArrived(IColorFrameReference* pColorFrameReference) {
IColorFrame* pColorFrame = NULL;
HRESULT hr = pColorFrameReference->AcquireFrame(&pColorFrame);
if(FAILED(hr)) {
//cout << "Couldn't acquire color frame" << endl;
return;
}
//cout << "got a color frame" << endl;
INT64 nColorTime = 0;
IFrameDescription* pColorFrameDescription = NULL;
int nColorWidth = 0;
int nColorHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nColorBufferSize = 0;
RGBQUAD *pColorBuffer = NULL;
// get color frame data
hr = pColorFrame->get_RelativeTime(&nColorTime);
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);
//cout << "creating the image" << endl;
Mat tmp = Mat(m_colorSize, COLOR_PIXEL_TYPE, pColorBuffer, Mat::AUTO_STEP);
boost::shared_ptr<Mat> img(new Mat());
*img = tmp.clone();
m_rgbTime = nColorTime;
if (image_signal_->num_slots () > 0) {
//cout << "img signal num slot!" << endl;
image_signal_->operator()(img);
}
if (num_slots<sig_cb_microsoft_point_cloud_rgba>() > 0 || all_data_signal_->num_slots() > 0 || image_depth_image_signal_->num_slots() > 0) {
rgb_sync_.add0 (img, m_rgbTime);
}
//ReleaseMutex(hColorMutex);
}
}
SafeRelease(pColorFrameDescription);
SafeRelease(pColorFrame);
}
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;
}
/// <summary>
/// Main processing function
/// </summary>
void CFaceBasics::Update()
{
if (!m_pColorFrameReader || !m_pBodyFrameReader)
{
return;
}
IColorFrame* pColorFrame = nullptr;
HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pFrameDescription = nullptr;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nBufferSize = 0;
RGBQUAD *pBuffer = nullptr;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer));
}
else if (m_pColorRGBX)
{
pBuffer = m_pColorRGBX;
nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
{
DrawStreams(nTime, pBuffer, nWidth, nHeight);
}
SafeRelease(pFrameDescription);
}
SafeRelease(pColorFrame);
}
/// <summary>
/// Main processing function
/// </summary>
// この関数がループ処理される
void CColorBasics::Update()
{
HRESULT hr = NULL;
// (1) カラーフレーム（背景描画のみに使用）
if (m_pColorFrameReader)
{
IColorFrame* pColorFrame = NULL;
hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hr))
{
INT64 nTime = 0;
IFrameDescription* pFrameDescription = NULL;
int nWidth = 0;
int nHeight = 0;
ColorImageFormat imageFormat = ColorImageFormat_None;
UINT nBufferSize = 0;
RGBQUAD *pBuffer = NULL;
hr = pColorFrame->get_RelativeTime(&nTime);
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Width(&nWidth);
}
if (SUCCEEDED(hr))
{
hr = pFrameDescription->get_Height(&nHeight);
}
if (SUCCEEDED(hr))
{
hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
}
if (SUCCEEDED(hr))
{
if (imageFormat == ColorImageFormat_Bgra)
{
hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast<BYTE**>(&pBuffer));
}
else if (m_pColorRGBX)
{
pBuffer = m_pColorRGBX;
nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pBuffer), ColorImageFormat_Bgra);
}
else
{
hr = E_FAIL;
}
}
if (SUCCEEDED(hr))
{
ProcessColor(nTime, pBuffer, nWidth, nHeight);
}
SafeRelease(pFrameDescription);
}
SafeRelease(pColorFrame);
}
// これ以降にBody処理を実装
TIMESPAN nBodyTime = 0;
if (m_pBodyFrameReader)
{
IBodyFrame* pBodyFrame = NULL;
hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame);
if (SUCCEEDED(hr))
{
hr = pBodyFrame->get_RelativeTime(&nBodyTime);
// ここに、UI描画処理 ボタン等
// ゲームのステータスによって描画を設定
if (!m_pGame){ m_pGame = new CSemaphoreGame(m_pDrawColor, m_pCoordinateMapper); }
if (m_pGame){ m_pGame->Display(nBodyTime); }
IBody* ppBodies[BODY_COUNT] = { 0 };
if (SUCCEEDED(hr))
{
hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
}
if (SUCCEEDED(hr))
{
// ここに、UI処理->ProcessBody()内でOK
// ボディデータにてUI処理を行う
// ステータスの変更
if (m_pGame){ m_pGame->Play(nBodyTime, BODY_COUNT, ppBodies); }
ProcessBody(nBodyTime, BODY_COUNT, ppBodies);
}
for (int i = 0; i < _countof(ppBodies); ++i)
{
SafeRelease(ppBodies[i]);
}
}
SafeRelease(pBodyFrame);
}
return;
}
