@justyoi wrote:
#include "ofApp.h" #include <opencv2\core\core.hpp> #include <opencv2\imgproc\imgproc.hpp> #include <opencv2\highgui\highgui.hpp> #include "Kinect.h" using namespace cv; #define DEPTH_IMAGE_WIDTH 512 #define DEPTH_IMAGE_HEIGHT 424 #define POINT_CLOUD_SCALE 2 #define CLUSTER_DISTANCE 0.04f*POINT_CLOUD_SCALE template<class Interface> //函数模板 inline void SafeRelease(Interface *& pInterfaceToRelease) { if (pInterfaceToRelease != NULL) { pInterfaceToRelease->Release(); pInterfaceToRelease = NULL; } } //-------------------------------------------------------------- void ofApp::setup() { //Do some environment settings. ofSetVerticalSync(true); ofSetWindowShape(512, 424); ofBackground(0); //Turn on depth test for OpenGL. glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glShadeModel(GL_SMOOTH); //Put a camera in the scene. m_camera.setDistance(1); m_camera.setNearClip(0.1f); //Turn on the light. m_light.enable(); //Allocate memory to store point cloud and normals. m_cloud_map.Resize(DEPTH_IMAGE_WIDTH, DEPTH_IMAGE_HEIGHT); //m_normal_map.Resize(DEPTH_IMAGE_WIDTH, DEPTH_IMAGE_HEIGHT); //Initialize Kinect. InitKinect(); } void ofApp::exit() { //Exit!! Clean all up. if (pSensor != NULL) { pSensor->Release(); pSensor = NULL; } } //-------------------------------------------------------------- void ofApp::update() { //Get a new depth frame from Kinect. //m_new_depth = UpdateDepthFrame(); HRESULT hResult; IDepthFrame* pDepthFrame; int width = 512; int height = 424; /*int width = 320; int height = 240;*/ unsigned int bufferSize = width * height * sizeof(unsigned short); Mat bufferMat(height, width, CV_16SC1); Mat depthMat(height, width, CV_8UC1); //Mat smothDepthMat(height, width, CV_8UC1); Mat smothDepthMat(height, width, CV_16SC1); hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame); while (FAILED(hResult)) { SafeRelease(pDepthFrame); SafeRelease(pDepthReader); hResult = pDepthSource->OpenReader(&pDepthReader); Sleep(60); if (SUCCEEDED(hResult)) { hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame); } } if (SUCCEEDED(hResult)) { hResult = pDepthFrame->AccessUnderlyingBuffer(&bufferSize, reinterpret_cast<UINT16**>(&bufferMat.data));//中断处，m_depth_buffer为空 //cout << "AccessUnderlyingBuffer successsful!!" << endl; //bufferMat.convertTo(depthMat, CV_8U, -255.0f / 4500.0f, 255.0f); //bufferMat.convertTo(depthMat, CV_8U); } if (!SUCCEEDED(hResult)) { cout << "AccessUnderlyingBuffer error!!" << endl; } //Copy depth data to our own buffer. //Release frame. //SafeRelease(pDepthFrame); imshow("depth", bufferMat); medianBlur(bufferMat, smothDepthMat, 5); imshow("Smoothed Depth", smothDepthMat); SafeRelease(pDepthFrame); m_cloud_map.Create(smothDepthMat, 2500, POINT_CLOUD_SCALE);//1200为取样的最深距离1.2m //Calculate normals for every point in the cloud. //为点云中的每一个点计算法线 //m_normal_map.Create(m_cloud_map, CLUSTER_DISTANCE); //Make normal vectors direct toward the camera! //使法向量指向相机 // m_normal_map.FlipNormalsToVector(ofVec3f(0, 0, 1)); } //-------------------------------------------------------------- void ofApp::draw() { if (!m_init_succeeded) { cout << "draw error!!" << endl; return; } m_camera.begin(); //cout << "draw begin!!" << endl; ofVec3f* points_line = m_cloud_map.m_points; ofVec3f* points_next_line = points_line + DEPTH_IMAGE_WIDTH; ofVec3f* normals_line = m_normal_map.m_normals; bool mesh_break = true; for (int y = 0; y < 424 - 1; y++) { for (int x = 0; x < 512-1; x++) { ofVec3f& space_point1 = points_line[x]; ofVec3f& space_point2 = points_next_line[x]; if (abs(space_point1.z) <= FLT_EPSILON*POINT_CLOUD_SCALE || abs(space_point2.z) <= FLT_EPSILON*POINT_CLOUD_SCALE) { if (!mesh_break) { //If there's no point here, the mesh should break. mesh_break = true; glEnd(); } continue; } if (mesh_break) { //Start connecting points to form mesh. glBegin(GL_TRIANGLE_STRIP); mesh_break = false; } //Draw the point and set its normal. glColor3f(0.8, 0.8, 0.8); // glNormal3f(normals_line[x].x, normals_line[x].y, normals_line[x].z); glVertex3f(space_point1.x, space_point1.y, space_point1.z); //Draw the point below the prior one to form a triangle. glColor3f(0.8, 0.8, 0.8); glVertex3f(space_point2.x, space_point2.y, space_point2.z); } if (!mesh_break) { //We break the mesh at the end of the line,. glEnd(); mesh_break = true; } points_line += DEPTH_IMAGE_WIDTH; points_next_line += DEPTH_IMAGE_WIDTH; normals_line += DEPTH_IMAGE_WIDTH; } m_camera.end(); //Draw frame rate for fun! ofSetColor(255); ofDrawBitmapString(ofToString(ofGetFrameRate()), 10, 20); } //-------------------------------------------------------------- void ofApp::keyPressed(int key) { } //-------------------------------------------------------------- void ofApp::keyReleased(int key) { } //-------------------------------------------------------------- void ofApp::mouseMoved(int x, int y) { } //-------------------------------------------------------------- void ofApp::mouseDragged(int x, int y, int button) { } //-------------------------------------------------------------- void ofApp::mousePressed(int x, int y, int button) { } //-------------------------------------------------------------- void ofApp::mouseReleased(int x, int y, int button) { } //-------------------------------------------------------------- void ofApp::windowResized(int w, int h) { } //-------------------------------------------------------------- void ofApp::gotMessage(ofMessage msg) { } //-------------------------------------------------------------- void ofApp::dragEvent(ofDragInfo dragInfo) { } //初始化kinect，并开辟空间为存储深度图做准备 void ofApp::InitKinect() { m_init_succeeded = false; pSensor = NULL; HRESULT hResult; hResult = GetDefaultKinectSensor(&pSensor); if (FAILED(hResult)) { cout << "No kinect sensor was found!!" << endl; goto Final; } hResult = pSensor->Open(); if (FAILED(hResult)) {cerr << "kinect sensor open error!!" << endl; goto Final; } //We only just need depth data. hResult = pSensor->get_DepthFrameSource(&pDepthSource); if (FAILED(hResult)) {cout << "create depthFrameSoure error!!" << endl; goto Final; } hResult = pDepthSource->OpenReader(&pDepthReader); if (FAILED(hResult)) {cout << "depthSource open Reader error!!" << endl; goto Final; } pDepthSource->Release(); //Resolution of 320x240 is good enough to reconstruct a 3D model. //Allocate memory to store depth data. m_init_succeeded = true; m_depth_buffer = new USHORT[DEPTH_IMAGE_WIDTH*DEPTH_IMAGE_HEIGHT]; Final: if (FAILED(hResult)){ if (pSensor!= NULL){ pSensor->Release(); pSensor = NULL; } } } bool ofApp::UpdateDepthFrame() { if (!m_init_succeeded)return false; HRESULT hResult; IDepthFrame* pDepthFrame ; int width = 512; int height = 424; /*int width = 320; int height = 240;*/ unsigned int bufferSize = width * height * sizeof(unsigned short); Mat bufferMat(height, width, CV_16SC1); Mat depthMat(height, width, CV_8UC1); Mat smothDepthMat(height, width, CV_8UC1); hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame); while (FAILED(hResult)) { SafeRelease(pDepthFrame); SafeRelease(pDepthReader); hResult = pDepthSource->OpenReader(&pDepthReader); Sleep(60); if (SUCCEEDED(hResult)) { hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame); } } if (SUCCEEDED(hResult)) { hResult = pDepthFrame->AccessUnderlyingBuffer(&bufferSize, reinterpret_cast<UINT16**>(&bufferMat.data));//中断处，m_depth_buffer为空 cout << "AccessUnderlyingBuffer successsful!!" << endl; bufferMat.convertTo(depthMat, CV_8U, -255.0f / 4500.0f, 255.0f); } if (!SUCCEEDED(hResult)) { cout << "AccessUnderlyingBuffer error!!" << endl; } //Copy depth data to our own buffer. //Release frame. //SafeRelease(pDepthFrame); imshow("depth", bufferMat); imshow("depth2", depthMat); medianBlur(depthMat, smothDepthMat, 5); imshow("Smoothed Depth", smothDepthMat); m_cloud_map.Create(depthMat, 1200, POINT_CLOUD_SCALE); //Calculate normals for every point in the cloud. //为点云中的每一个点计算法线 m_normal_map.Create(m_cloud_map, CLUSTER_DISTANCE); //Make normal vectors direct toward the camera! //使法向量指向相机 m_normal_map.FlipNormalsToVector(ofVec3f(0, 0, 1)); SafeRelease(pDepthFrame); if (SUCCEEDED(hResult))return true; return false; }
Posts: 1
Participants: 1