#include "ofApp.h"
//--------------------------------------------------------------
void ofApp::setup() {
ofSetFrameRate(30);
ofBackground(0);
ofSetWindowTitle("InstaKinect");
ofEnableBlendMode(ofBlendMode::OF_BLENDMODE_ADD);
// Create Instance
this->sensor = nullptr;
HRESULT result = S_OK;
result = NuiCreateSensorByIndex(0, &sensor);
if (FAILED(result)) {
cout << "Error : Create Sensor" << endl;
return;
}
// Initialize
result = sensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH);
if (FAILED(result)) {
cout << "Error : Initialise COLOR" << endl;
return;
}
// Color
this->color_event = INVALID_HANDLE_VALUE;
this->color_handle = INVALID_HANDLE_VALUE;
color_event = CreateEventW(nullptr, true, false, nullptr);
result = sensor->NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, color_event, &color_handle);
if (FAILED(result)) {
cout << "Error : Color Stream Open" << endl;
return;
}
// Depth
this->depth_event = INVALID_HANDLE_VALUE;
this->depth_handle = INVALID_HANDLE_VALUE;
depth_event = CreateEventW(nullptr, true, false, nullptr);
result = sensor->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH, NUI_IMAGE_RESOLUTION_640x480, 0, 2, depth_event, &depth_handle);
if (FAILED(result)) {
cout << "Error : Depth Stream Open" << endl;
return;
}
// Size
unsigned long refWidth = 0;
unsigned long refHeight = 0;
NuiImageResolutionToSize(NUI_IMAGE_RESOLUTION_640x480, refWidth, refHeight);
this->width = static_cast<int>(refWidth);
this->height = static_cast<int>(refHeight);
// Mapp
this->cordinate_mapper = nullptr;
result = sensor->NuiGetCoordinateMapper(&this->cordinate_mapper);
if (FAILED(result)) {
cout << "Error : Cordinate Mapper " << endl;
return;
}
// OpenCV
this->frame_img.allocate(this->width, this->height, OF_IMAGE_COLOR);
this->frame = cv::Mat(this->frame_img.getHeight(), this->frame_img.getWidth(), CV_MAKETYPE(CV_8UC3, this->frame_img.getPixels().getNumChannels()), this->frame_img.getPixels().getData(), 0);
this->depth = cv::Mat(height, width, CV_16UC1);
this->events[0] = this->color_event;
this->events[1] = this->depth_event;
}
//--------------------------------------------------------------
void ofApp::update() {
HRESULT result = S_OK;
// Color Table
cv::Vec3b color[7];
color[0] = cv::Vec3b(0, 0, 0);
color[1] = cv::Vec3b(255, 0, 0);
color[2] = cv::Vec3b(0, 255, 0);
color[3] = cv::Vec3b(0, 0, 255);
color[4] = cv::Vec3b(255, 255, 0);
color[5] = cv::Vec3b(255, 0, 255);
color[6] = cv::Vec3b(0, 255, 255);
std::vector<NUI_COLOR_IMAGE_POINT> color_point(width * height);
// Wait frame update.
ResetEvent(this->color_event);
ResetEvent(this->depth_event);
WaitForMultipleObjects(ARRAYSIZE(this->events), this->events, true, INFINITE);
// Get frame from color camera.
NUI_IMAGE_FRAME colorImageFrame = { 0 };
result = sensor->NuiImageStreamGetNextFrame(this->color_handle, 0, &colorImageFrame);
if (FAILED(result)) {
std::cerr << "Error : NuiImageStreamGetNextFrame( COLOR )" << std::endl;
return;
}
// Get image data.
INuiFrameTexture* pColorFrameTexture = colorImageFrame.pFrameTexture;
NUI_LOCKED_RECT colorLockedRect;
pColorFrameTexture->LockRect(0, &colorLockedRect, nullptr, 0);
//// Get frame from depth camera.
NUI_IMAGE_FRAME depthImageFrame = { 0 };
result = this->sensor->NuiImageStreamGetNextFrame(this->depth_handle, 0, &depthImageFrame);
if (FAILED(result)) {
std::cerr << "Error : NuiImageStreamGetNextFrame( DEPTH )" << std::endl;
return;
}
// Get depth data.
BOOL nearMode = false;
INuiFrameTexture* pDepthFrameTexture = nullptr;
this->sensor->NuiImageFrameGetDepthImagePixelFrameTexture(this->depth_handle, &depthImageFrame, &nearMode, &pDepthFrameTexture);
NUI_LOCKED_RECT depthLockedRect;
pDepthFrameTexture->LockRect(0, &depthLockedRect, nullptr, 0);
// OpenCV(image)
cv::Mat src = cv::Mat(this->height, this->width, CV_8UC4, reinterpret_cast<unsigned char*>(colorLockedRect.pBits));
cv::cvtColor(src, this->frame, CV_BGRA2RGB);
if (ofGetFrameNum() == 0) {
this->frame.copyTo(this->save_frame);
}
// OpenCV(depth)
cv::Mat bufferMat = cv::Mat::zeros(height, width, CV_16UC1);
NUI_DEPTH_IMAGE_PIXEL* depth_pixel = reinterpret_cast<NUI_DEPTH_IMAGE_PIXEL*>(depthLockedRect.pBits);
this->cordinate_mapper->MapDepthFrameToColorFrame(NUI_IMAGE_RESOLUTION_640x480, width * height, depth_pixel, NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, width * height, &color_point[0]);
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
unsigned int index = y * width + x;
bufferMat.at<unsigned short>(color_point[index].y, color_point[index].x) = depth_pixel[index].depth;
}
}
bufferMat.copyTo(this->depth);
//cv::imshow("depth", this->depth);
// Release
pColorFrameTexture->UnlockRect(0);
pDepthFrameTexture->UnlockRect(0);
this->sensor->NuiImageStreamReleaseFrame(this->color_handle, &colorImageFrame);
this->sensor->NuiImageStreamReleaseFrame(this->depth_handle, &depthImageFrame);
}
//--------------------------------------------------------------
void ofApp::draw() {
for (int y = 0; y < this->depth.rows; y += 1) {
for (int x = 0; x < this->depth.cols; x += 1) {
int z = depth.at<unsigned short>(y, x);
if (z == 0 || z > 1024) {
this->frame.at<cv::Vec3b>(y, x) = this->save_frame.at<cv::Vec3b>(y, x);
}
}
}
this->frame_img.update();
this->frame_img.draw(0, 0);
}
//--------------------------------------------------------------
void ofApp::keyPressed(int key) {
if (key == 's') {
this->frame.copyTo(this->save_frame);
}
}
//========================================================================
int main() {
ofSetupOpenGL(640, 480, OF_WINDOW);
ofRunApp(new ofApp());
}