#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;
}
Bunches of thanks in advance if you can help me out too with this.
