#include "ofApp.h"
//--------------------------------------------------------------
void ofApp::setup(){
ofSetFrameRate(30);
ofBackground(255);
ofSetWindowTitle("InstaKinect");
ofEnableDepthTest();
// 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;
// Box2d
this->box2d.init();
this->box2d.setGravity(0, 10);
this->box2d.createBounds();
this->box2d.setFPS(30);
this->box2d.enableGrabbing();
}
//--------------------------------------------------------------
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);
// 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);
for (int i = this->rects.size() - 1; i > -1; i--) {
this->rects_life[i] -= 2;
if (this->rects_life[i] <= 0) {
this->rects[i].get()->destroy();
this->rects.erase(this->rects.begin() + i);
this->rects_color.erase(this->rects_color.begin() + i);
this->rects_life.erase(this->rects_life.begin() + i);
}
}
this->box2d.update();
}
//--------------------------------------------------------------
void ofApp::draw(){
this->locations.clear();
this->colors.clear();
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) {
if (y % 15 == 0 && x % 15 == 0) {
if (z < 550) {
this->rects.push_back(shared_ptr<ofxBox2dRect>(new ofxBox2dRect));
this->rects.back().get()->setPhysics(3.0, 0.6, 0.1);
this->rects.back().get()->setup(this->box2d.world, x, y, 15, 15);
this->rects_color.push_back(ofColor(this->frame.at<cv::Vec3b>(y, x)[0], this->frame.at<cv::Vec3b>(y, x)[1], this->frame.at<cv::Vec3b>(y, x)[2]));
this->rects_life.push_back(255);
}
}
if (z > 550) {
ofSetColor(ofColor(this->frame.at<cv::Vec3b>(y, x)[0], this->frame.at<cv::Vec3b>(y, x)[1], this->frame.at<cv::Vec3b>(y, x)[2]));
ofDrawRectangle(x, y, 1, 1);
}
}
}
}
for (int i = 0; i < this->rects.size(); i++) {
ofSetColor(this->rects_color[i], this->rects_life[i]);
this->rects[i].get()->draw();
}
this->box2d.drawGround();
}
//========================================================================
int main() {
ofSetupOpenGL(720, 720, OF_WINDOW);
ofRunApp(new ofApp());
}